diff --git a/nhf/geometry.py b/nhf/geometry.py index b4d14d1..5ae48c5 100644 --- a/nhf/geometry.py +++ b/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 diff --git a/nhf/test.py b/nhf/test.py index cd23c56..6ca9da5 100644 --- a/nhf/test.py +++ b/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): diff --git a/nhf/touhou/houjuu_nue/electronics.py b/nhf/touhou/houjuu_nue/electronics.py index 8469d52..02468ec 100644 --- a/nhf/touhou/houjuu_nue/electronics.py +++ b/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) diff --git a/nhf/touhou/houjuu_nue/joints.py b/nhf/touhou/houjuu_nue/joints.py index 36d1914..56433fb 100644 --- a/nhf/touhou/houjuu_nue/joints.py +++ b/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", diff --git a/nhf/touhou/houjuu_nue/test.py b/nhf/touhou/houjuu_nue/test.py index f67c0a2..91d81ad 100644 --- a/nhf/touhou/houjuu_nue/test.py +++ b/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): diff --git a/nhf/touhou/houjuu_nue/wing.py b/nhf/touhou/houjuu_nue/wing.py index 3610d4c..ce3fe7b 100644 --- a/nhf/touhou/houjuu_nue/wing.py +++ b/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