33 lines
1.1 KiB
Python
33 lines
1.1 KiB
Python
"""
|
|
Geometry functions
|
|
"""
|
|
from typing import Tuple
|
|
import math
|
|
|
|
def contraction_actuator_span_pos(
|
|
d_open: float,
|
|
d_closed: float,
|
|
theta: float,
|
|
) -> Tuple[float, float]:
|
|
"""
|
|
Calculates the position of the two ends of an actuator, whose fully opened
|
|
length is `d_open`, closed length is `d_closed`, and whose motion spans a
|
|
range `theta` (in radians). Returns (r, phi): If one end of the actuator is
|
|
held at `(r, 0)`, then the other end will trace an arc `r` away from the
|
|
origin with span `theta`
|
|
|
|
Let `P` (resp. `P'`) be the position of the front of the actuator when its
|
|
fully open (resp. closed), `Q` be the position of the back of the actuator,
|
|
we note that `OP = OP' = OQ`.
|
|
"""
|
|
pq2 = d_open * d_open
|
|
p_q2 = d_closed * d_closed
|
|
# angle of PQP'
|
|
psi = 0.5 * theta
|
|
# |P-P'|, via the triangle PQP'
|
|
pp_2 = pq2 + p_q2 - 2 * d_open * d_closed * math.cos(psi)
|
|
r2 = pp_2 / (2 - 2 * math.cos(theta))
|
|
# Law of cosines on POQ:
|
|
phi = math.acos(1 - pq2 / 2 / r2)
|
|
return math.sqrt(r2), phi
|