aniva
/
Cosplay
1
1
Fork 0
Code Issues 1 Pull Requests 2 Packages Projects Releases Wiki Activity

feat: Solve actuator position with variable r

This commit is contained in:
Leni Aniva 2024-07-24 21:49:54 -07:00
parent 57262f542f
commit ac6710eeeb
Signed by: aniva
GPG Key ID: 4D9B1C8D10EA4C50
5 changed files with 189 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
nhf/geometry.py
View File

@ -1,7 +1,7 @@
"""
Geometry functions
"""
from typing import Tuple
from typing import Tuple, Optional
import math
def min_radius_contraction_span_pos(
@ -62,3 +62,49 @@ def min_tangent_contraction_span_pos(
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_

38
nhf/test.py
View File

@ -112,6 +112,44 @@ class TestGeometry(unittest.TestCase):
y = r * math.sin(phi - theta)
d = math.sqrt((x - rp) ** 2 + y ** 2)
self.assertAlmostEqual(d, dc)
def test_contraction_span_pos_from_radius(self):
sl = 50.0
dc = 112.0
do = dc + sl
r = 70.0
theta = math.radians(60.0)
for smaller in [False, True]:
with self.subTest(smaller=smaller):
r, phi, rp = nhf.geometry.contraction_span_pos_from_radius(do, dc, r=r, theta=theta, smaller=smaller)
with self.subTest(state='open'):
x = r * math.cos(phi)
y = r * math.sin(phi)
d = math.sqrt((x - rp) ** 2 + y ** 2)
self.assertAlmostEqual(d, do)
with self.subTest(state='closed'):
x = r * math.cos(phi - theta)
y = r * math.sin(phi - theta)
d = math.sqrt((x - rp) ** 2 + y ** 2)
self.assertAlmostEqual(d, dc)
def test_contraction_span_pos_from_radius_2(self):
sl = 40.0
dc = 170.0
do = dc + sl
r = 50.0
theta = math.radians(120.0)
for smaller in [False, True]:
with self.subTest(smaller=smaller):
r, phi, rp = nhf.geometry.contraction_span_pos_from_radius(do, dc, r=r, theta=theta, smaller=smaller)
with self.subTest(state='open'):
x = r * math.cos(phi)
y = r * math.sin(phi)
d = math.sqrt((x - rp) ** 2 + y ** 2)
self.assertAlmostEqual(d, do)
with self.subTest(state='closed'):
x = r * math.cos(phi - theta)
y = r * math.sin(phi - theta)
d = math.sqrt((x - rp) ** 2 + y ** 2)
self.assertAlmostEqual(d, dc)
class TestUtils(unittest.TestCase):

29
nhf/touhou/houjuu_nue/electronics.py
View File

@ -51,7 +51,7 @@ class LinearActuator(Item):
return self.segment1_length + self.segment2_length + self.front_hole_ext + self.back_hole_ext
def generate(self, pos: float=0) -> Cq.Assembly:
assert -1e-6 <= pos <= 1 + 1e-6
assert -1e-6 <= pos <= 1 + 1e-6, f"Illegal position: {pos}"
stroke_x = pos * self.stroke_length
front = (
Cq.Workplane('XZ')
@ -339,6 +339,7 @@ class Flexor:
Actuator assembly which flexes, similar to biceps
"""
motion_span: float
arm_radius: Optional[float] = None
actuator: LinearActuator = LINEAR_ACTUATOR_50
nut: HexNut = LINEAR_ACTUATOR_HEX_NUT
@ -352,22 +353,11 @@ class Flexor:
return self.bracket.hole_to_side_ext
def open_pos(self) -> Tuple[float, float, float]:
r, phi, r_ = nhf.geometry.min_tangent_contraction_span_pos(
d_open=self.actuator.conn_length + self.actuator.stroke_length,
d_closed=self.actuator.conn_length,
theta=math.radians(self.motion_span),
)
return r, math.degrees(phi), r_
#r, phi = nhf.geometry.min_radius_contraction_span_pos(
# d_open=self.actuator.conn_length + self.actuator.stroke_length,
# d_closed=self.actuator.conn_length,
# theta=math.radians(self.motion_span),
#)
#return r, math.degrees(phi), r
r, phi, r_ = nhf.geometry.min_tangent_contraction_span_pos(
r, phi, r_ = nhf.geometry.contraction_span_pos_from_radius(
d_open=self.actuator.conn_length + self.actuator.stroke_length,
d_closed=self.actuator.conn_length,
theta=math.radians(self.motion_span),
r=self.arm_radius,
)
return r, math.degrees(phi), r_
@ -378,12 +368,15 @@ class Flexor:
"""
Length of the actuator at some angle
"""
assert 0 <= angle <= self.motion_span
r, phi, rp = self.open_pos()
th = math.radians(phi - angle)
return math.sqrt((r * math.cos(th) - rp) ** 2 + (r * math.sin(th)) ** 2)
# Law of cosines
d2 = r * r + rp * rp - 2 * r * rp * math.cos(th)
return math.sqrt(d2)
result = math.sqrt(r * r + rp * rp - 2 * r * rp * math.cos(th))
#result = math.sqrt((r * math.cos(th) - rp) ** 2 + (r * math.sin(th)) ** 2)
assert self.actuator.conn_length <= result <= self.actuator.conn_length + self.actuator.stroke_length, \
f"Illegal length: {result} in {self.actuator.conn_length}+{self.actuator.stroke_length}"
return result
def add_to(

24
nhf/touhou/houjuu_nue/joints.py
View File

@ -768,7 +768,7 @@ class DiskJoint(Model):
radius_axle: float = 3.0
housing_thickness: float = 4.0
disk_thickness: float = 7.0
disk_thickness: float = 8.0
# Amount by which the wall carves in
wall_inset: float = 2.0
@ -784,7 +784,7 @@ class DiskJoint(Model):
# leave some gap for cushion
movement_gap: float = 5.0
# Angular span of tongue on disk
tongue_span: float = 30.0
tongue_span: float = 25.0
tongue_length: float = 10.0
generate_inner_wall: bool = False
@ -1053,7 +1053,7 @@ class ElbowJoint(Model):
# Extra bit on top of the lip to connect to actuator mount
child_lip_extra_length: float = 1.0
lip_length: float = 60.0
hole_pos: list[float] = field(default_factory=lambda: [15, 25])
hole_pos: list[float] = field(default_factory=lambda: [12, 24])
parent_arm_width: float = 10.0
# Angle of the beginning of the parent arm
parent_arm_angle: float = 180.0
@ -1074,6 +1074,7 @@ class ElbowJoint(Model):
# Rotates the surface of the mount relative to radially inwards
flexor_mount_angle_parent: float = 0
flexor_mount_angle_child: float = -90
flexor_child_arm_radius: Optional[float] = None
def __post_init__(self):
assert self.child_arm_radius > self.disk_joint.radius_housing
@ -1082,7 +1083,8 @@ class ElbowJoint(Model):
if self.actuator:
self.flexor = Flexor(
actuator=self.actuator,
motion_span=self.motion_span
motion_span=self.motion_span,
arm_radius=self.flexor_child_arm_radius,
)
def hole_loc_tags(self):
@ -1263,18 +1265,18 @@ class ElbowJoint(Model):
#.solve()
)
if self.flexor:
result.add(
Cq.Edge.makeLine((-1,0,0), (1,0,0)),
name="act",
loc=self.actuator_mount_loc(child=False, unflip=True))
if generate_mount:
# Orientes the hole surface so it faces +X
loc_thickness = Cq.Location((-self.lip_thickness, 0, 0), (0, 1, 0), 90)
result.add(
self.actuator_mount(),
name="act",
loc=self.actuator_mount_loc(child=False, unflip=True) * loc_thickness)
else:
result.add(
Cq.Edge.makeLine((-1,0,0), (1,0,0)),
name="act",
loc=self.actuator_mount_loc(child=False, unflip=True))
name="act_mount",
loc=self.actuator_mount_loc(child=False, unflip=True) * loc_thickness
)
return result
@assembly()

167
nhf/touhou/houjuu_nue/wing.py
View File

@ -22,6 +22,31 @@ from nhf.touhou.houjuu_nue.electronics import (
)
import nhf.utils
ELBOW_PARAMS = dict(
disk_joint=DiskJoint(
movement_angle=55,
),
hole_diam=4.0,
actuator=LINEAR_ACTUATOR_50,
parent_arm_width=15,
)
WRIST_PARAMS = dict(
disk_joint=DiskJoint(
movement_angle=30,
radius_disk=13.0,
radius_housing=15.0,
),
hole_pos=[10],
lip_length=30,
child_arm_radius=23.0,
parent_arm_radius=30.0,
hole_diam=4.0,
angle_neutral=0.0,
actuator=LINEAR_ACTUATOR_10,
flexor_offset_angle=30.0,
flexor_child_arm_radius=None,
)
@dataclass(kw_only=True)
class WingProfile(Model):
@ -55,37 +80,11 @@ class WingProfile(Model):
s1_thickness: float = 25.0
elbow_joint: ElbowJoint = field(default_factory=lambda: ElbowJoint(
disk_joint=DiskJoint(
movement_angle=55,
),
hole_diam=4.0,
angle_neutral=10.0,
actuator=LINEAR_ACTUATOR_50,
flexor_offset_angle=30,
parent_arm_width=15,
child_lip_extra_length=8,
flip=False,
))
elbow_joint: ElbowJoint
# Distance between the two spacers on the elbow, halved
elbow_h2: float = 5.0
wrist_joint: ElbowJoint = field(default_factory=lambda: ElbowJoint(
disk_joint=DiskJoint(
movement_angle=30,
radius_disk=13.0,
radius_housing=15.0,
),
hole_pos=[10],
lip_length=30,
child_arm_radius=23.0,
parent_arm_radius=30.0,
hole_diam=4.0,
angle_neutral=0.0,
actuator=LINEAR_ACTUATOR_10,
flexor_offset_angle=30.0,
flip=True,
))
wrist_joint: ElbowJoint
# Distance between the two spacers on the elbow, halved
wrist_h2: float = 5.0
@ -99,7 +98,7 @@ class WingProfile(Model):
elbow_height: float
wrist_bot_loc: Cq.Location
wrist_height: float
elbow_rotate: float = 10.0
elbow_rotate: float
wrist_rotate: float = -30.0
# Position of the elbow axle with 0 being bottom and 1 being top (flipped on the left side)
elbow_axle_pos: float
@ -575,35 +574,18 @@ class WingProfile(Model):
Polygon shape to mask wrist
"""
def spacer_of_joint(
self,
joint: ElbowJoint,
segment_thickness: float,
dx: float) -> MountingBox:
length = joint.lip_length / 2 - dx
holes = [
Hole(x - dx)
for x in joint.hole_pos
]
mbox = MountingBox(
length=length,
width=segment_thickness,
thickness=self.spacer_thickness,
holes=holes,
hole_diam=joint.hole_diam,
centred=(False, True),
)
return mbox
def _spacer_from_disk_joint(
self,
joint: ElbowJoint,
segment_thickness: float,
child: bool=False,
) -> MountingBox:
sign = 1 if child else -1
holes = [
Hole(x, tag=tag)
Hole(sign * x, tag=tag)
for x, tag in joint.hole_loc_tags()
]
# FIXME: Carve out the sides so light can pass through
mbox = MountingBox(
length=joint.lip_length,
width=segment_thickness,
@ -789,6 +771,7 @@ class WingProfile(Model):
return self._spacer_from_disk_joint(
joint=self.elbow_joint,
segment_thickness=self.s2_thickness,
child=True,
)
@submodel(name="spacer-s2-wrist")
def spacer_s2_wrist(self) -> MountingBox:
@ -847,7 +830,7 @@ class WingProfile(Model):
result,
o.generate(),
point_tag=t,
flipped=is_parent,
flipped=True,#is_parent,
)
return result.solve()
@ -907,6 +890,7 @@ class WingProfile(Model):
return self._spacer_from_disk_joint(
joint=self.wrist_joint,
segment_thickness=self.s3_thickness,
child=True,
)
@assembly()
def assembly_s3(self) -> Cq.Assembly:
@ -952,6 +936,9 @@ class WingProfile(Model):
ignore_electronics: bool = False,
ignore_actuators: bool = False,
) -> Cq.Assembly():
assert 0 <= elbow_wrist_deflection <= 1
assert 0 <= shoulder_deflection <= 1
assert 0 <= fastener_pos <= 1
if parts is None:
parts = [
"root",
@ -963,9 +950,7 @@ class WingProfile(Model):
"wrist",
"s3",
]
result = (
Cq.Assembly()
)
result = Cq.Assembly()
tag_top, tag_bot = "top", "bot"
if self.flip:
tag_top, tag_bot = tag_bot, tag_top
@ -1019,11 +1004,11 @@ class WingProfile(Model):
result.constrain(
f"s1/elbow?{tag}",
f"elbow/parent_upper/lip?{tag}", "Plane")
if not ignore_actuators:
result.constrain(
"elbow/bracket_back?conn_side",
"s1/elbow_act?conn0",
"Plane")
#if not ignore_actuators:
# result.constrain(
# "elbow/bracket_back?conn_side",
# "s1/elbow_act?conn0",
# "Plane")
if "s2" in parts:
result.add(self.assembly_s2(), name="s2")
if "s2" in parts and "elbow" in parts:
@ -1048,11 +1033,11 @@ class WingProfile(Model):
result.constrain(
f"s3/wrist?{tag}",
f"wrist/child/lip?{tag}", "Plane")
if not ignore_actuators:
result.constrain(
"wrist/bracket_back?conn_side",
"s2/wrist_act?conn0",
"Plane")
#if not ignore_actuators:
# result.constrain(
# "wrist/bracket_back?conn_side",
# "s2/wrist_act?conn0",
# "Plane")
if len(parts) > 1:
result.solve()
@ -1068,9 +1053,23 @@ class WingR(WingProfile):
elbow_bot_loc: Cq.Location = Cq.Location.from2d(290.0, 30.0, 27.0)
elbow_height: float = 111.0
elbow_rotate: float = 10.0
elbow_joint: ElbowJoint = field(default_factory=lambda: ElbowJoint(
flexor_offset_angle=15,
flexor_mount_angle_child=-75,
flexor_child_arm_radius=None,
angle_neutral=10.0,
child_lip_extra_length=8,
flip=False,
**ELBOW_PARAMS
))
wrist_bot_loc: Cq.Location = Cq.Location.from2d(403.0, 289.0, 45.0)
wrist_height: float = 60.0
wrist_joint: ElbowJoint = field(default_factory=lambda: ElbowJoint(
flip=True,
**WRIST_PARAMS
))
# Extends from the wrist to the tip of the arrow
arrow_height: float = 300
@ -1104,8 +1103,6 @@ class WingR(WingProfile):
* Cq.Location.rot2d(self.arrow_angle) \
* Cq.Location.from2d(0, self.arrow_height + self.wrist_height)
self.ring_loc = self.wrist_top_loc * self.ring_rel_loc
self.elbow_joint.flexor_offset_angle = 15
self.elbow_joint.flexor_mount_angle_child = -75
assert self.ring_radius > self.ring_radius_inner
assert 0 > self.blade_overlap_angle > self.arrow_angle
@ -1216,25 +1213,11 @@ class WingR(WingProfile):
This extension profile is required to accomodate the awkward shaped
joint next to the scythe
"""
# Generates the extension profile, which is required on both sides
profile = self._child_joint_extension_profile(
axle_loc=self.wrist_axle_loc,
radius=self.wrist_height,
angle_span=self.wrist_joint.motion_span,
bot=self.flip,
)
# Generates the contraction (cut) profile. only required on the left
if self.flip:
extra = (
self.profile()
.reset()
.push([self.wrist_axle_loc])
.each(self._wrist_joint_retract_cut_polygon, mode='i')
)
profile = (
profile
.push([self.wrist_axle_loc])
.each(lambda _: extra, mode='a')
bot=False,
)
return profile
@ -1274,7 +1257,6 @@ class WingR(WingProfile):
.circle(self.blade_hole_diam / 2, mode='s')
)
def _mask_elbow(self) -> list[Tuple[float, float]]:
l = 200
elbow_x, _ = self.elbow_bot_loc.to2d_pos()
@ -1306,11 +1288,24 @@ class WingR(WingProfile):
class WingL(WingProfile):
elbow_bot_loc: Cq.Location = Cq.Location.from2d(260.0, 110.0, 0.0)
elbow_height: float = 80.0
elbow_height: float = 90.0
elbow_rotate: float = 15.0
elbow_joint: ElbowJoint = field(default_factory=lambda: ElbowJoint(
angle_neutral=30.0,
flexor_mount_angle_child=180,
flexor_offset_angle=15,
flexor_child_arm_radius=60.0,
flip=True,
**ELBOW_PARAMS
))
wrist_angle: float = -45.0
wrist_bot_loc: Cq.Location = Cq.Location.from2d(460.0, -10.0, -45.0)
wrist_height: float = 43.0
wrist_joint: ElbowJoint = field(default_factory=lambda: ElbowJoint(
flip=False,
**WRIST_PARAMS
))
shoulder_bezier_ext: float = 120.0
shoulder_bezier_drop: float = 15.0
@ -1326,15 +1321,13 @@ class WingL(WingProfile):
elbow_joint_overlap_median: float = 0.5
wrist_joint_overlap_median: float = 0.5
def __post_init__(self):
assert self.wrist_height <= self.shoulder_joint.height
self.wrist_bot_loc = self.wrist_bot_loc.with_angle_2d(self.wrist_angle)
self.elbow_joint.angle_neutral = 15.0
self.elbow_joint.flip = True
self.elbow_rotate = 5.0
self.wrist_joint.angle_neutral = self.wrist_bot_loc.to2d_rot() + 30.0
self.wrist_rotate = -self.wrist_joint.angle_neutral
self.wrist_joint.flip = False
self.shoulder_joint.flip = True
super().__post_init__()