65 lines
2.0 KiB
Python
65 lines
2.0 KiB
Python
"""
|
|
Geometry functions
|
|
"""
|
|
from typing import Tuple
|
|
import math
|
|
|
|
def min_radius_contraction_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`.
|
|
"""
|
|
assert d_open > d_closed
|
|
assert 0 < theta < math.pi
|
|
|
|
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
|
|
|
|
def min_tangent_contraction_span_pos(
|
|
d_open: float,
|
|
d_closed: float,
|
|
theta: float,
|
|
) -> Tuple[float, float, float]:
|
|
"""
|
|
Returns `(r, phi, r')` where `r` is the distance of the arm to origin, `r'`
|
|
is the distance of the base to origin, and `phi` the angle in the open
|
|
state.
|
|
"""
|
|
assert d_open > d_closed
|
|
assert 0 < theta < math.pi
|
|
# Angle of OPQ = OPP'
|
|
pp_ = d_open - d_closed
|
|
pq = d_open
|
|
p_q = d_closed
|
|
|
|
a = (math.pi - theta) / 2
|
|
# Law of sines on POP'
|
|
r = math.sin(a) / math.sin(theta) * pp_
|
|
# Law of cosine on OPQ
|
|
oq = math.sqrt(r * r + pq * pq - 2 * r * pq * math.cos(a))
|
|
# Law of sines on OP'Q. Not using OPQ for numerical reasons since the angle
|
|
# `phi` could be very close to `pi/2`
|
|
phi_ = math.asin(math.sin(a) / oq * p_q)
|
|
phi = phi_ + theta
|
|
assert theta <= phi < math.pi
|
|
return r, phi, oq
|