From 7371333a84cd964adc2274f0a878c4e67e8576cb Mon Sep 17 00:00:00 2001 From: Leni Aniva Date: Mon, 22 Jul 2024 01:28:58 -0700 Subject: [PATCH] refactor: Actuator arm position into its own class --- nhf/geometry.py | 32 ++++++++++++++++++++++++++++ nhf/test.py | 21 ++++++++++++++++++ nhf/touhou/houjuu_nue/electronics.py | 29 +++++++++---------------- nhf/touhou/houjuu_nue/joints.py | 6 ++++-- 4 files changed, 67 insertions(+), 21 deletions(-) create mode 100644 nhf/geometry.py diff --git a/nhf/geometry.py b/nhf/geometry.py new file mode 100644 index 0000000..b4d14d1 --- /dev/null +++ b/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 diff --git a/nhf/test.py b/nhf/test.py index b2dbb45..cd23c56 100644 --- a/nhf/test.py +++ b/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): diff --git a/nhf/touhou/houjuu_nue/electronics.py b/nhf/touhou/houjuu_nue/electronics.py index b07bdd7..75d8ea5 100644 --- a/nhf/touhou/houjuu_nue/electronics.py +++ b/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 - th = math.radians(self.mount_loc_angle - angle) + r, phi = self.open_pos() + th = math.radians(phi - angle) d2 = 2 * r * r * (1 - math.cos(th)) return math.sqrt(d2) diff --git a/nhf/touhou/houjuu_nue/joints.py b/nhf/touhou/houjuu_nue/joints.py index 76d6ccb..1920ed0 100644 --- a/nhf/touhou/houjuu_nue/joints.py +++ b/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) - r = (-self.flexor.mount_loc_angle if child else 0) + 180 + mount_r, mount_loc_angle = self.flexor.open_pos() + 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