From ac6710eeeb709b7f4dfa5dda710f4b6388b12ace Mon Sep 17 00:00:00 2001 From: Leni Aniva Date: Wed, 24 Jul 2024 21:49:54 -0700 Subject: [PATCH] feat: Solve actuator position with variable r --- nhf/geometry.py | 48 +++++++- nhf/test.py | 38 ++++++ nhf/touhou/houjuu_nue/electronics.py | 29 ++--- nhf/touhou/houjuu_nue/joints.py | 24 ++-- nhf/touhou/houjuu_nue/wing.py | 167 +++++++++++++-------------- 5 files changed, 189 insertions(+), 117 deletions(-) diff --git a/nhf/geometry.py b/nhf/geometry.py index 5ae48c5..b4aa68f 100644 --- a/nhf/geometry.py +++ b/nhf/geometry.py @@ -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_ diff --git a/nhf/test.py b/nhf/test.py index 6ca9da5..40618b7 100644 --- a/nhf/test.py +++ b/nhf/test.py @@ -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): diff --git a/nhf/touhou/houjuu_nue/electronics.py b/nhf/touhou/houjuu_nue/electronics.py index 6d3df18..b06ec72 100644 --- a/nhf/touhou/houjuu_nue/electronics.py +++ b/nhf/touhou/houjuu_nue/electronics.py @@ -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( diff --git a/nhf/touhou/houjuu_nue/joints.py b/nhf/touhou/houjuu_nue/joints.py index 25b0ebc..4e15372 100644 --- a/nhf/touhou/houjuu_nue/joints.py +++ b/nhf/touhou/houjuu_nue/joints.py @@ -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() diff --git a/nhf/touhou/houjuu_nue/wing.py b/nhf/touhou/houjuu_nue/wing.py index 8d4c241..4cfcf4c 100644 --- a/nhf/touhou/houjuu_nue/wing.py +++ b/nhf/touhou/houjuu_nue/wing.py @@ -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,26 +1213,12 @@ 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, + bot=False, ) - # 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') - ) return profile def profile_s3_extra(self) -> Cq.Sketch: @@ -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__()