refactor: Actuator arm position into its own class

nhf/geometry.py

@@ -0,0 +1,32 @@
+"""
+Geometry functions
+"""
+from typing import Tuple
+import math
+
+def contraction_actuator_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`.
+    """
+    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

nhf/test.py

@@ -2,11 +2,13 @@
 Unit tests for tooling
 """
 from dataclasses import dataclass
+import math
 import unittest
 import cadquery as Cq
 from nhf.build import Model, target
 from nhf.parts.item import Item
 import nhf.checks
+import nhf.geometry
 import nhf.utils
 
 # Color presets for testing purposes
@@ -76,6 +78,25 @@ class TestChecks(unittest.TestCase):
             with self.subTest(offset=offset):
                 self.intersect_test_case(offset)
 
+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)
+        with self.subTest(state='open'):
+            x = r * math.cos(phi)
+            y = r * math.sin(phi)
+            d = math.sqrt((x - r) ** 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 - r) ** 2 + y ** 2)
+            self.assertAlmostEqual(d, dc)
+
+
 class TestUtils(unittest.TestCase):
 
     def test_2d_orientation(self):

nhf/touhou/houjuu_nue/electronics.py

@@ -9,7 +9,6 @@ from nhf.materials import Role
 from nhf.parts.item import Item
 from nhf.parts.fasteners import FlatHeadBolt, HexNut
 import nhf.utils
-import scipy.optimize as SO
 
 @dataclass(frozen=True)
 class LinearActuator(Item):
@@ -333,22 +332,6 @@ class Flexor:
     bracket: MountingBracket = LINEAR_ACTUATOR_BRACKET
 
     # FIXME: Add a compression spring so the serviceable distances are not as fixed
-    mount_loc_r: float = float('nan')
-    mount_loc_angle: float = float('nan')
-
-    def __post_init__(self):
-        d_open = self.actuator.conn_length + self.actuator.stroke_length
-        d_closed = self.actuator.conn_length
-        theta = math.radians(self.motion_span)
-
-        def target(args):
-            r, phi = args
-            e1 = d_open * d_open - 2 * r * r * (1 - math.cos(theta + phi))
-            e2 = d_closed * d_closed - 2 * r * r * (1 - math.cos(phi))
-            return [e1, e2]
-
-        self.mount_loc_r, phi = SO.fsolve(target, [self.actuator.conn_length, theta])
-        self.mount_loc_angle = math.degrees(theta + phi)
 
     @property
     def mount_height(self):
@@ -360,13 +343,21 @@ class Flexor:
     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(
+            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)
+
     def target_length_at_angle(
             self,
             angle: float = 0.0
         ) -> float:
         # law of cosines
-        r = self.mount_loc_r
+        r, phi = self.open_pos()
-        th = math.radians(self.mount_loc_angle - angle)
+        th = math.radians(phi - angle)
         d2 = 2 * r * r * (1 - math.cos(th))
         return math.sqrt(d2)
 

nhf/touhou/houjuu_nue/joints.py

@@ -9,6 +9,7 @@ from nhf.parts.fasteners import FlatHeadBolt, HexNut, ThreaddedKnob
 from nhf.parts.joints import TorsionJoint, HirthJoint
 from nhf.parts.box import Hole, MountingBox, box_with_centre_holes
 from nhf.touhou.houjuu_nue.electronics import Flexor, LinearActuator
+import nhf.geometry
 import nhf.utils
 
 TOL = 1e-6
@@ -941,8 +942,9 @@ 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
-        loc_span = Cq.Location.from2d(self.flexor.mount_loc_r, 0)
+        mount_r, mount_loc_angle = self.flexor.open_pos()
-        r = (-self.flexor.mount_loc_angle if child else 0) + 180
+        loc_span = Cq.Location.from2d(mount_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