fix(actuators): read an actuator's phase from the position it is put in

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.

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.
 

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.
 

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,

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:

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

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.*

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)

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``.
 

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))

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)