From 4c919adcd0665e525e935ef0154cfd4cb9f8f991 Mon Sep 17 00:00:00 2001 From: HugoFara Date: Fri, 18 Sep 2026 14:25:56 +0200 Subject: [PATCH] fix(actuators): read an actuator's phase from the position it is put in MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit An ArcCrank kept its angle and sweep direction, a LinearActuator its extension and direction, and a Mechanism DriverLink / ArcDriverLink its current_angle, as internal state that set_coords() left alone; only a Crank reads its angle from its coordinates. So the second evaluation of a candidate by kinematic_minimization, a sensitivity analysis or a simulation() context started with the actuator wherever the previous run had stopped it, and two evaluations of the same candidate returned different loci. ArcCrank.set_coord() and LinearActuator.set_coord() now read the angle or the extension from the position they are given — the angle brought onto the arc, since an arc can reach past the range of atan2, the extension held within the stroke — and restart the sweep in the positive direction, as a new actuator does. reload() writes x and y directly, so it is unaffected. Mechanism.set_joint_positions(), hence set_coords(), rebuild() and set_completely(), calls the new DriverLink.read_angle() / ArcDriverLink.read_angle(), which do the same from the motor and output joints. Noted in the review of #62. --- CHANGELOG.md | 21 ++++++-- src/pylinkage/actuators/arc_crank.py | 18 +++++++ src/pylinkage/actuators/linear.py | 19 +++++++ src/pylinkage/geometry/core.py | 16 ++++++ src/pylinkage/linkage/analysis.py | 4 +- src/pylinkage/mechanism/link.py | 34 +++++++++++++ src/pylinkage/mechanism/mechanism.py | 4 +- tests/dyads/test_dyads.py | 49 +++++++++++++++++++ tests/linkage/test_analysis.py | 38 +++++++++++++- tests/mechanism/test_link.py | 28 +++++++++++ .../mechanism/test_single_shot_and_rebuild.py | 26 ++++++++++ 11 files changed, 249 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 13d9791..cbda15d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,23 @@ the [Deprecations](https://hugofara.github.io/pylinkage/deprecations.html) page. ### Fixed +- **Putting a linkage back in a position puts its actuators back too.** An + `ArcCrank` kept its angle and sweep direction, a `LinearActuator` its + extension and direction, and a `Mechanism` `DriverLink` / `ArcDriverLink` + its `current_angle`, as internal state that `set_coords()` left alone — + only a `Crank` reads its angle from its coordinates. So the second + evaluation of a candidate by `kinematic_minimization`, a sensitivity + analysis or a `simulation()` context started with the actuator wherever + the previous run had stopped it, and two evaluations of the same + candidate returned different loci. `ArcCrank.set_coord()` and + `LinearActuator.set_coord()` now read the angle or the extension from the + position they are given (brought onto the arc or within the stroke) and + restart the sweep in the positive direction, as a new actuator does; + `Mechanism.set_joint_positions()` — hence `set_coords()`, `rebuild()` and + `set_completely()` — calls the new `DriverLink.read_angle()` / + `ArcDriverLink.read_angle()`, which do the same from the motor and output + joints. + - **`kinematic_minimization` / `kinematic_maximization` score the linkage the caller will simulate.** The wrapper first swept one revolution in 12 long strides to reject the unbuildable early, then recorded the loci in a fine @@ -19,9 +36,7 @@ the [Deprecations](https://hugofara.github.io/pylinkage/deprecations.html) page. assembly branch past a toggle, so the score certified a mechanism that raised `UnbuildableError` as soon as it was stepped from its initial position; `docs/examples/fourbar_linkage.py` did so on about half its - runs. The sweep is gone: an `ArcCrank`, a `LinearActuator` or a - `Mechanism` driver keeps its phase as internal state that `set_coords` - cannot rewind, so the recorded run has to be the first thing that moves. + runs. The sweep is gone; the recorded run is the first thing that moves. It now covers a complete cycle: it was 96 steps at `dt=1` since 2021 whatever the crank speed, a quarter turn for the default crank of one degree per step, so a fitness read the locus of a quarter of the cycle. diff --git a/src/pylinkage/actuators/arc_crank.py b/src/pylinkage/actuators/arc_crank.py index 57aebc4..4ef0ef1 100644 --- a/src/pylinkage/actuators/arc_crank.py +++ b/src/pylinkage/actuators/arc_crank.py @@ -9,6 +9,7 @@ import math from ..components import ConnectedComponent, Ground, _AnchorProxy +from ..geometry.core import _angle_within_arc from .crank import DEFAULT_ANGULAR_VELOCITY @@ -181,6 +182,23 @@ def set_constraints( if self._angle > arc_end: self._angle = arc_end + def set_coord(self, x: float | None, y: float | None) -> None: + """Place the output, and read the crank angle from where it lands. + + The angle is the direction from the anchor to ``(x, y)``, brought + onto the arc; the sweep restarts in the positive direction, as it + does on a new crank. Like a :class:`Crank`, the arc crank is then + entirely in the state its coordinates describe, so putting a + linkage back in a position puts its actuators back too. + """ + super().set_coord(x, y) + if x is None or y is None or self.anchor.x is None or self.anchor.y is None: + return + self._angle = _angle_within_arc( + math.atan2(y - self.anchor.y, x - self.anchor.x), self.arc_start, self.arc_end + ) + self._direction = 1.0 + def reload(self, dt: float = 1) -> None: """Advance the arc crank by one step. diff --git a/src/pylinkage/actuators/linear.py b/src/pylinkage/actuators/linear.py index 5df1877..bb2d68a 100644 --- a/src/pylinkage/actuators/linear.py +++ b/src/pylinkage/actuators/linear.py @@ -157,6 +157,25 @@ def set_constraints( if speed is not None: self.speed = speed + def set_coord(self, x: float | None, y: float | None) -> None: + """Place the output, and read the extension from where it lands. + + The extension is the projection of ``(x, y)`` from the anchor + onto the actuator's axis, held within ``[0, stroke]``; the stroke + restarts in the extending direction, as it does on a new + actuator. The actuator is then entirely in the state its + coordinates describe, so putting a linkage back in a position + puts its actuators back too. + """ + super().set_coord(x, y) + if x is None or y is None or self.anchor.x is None or self.anchor.y is None: + return + along = (x - self.anchor.x) * math.cos(self.angle) + (y - self.anchor.y) * math.sin( + self.angle + ) + self._extension = min(max(along, 0.0), self.stroke) + self._direction = 1.0 + def reload(self, dt: float = 1) -> None: """Advance the actuator by one step. diff --git a/src/pylinkage/geometry/core.py b/src/pylinkage/geometry/core.py index b2dd01d..44e5abf 100644 --- a/src/pylinkage/geometry/core.py +++ b/src/pylinkage/geometry/core.py @@ -117,6 +117,22 @@ def cyl_to_cart( return (radius * math.cos(theta) + ori_x, radius * math.sin(theta) + ori_y) +def _angle_within_arc(angle: float, arc_start: float, arc_end: float) -> float: + """Return ``angle`` (mod 2π) as the value in ``[arc_start, arc_end]``. + + Angles are the same direction 2π apart, and an arc such as + ``[0, 3π/2]`` reaches past the ``(-π, π]`` range of ``atan2``. When + no representative of the direction lies on the arc, the nearer end + is returned. + """ + shifted = arc_start + (angle - arc_start) % math.tau + if shifted <= arc_end: + return shifted + past_end = shifted - arc_end + before_start = math.tau - (shifted - arc_start) + return arc_end if past_end <= before_start else arc_start + + def line_from_points( first_x: float, first_y: float, diff --git a/src/pylinkage/linkage/analysis.py b/src/pylinkage/linkage/analysis.py index 76027f8..472366e 100644 --- a/src/pylinkage/linkage/analysis.py +++ b/src/pylinkage/linkage/analysis.py @@ -56,8 +56,8 @@ def wrapper( # natural stride: dt=1 is what ``step()`` and the visualizers # use, and which assembly branch the solver follows past a # toggle position depends on the stride. No quick sweep in - # long strides beforehand: it would leave the actuators - # elsewhere in their cycle, and there is no way to rewind them. + # long strides beforehand: a full cycle rejects the unbuildable + # just as surely, and this run is then the one the caller gets. n = linkage.get_rotation_period() loci = tuple(tuple(i) for i in linkage.step(iterations=max(96, n))) except UnbuildableError: diff --git a/src/pylinkage/mechanism/link.py b/src/pylinkage/mechanism/link.py index b4b7d8a..961bd11 100644 --- a/src/pylinkage/mechanism/link.py +++ b/src/pylinkage/mechanism/link.py @@ -16,6 +16,8 @@ from enum import IntEnum from typing import TYPE_CHECKING +from ..geometry.core import _angle_within_arc + if TYPE_CHECKING: from .joint import GroundJoint, Joint @@ -331,6 +333,16 @@ def reset(self) -> None: """Reset the crank to its initial angle.""" self.current_angle = self.initial_angle + def read_angle(self) -> None: + """Set ``current_angle`` from where the motor and output joints are. + + The crank is then in the state its joints describe, so putting + a mechanism back in a position puts its drivers back too. + """ + angle = _angle_of(self.motor_joint, self.output_joint) + if angle is not None: + self.current_angle = angle + @dataclass(eq=False) class ArcDriverLink(Link): @@ -446,6 +458,28 @@ def reset(self) -> None: ) self._direction = 1.0 + def read_angle(self) -> None: + """Set ``current_angle`` from where the motor and output joints are. + + The angle is brought onto the arc and the sweep restarts in the + positive direction, as on a new crank. + """ + angle = _angle_of(self.motor_joint, self.output_joint) + if angle is not None: + self.current_angle = _angle_within_arc(angle, self.arc_start, self.arc_end) + self._direction = 1.0 + + +def _angle_of(motor: Joint | None, output: Joint | None) -> float | None: + """Direction from ``motor`` to ``output``, or None while either is unplaced.""" + if motor is None or output is None: + return None + mx, my = motor.position + ox, oy = output.position + if mx is None or my is None or ox is None or oy is None: + return None + return math.atan2(oy - my, ox - mx) + # Type alias for any link AnyLink = Link | GroundLink | DriverLink | ArcDriverLink diff --git a/src/pylinkage/mechanism/mechanism.py b/src/pylinkage/mechanism/mechanism.py index db31d6d..e4a72b8 100644 --- a/src/pylinkage/mechanism/mechanism.py +++ b/src/pylinkage/mechanism/mechanism.py @@ -634,9 +634,11 @@ def get_joint_positions(self) -> list[Coord]: return positions def set_joint_positions(self, positions: list[Coord]) -> None: - """Set positions of all joints.""" + """Set positions of all joints, and read the drivers' angles from them.""" for joint, pos in zip(self.joints, positions, strict=False): joint.set_coord(pos[0], pos[1]) + for driver in self._driver_links: + driver.read_angle() # ------------------------------------------------------------------ # Analysis bound methods — thin shims over pylinkage.linkage.* diff --git a/tests/dyads/test_dyads.py b/tests/dyads/test_dyads.py index 529098d..57fa3b9 100644 --- a/tests/dyads/test_dyads.py +++ b/tests/dyads/test_dyads.py @@ -117,6 +117,28 @@ def test_linear_actuator_creation(self): assert actuator.x == pytest.approx(0.0) assert actuator.y == pytest.approx(0.0) + def test_set_coord_reads_extension_and_restarts_extending(self): + """Placing the output puts the actuator in the state its position describes.""" + origin = Ground(0.0, 0.0) + actuator = LinearActuator(anchor=origin, angle=math.pi / 2, stroke=2.0, speed=0.5) + for _ in range(7): # past the end of the stroke and back: retracting + actuator.reload() + assert actuator._direction == -1.0 + + actuator.set_coord(0.0, 1.5) + assert actuator.extension == pytest.approx(1.5) + assert actuator._direction == 1.0 + actuator.reload() + assert actuator.extension == pytest.approx(2.0) + + def test_set_coord_holds_extension_within_stroke(self): + origin = Ground(0.0, 0.0) + actuator = LinearActuator(anchor=origin, angle=0.0, stroke=2.0, speed=0.1) + actuator.set_coord(5.0, 0.3) # off the axis and past the stroke + assert actuator.extension == pytest.approx(2.0) + actuator.set_coord(-1.0, 0.0) + assert actuator.extension == pytest.approx(0.0) + def test_linear_actuator_with_angle(self): """Test creating a linear actuator with non-zero angle.""" origin = Ground(0.0, 0.0) @@ -591,6 +613,33 @@ def test_arc_crank_creation(self): assert arc_crank.x == pytest.approx(2.0) assert arc_crank.y == pytest.approx(0.0) + def test_set_coord_reads_angle_and_restarts_forward(self): + """Placing the output puts the crank in the state its position describes.""" + origin = Ground(0.0, 0.0) + arc_crank = ArcCrank( + anchor=origin, radius=2.0, angular_velocity=0.5, arc_start=0.0, arc_end=math.pi / 2 + ) + for _ in range(5): # past the end of the arc and back: sweeping backwards + arc_crank.reload() + assert arc_crank._direction == -1.0 + + arc_crank.set_coord(2 * math.cos(1.0), 2 * math.sin(1.0)) + assert arc_crank.angle == pytest.approx(1.0) + assert arc_crank._direction == 1.0 + arc_crank.reload() + assert arc_crank.angle == pytest.approx(1.5) + + def test_set_coord_brings_angle_onto_the_arc(self): + """An arc past the range of atan2 still reads the right angle.""" + origin = Ground(0.0, 0.0) + arc_crank = ArcCrank( + anchor=origin, radius=1.0, angular_velocity=0.1, arc_start=0.0, arc_end=3 * math.pi / 2 + ) + arc_crank.set_coord(math.cos(4.0), math.sin(4.0)) # atan2 says 4 − 2π + assert arc_crank.angle == pytest.approx(4.0) + arc_crank.set_coord(math.cos(5.0), math.sin(5.0)) # off the arc: nearer end + assert arc_crank.angle == pytest.approx(3 * math.pi / 2) + def test_arc_crank_with_initial_angle(self): """Test creating an arc crank with custom initial angle.""" origin = Ground(0.0, 0.0) diff --git a/tests/linkage/test_analysis.py b/tests/linkage/test_analysis.py index a6be7f3..36f083d 100644 --- a/tests/linkage/test_analysis.py +++ b/tests/linkage/test_analysis.py @@ -14,7 +14,7 @@ import numpy as np import pytest -from pylinkage.actuators import ArcCrank, Crank +from pylinkage.actuators import ArcCrank, Crank, LinearActuator from pylinkage.components import Ground from pylinkage.dyads import RRRDyad from pylinkage.linkage.analysis import ( @@ -315,7 +315,41 @@ def _make_arc_fourbar() -> Linkage: return Linkage([o1, o2, crank, rocker], name="ArcFourBar") -@pytest.mark.parametrize("make", [_make_fourbar, _make_arc_fourbar]) +def _make_linear_fourbar() -> Linkage: + """A ``LinearActuator`` drives the rocker; its extension is internal state.""" + o1 = Ground(0.0, 0.0, name="O1") + o2 = Ground(3.0, 0.0, name="O2") + piston = LinearActuator(anchor=o1, angle=math.pi / 2, stroke=1.0, speed=0.05, name="piston") + rocker = RRRDyad( + anchor1=piston.output, anchor2=o2, distance1=2.5, distance2=2.0, name="rocker" + ) + return Linkage([o1, o2, piston, rocker], name="LinearFourBar") + + +@pytest.mark.parametrize("make", [_make_fourbar, _make_arc_fourbar, _make_linear_fourbar]) +def test_kinematic_default_test_is_repeatable(make): + """Evaluating the same candidate twice gives the same loci. + + ``init_pos`` puts the actuators back too: an ``ArcCrank`` or a + ``LinearActuator`` reads its phase from where it is placed, so the + second run does not start where the first one's actuator stopped. + """ + runs = [] + + def fitness(linkage, params, init_pos, loci): + runs.append(np.array(loci, dtype=float)) + return 0.0 + + lk = make() + params = list(lk.get_constraints()) + init_pos = lk.get_coords() + wrapped = kinematic_default_test(fitness, error_penalty=float("inf")) + wrapped(lk, params, init_pos) + wrapped(lk, params, init_pos) + assert np.allclose(runs[0], runs[1]) + + +@pytest.mark.parametrize("make", [_make_fourbar, _make_arc_fourbar, _make_linear_fourbar]) def test_kinematic_default_test_loci_start_from_the_initial_position(make): """The recorded run is the one the caller gets from ``init_pos``. diff --git a/tests/mechanism/test_link.py b/tests/mechanism/test_link.py index 9011066..92d784d 100644 --- a/tests/mechanism/test_link.py +++ b/tests/mechanism/test_link.py @@ -198,6 +198,18 @@ def test_radius_no_motor(self): d = DriverLink("crank", joints=[origin, A], motor_joint=None) assert d.radius is None + def test_read_angle_from_joints(self): + origin = GroundJoint("O", position=(0.0, 0.0)) + A = RevoluteJoint("A", position=(1.0, 0.0)) + d = DriverLink("crank", joints=[origin, A], motor_joint=origin, angular_velocity=0.5) + for _ in range(20): + d.step() + A.set_coord(math.cos(1.0), math.sin(1.0)) + d.read_angle() + assert d.current_angle == pytest.approx(1.0) + d.step() + assert d.current_angle == pytest.approx(1.5) + def test_radius_non_binary(self): origin = GroundJoint("O", position=(0.0, 0.0)) d = DriverLink("crank", joints=[origin], motor_joint=origin) @@ -270,6 +282,22 @@ def test_radius(self): arc = self._make_arc() assert abs(arc.radius - 1.0) < 1e-10 + def test_read_angle_from_joints(self): + arc = self._make_arc(arc_start=0.0, arc_end=math.pi, omega=0.5) + for _ in range(8): # past the end of the arc and back + arc.step() + assert arc._direction == -1.0 + arc.output_joint.set_coord(math.cos(2.0), math.sin(2.0)) + arc.read_angle() + assert arc.current_angle == pytest.approx(2.0) + assert arc._direction == 1.0 + + def test_read_angle_leaves_unplaced_joints_alone(self): + arc = self._make_arc(arc_start=0.5) + arc.output_joint.set_coord(None, None) + arc.read_angle() + assert arc.current_angle == 0.5 + def test_output_joint(self): origin = GroundJoint("O", position=(0.0, 0.0)) A = RevoluteJoint("A", position=(1.0, 0.0)) diff --git a/tests/mechanism/test_single_shot_and_rebuild.py b/tests/mechanism/test_single_shot_and_rebuild.py index 44cf642..2c717b8 100644 --- a/tests/mechanism/test_single_shot_and_rebuild.py +++ b/tests/mechanism/test_single_shot_and_rebuild.py @@ -105,3 +105,29 @@ def test_rebuild_after_step_recompiles(self) -> None: m.step_fast(iterations=5) assert m._solver_data is not None assert m._solver_data is not first + + +class TestSetCoordsReadsDriverAngles: + def test_run_repeats_after_set_coords(self) -> None: + """Putting the joints back puts the crank's angle back with them.""" + import numpy as np + + m = fourbar(crank=1.0, coupler=3.0, rocker=3.0, ground=4.0) + init = m.get_coords() + first = np.array(list(m.step(iterations=40)), dtype=float) + m.set_coords(init) + again = np.array(list(m.step(iterations=40)), dtype=float) + assert np.allclose(first, again) + + def test_rebuild_reads_driver_angle(self) -> None: + import math + + m = fourbar(crank=1.0, coupler=3.0, rocker=3.0, ground=4.0) + for _ in m.step(iterations=25): + pass + positions = m.get_coords() + tip = m._driver_links[0].output_joint + assert tip is not None + positions[m.joints.index(tip)] = (math.cos(2.0), math.sin(2.0)) + m.rebuild(positions) + assert m._driver_links[0].current_angle == pytest.approx(2.0)