feat: Solve actuator position with variable r

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_

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):

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(
+        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),
-        )
-        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(
             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
+        result = math.sqrt(r * r + rp * rp - 2 * r * rp * math.cos(th))
-        d2 = r * r + rp * rp - 2 * r * rp * math.cos(th)
+        #result = math.sqrt((r * math.cos(th) - rp) ** 2 + (r * math.sin(th)) ** 2)
-        return math.sqrt(d2)
+        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(

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",
+                    name="act_mount",
-                    loc=self.actuator_mount_loc(child=False, unflip=True) * loc_thickness)
+                    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))
         return result
 
     @assembly()

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(
+    elbow_joint: 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,
-    ))
     # Distance between the two spacers on the elbow, halved
     elbow_h2: float = 5.0
 
-    wrist_joint: ElbowJoint = field(default_factory=lambda: ElbowJoint(
+    wrist_joint: 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,
-    ))
     # 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 = (
+        result = Cq.Assembly()
-            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:
+            #if not ignore_actuators:
-                result.constrain(
+            #    result.constrain(
-                    "elbow/bracket_back?conn_side",
+            #        "elbow/bracket_back?conn_side",
-                    "s1/elbow_act?conn0",
+            #        "s1/elbow_act?conn0",
-                    "Plane")
+            #        "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:
+            #if not ignore_actuators:
-                result.constrain(
+            #    result.constrain(
-                    "wrist/bracket_back?conn_side",
+            #        "wrist/bracket_back?conn_side",
-                    "s2/wrist_act?conn0",
+            #        "s2/wrist_act?conn0",
-                    "Plane")
+            #        "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,
+            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
 
@@ -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__()