111 lines
3.6 KiB
Python
111 lines
3.6 KiB
Python
|
"""
|
||
|
|
Geometry functions
|
||
|
|
"""
|
||
|
|
from typing import Tuple, Optional
|
||
|
|
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
|
||
|
|
|
||
|
|
def contraction_span_pos_from_radius(
|
||
|
|
d_open: float,
|
||
|
|
d_closed: float,
|
||
|
|
theta: float,
|
||
|
|
r: Optional[float] = None,
|
||
|
|
smaller: bool = True,
|
||
|
|
) -> Tuple[float, float, float]:
|
||
|
|
"""
|
||
|
|
Returns `(r, phi, r')`
|
||
|
|
|
||
|
|
Set `smaller` to false to use the other solution, which has a larger
|
||
|
|
profile.
|
||
|
|
"""
|
||
|
|
if r is None:
|
||
|
|
return min_tangent_contraction_span_pos(
|
||
|
|
d_open=d_open,
|
||
|
|
d_closed=d_closed,
|
||
|
|
theta=theta)
|
||
|
|
assert 0 < theta < math.pi
|
||
|
|
assert d_open > d_closed
|
||
|
|
assert r > 0
|
||
|
|
# Law of cosines
|
||
|
|
pp_ = r * math.sqrt(2 * (1 - math.cos(theta)))
|
||
|
|
d = d_open - d_closed
|
||
|
|
assert pp_ > d, f"Triangle inequality is violated. This joint is impossible: {pp_}, {d}"
|
||
|
|
assert d_open + d_closed > pp_, f"The span is too great to cover with this stroke length: {pp_}"
|
||
|
|
# Angle of PP'Q, via a numerically stable acos
|
||
|
|
beta = math.acos(
|
||
|
|
- d / pp_ * (1 + d / (2 * d_closed))
|
||
|
|
+ pp_ / (2 * d_closed))
|
||
|
|
# Two solutions based on angle complementarity
|
||
|
|
if smaller:
|
||
|
|
contra_phi = beta - (math.pi - theta) / 2
|
||
|
|
else:
|
||
|
|
# technically there's a 2pi in front
|
||
|
|
contra_phi = -(math.pi - theta) / 2 - beta
|
||
|
|
# Law of cosines, calculates `r'`
|
||
|
|
r_ = math.sqrt(
|
||
|
|
r * r + d_closed * d_closed - 2 * r * d_closed * math.cos(contra_phi)
|
||
|
|
)
|
||
|
|
# sin phi_ / P'Q = sin contra_phi / r'
|
||
|
|
phi_ = math.asin(math.sin(contra_phi) / r_ * d_closed)
|
||
|
|
assert phi_ > 0, f"Actuator would need to traverse pass its minimal point, {math.degrees(phi_)}"
|
||
|
|
assert 0 <= theta + phi_ <= math.pi
|
||
|
|
return r, theta + phi_, r_
|