feat: Actuator position to minimize tangential

nhf/geometry.py

@@ -4,7 +4,7 @@ Geometry functions
 from typing import Tuple
 import math
 
-def contraction_actuator_span_pos(
+def min_radius_contraction_span_pos(
         d_open: float,
         d_closed: float,
         theta: float,
@@ -20,6 +20,9 @@ def contraction_actuator_span_pos(
     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'
@@ -30,3 +33,32 @@ def contraction_actuator_span_pos(
     # 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

nhf/test.py

@@ -80,11 +80,12 @@ class TestChecks(unittest.TestCase):
 
 class TestGeometry(unittest.TestCase):
 
-    def test_actuator_arm_pos(self):
-        do = 70.0
-        dc = 40.0
-        theta = math.radians(35.0)
-        r, phi = nhf.geometry.contraction_actuator_span_pos(do, dc, theta)
+    def test_min_radius_contraction_span_pos(self):
+        sl = 50.0
+        dc = 112.0
+        do = dc + sl
+        theta = math.radians(60.0)
+        r, phi = nhf.geometry.min_radius_contraction_span_pos(do, dc, theta)
         with self.subTest(state='open'):
             x = r * math.cos(phi)
             y = r * math.sin(phi)
@@ -95,6 +96,22 @@ class TestGeometry(unittest.TestCase):
             y = r * math.sin(phi - theta)
             d = math.sqrt((x - r) ** 2 + y ** 2)
             self.assertAlmostEqual(d, dc)
+    def test_min_tangent_contraction_span_pos(self):
+        sl = 50.0
+        dc = 112.0
+        do = dc + sl
+        theta = math.radians(60.0)
+        r, phi, rp = nhf.geometry.min_tangent_contraction_span_pos(do, dc, theta)
+        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

@@ -338,29 +338,39 @@ class Flexor:
     @property
     def mount_height(self):
         return self.bracket.hole_to_side_ext
-    @property
-    def min_serviceable_distance(self):
-        return self.bracket.hole_to_side_ext * 2 + self.actuator.conn_length
-    @property
-    def max_serviceable_distance(self):
-        return self.min_serviceable_distance + self.actuator.stroke_length
 
-    def open_pos(self) -> Tuple[float, float]:
-        r, phi = nhf.geometry.contraction_actuator_span_pos(
+    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)
+            theta=math.radians(self.motion_span),
         )
-        return r, math.degrees(phi)
+        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),
+        )
+        return r, math.degrees(phi), r_
 
     def target_length_at_angle(
             self,
             angle: float = 0.0
         ) -> float:
-        # law of cosines
-        r, phi = self.open_pos()
+        """
+        Length of the actuator at some angle
+        """
+        r, phi, rp = self.open_pos()
         th = math.radians(phi - angle)
-        d2 = 2 * r * r * (1 - math.cos(th))
+        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)
 
 

nhf/touhou/houjuu_nue/joints.py

@@ -943,8 +943,8 @@ class ElbowJoint(Model):
         loc_mount = Cq.Location.from2d(self.flexor.mount_height, 0) * Cq.Location.rot2d(180)
         loc_mount_orient = Cq.Location.rot2d(self.flexor_mount_rot * (-1 if child else 1))
         # Moves the hole to be some distance apart from 0
-        mount_r, mount_loc_angle = self.flexor.open_pos()
-        loc_span = Cq.Location.from2d(mount_r, 0)
+        mount_r, mount_loc_angle, mount_parent_r = self.flexor.open_pos()
+        loc_span = Cq.Location.from2d(mount_r if child else mount_parent_r, 0)
         r = (-mount_loc_angle if child else 0) + 180
         loc_rot = Cq.Location.rot2d(r + self.flexor_offset_angle)
         return loc_rot * loc_span * loc_mount_orient * loc_mount * loc_thickness
@@ -1055,6 +1055,7 @@ class ElbowJoint(Model):
 
     @assembly()
     def assembly(self, angle: float = 0) -> Cq.Assembly:
+        assert 0 <= angle <= self.motion_span
         result = (
             Cq.Assembly()
             .addS(self.child_joint(), name="child",

nhf/touhou/houjuu_nue/test.py

@@ -2,8 +2,23 @@ import unittest
 import cadquery as Cq
 import nhf.touhou.houjuu_nue as M
 import nhf.touhou.houjuu_nue.joints as MJ
+import nhf.touhou.houjuu_nue.electronics as ME
 from nhf.checks import pairwise_intersection
 
+class TestElectronics(unittest.TestCase):
+
+    def test_flexor(self):
+        flexor = ME.Flexor(
+            motion_span=60,
+        )
+        self.assertAlmostEqual(
+            flexor.target_length_at_angle(0),
+            flexor.actuator.stroke_length + flexor.actuator.conn_length)
+        self.assertAlmostEqual(
+            flexor.target_length_at_angle(flexor.motion_span),
+            flexor.actuator.conn_length)
+
+
 class TestJoints(unittest.TestCase):
 
     def test_shoulder_collision_of_torsion_joint(self):

nhf/touhou/houjuu_nue/wing.py

@@ -6,6 +6,7 @@ import math
 from enum import Enum
 from dataclasses import dataclass, field
 from typing import Mapping, Tuple, Optional
+
 import cadquery as Cq
 from nhf import Material, Role
 from nhf.build import Model, TargetKind, target, assembly, submodel
@@ -71,8 +72,7 @@ class WingProfile(Model):
         parent_arm_radius=30.0,
         hole_diam=4.0,
         angle_neutral=-30.0,
-        # The left side wrist is too small for an actuator to work
-        actuator=None, #LINEAR_ACTUATOR_10,
+        actuator=LINEAR_ACTUATOR_10,
     ))
     # Distance between the two spacers on the elbow, halved
     wrist_h2: float = 5.0