From 4060aa8938891fa8e0b7f0f7927145fb7793e394 Mon Sep 17 00:00:00 2001 From: HugoFara Date: Sat, 12 Sep 2026 21:12:56 +0200 Subject: [PATCH] fix(mechanism): make set_constraints change the dimensions the solver maintains Link lengths live in Link._cached_distances and in the group solver's Dimensions, both captured when the mechanism is built. set_constraints only moved the driver's output joint, so the next step() re-solved every joint against the original lengths and snapped the crank tip back to its original radius; get_constraints() (position-based for binary links) then read the old values too. Optimizing a Mechanism was a silent no-op. - Link.set_distance(j1, j2, d): write the constraint (both orderings), positions untouched, re-solved on the next step. - Mechanism.set_constraints: write driver radii and binary link lengths through set_distance, reposition the crank tip at its current angle, rebuild the Assur decomposition so the group solver sees the new Dimensions. Skips the same links get_constraints skips. - Link.length reads get_distance() (the cache when built), consistent with DriverLink.radius, so get/set_constraints round-trip before any step. Tests cover the round trip, both solver paths maintaining the new lengths over 20 steps, and dict serialization after a dt=0 step. --- CHANGELOG.md | 13 +++++++ src/pylinkage/mechanism/link.py | 42 +++++++++++++++------- src/pylinkage/mechanism/mechanism.py | 31 ++++++++-------- tests/mechanism/test_link.py | 22 ++++++++++++ tests/mechanism/test_mechanism_extended.py | 34 ++++++++++++++++++ 5 files changed, 116 insertions(+), 26 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b800fd7..8c4b3b9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,19 @@ the [Deprecations](https://hugofara.github.io/pylinkage/deprecations.html) page. ## [Unreleased] +### Fixed + +- **`Mechanism.set_constraints()` now changes the mechanism.** Link lengths + live in per-link distance caches and in the Assur-group solver's + `Dimensions`, both captured when the mechanism is built; `set_constraints` + only moved the crank tip, so the next `step()` snapped every joint back to + the original geometry and `get_constraints()` read the old lengths. Every + optimizer run on a `Mechanism` was a no-op since 0.8.0. The values are now + written to the caches (new `Link.set_distance()`), the group solver is + rebuilt, and `Link.length` reads the maintained constraint like + `DriverLink.radius` does. Only binary links and driver radii are part of + the constraint vector, as before. + ## [1.2.1] - 2026-09-12 ### Added diff --git a/src/pylinkage/mechanism/link.py b/src/pylinkage/mechanism/link.py index 7814c33..b4b7d8a 100644 --- a/src/pylinkage/mechanism/link.py +++ b/src/pylinkage/mechanism/link.py @@ -95,8 +95,10 @@ def order(self) -> int: def length(self) -> float | None: """Return the length of a binary link. - Only meaningful for binary links (2 joints). Returns the - Euclidean distance between the two joint positions. + Only meaningful for binary links (2 joints). Returns the rigid + distance the solver maintains between the two joints (see + :meth:`get_distance`): the cached constraint once the mechanism is + built, the distance between the joint positions before that. Returns: Distance between joints, or None if not a binary link @@ -106,16 +108,7 @@ def length(self) -> float | None: return None j1, j2 = self.joints - if not j1.is_defined() or not j2.is_defined(): - return None - - x1, y1 = j1.position - x2, y2 = j2.position - # Type narrowing: is_defined() guarantees non-None - assert x1 is not None and y1 is not None - assert x2 is not None and y2 is not None - - return math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + return self.get_distance(j1, j2) def cache_distances(self) -> None: """Cache distances between all pairs of joints. @@ -174,6 +167,31 @@ def get_distance(self, joint1: Joint, joint2: Joint) -> float | None: return math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + def set_distance(self, joint1: Joint, joint2: Joint, distance: float) -> None: + """Set the distance constraint between two joints on this link. + + Changes the rigid dimension the solver maintains between the two + joints; their positions are not moved, the next simulation step + re-solves them against the new value. + + Args: + joint1: First joint (must be in this link). + joint2: Second joint (must be in this link). + distance: New distance, must be positive. + + Raises: + ValueError: If either joint is not part of this link, or if + ``distance`` is not positive. + """ + if joint1 not in self.joints: + raise ValueError(f"Joint {joint1.id} is not part of link {self.id}") + if joint2 not in self.joints: + raise ValueError(f"Joint {joint2.id} is not part of link {self.id}") + if distance <= 0: + raise ValueError(f"Distance on link {self.id} must be positive, got {distance}") + self._cached_distances[(joint1.id, joint2.id)] = distance + self._cached_distances[(joint2.id, joint1.id)] = distance + def other_joint(self, joint: Joint) -> Joint | None: """Get the other joint in a binary link. diff --git a/src/pylinkage/mechanism/mechanism.py b/src/pylinkage/mechanism/mechanism.py index e582792..92a6772 100644 --- a/src/pylinkage/mechanism/mechanism.py +++ b/src/pylinkage/mechanism/mechanism.py @@ -516,8 +516,12 @@ def set_constraints(self, values: list[float]) -> None: """Set distance constraints from a flat list. Used for optimization. Applies constraints in the same order - as :meth:`get_constraints`. Invalidates any cached SolverData - so the next :meth:`step_fast` recompiles. + as :meth:`get_constraints`: the radius of each driver and the + length of each binary link. The values become the rigid + dimensions the solver maintains, so the next :meth:`step` + re-solves every joint against them (a driver's output joint is + moved right away, keeping its current angle). Invalidates any + cached SolverData so the next :meth:`step_fast` recompiles. Args: values: List of constraint values to apply. @@ -533,30 +537,29 @@ def set_constraints(self, values: list[float]) -> None: break if isinstance(link, (DriverLink, ArcDriverLink)): - # Update driver radius by moving output joint - new_radius = values[idx] - idx += 1 - output = link.output_joint - if output is None or link.motor_joint is None: + if output is None or link.motor_joint is None or link.radius is None: continue + new_radius = values[idx] + idx += 1 + link.set_distance(link.motor_joint, output, new_radius) mx, my = link.motor_joint.position if mx is None or my is None: continue - # Recompute output position with new radius new_x = mx + new_radius * math.cos(link.current_angle) new_y = my + new_radius * math.sin(link.current_angle) output.set_coord(new_x, new_y) - elif len(link.joints) == 2: - # Binary link: update the movable joint's position - # (This is a simplified approach; full implementation - # would need to track which joint is the dependent one) - _new_length = values[idx] + elif link.length is not None: + # Binary link: the joints move when the mechanism is next solved + link.set_distance(link.joints[0], link.joints[1], values[idx]) idx += 1 - # Position update happens during simulation + + # The group solver keeps its own copy of the dimensions + if self._use_group_solver: + self._build_decomposition() # Cross-API aliases — ``simulation.Linkage`` and the legacy Linkage # both used the ``*_num_constraints`` / ``*_coords`` spellings, and diff --git a/tests/mechanism/test_link.py b/tests/mechanism/test_link.py index 07e436f..9011066 100644 --- a/tests/mechanism/test_link.py +++ b/tests/mechanism/test_link.py @@ -122,6 +122,28 @@ def test_get_distance_joint_not_in_link(self): with pytest.raises(ValueError, match="not part of link"): link.get_distance(j1, j3) + def test_set_distance_overrides_cache_symmetrically(self): + j1 = RevoluteJoint("A", position=(0.0, 0.0)) + j2 = RevoluteJoint("B", position=(3.0, 4.0)) + link = Link("AB", joints=[j1, j2]) + link.cache_distances() + link.set_distance(j1, j2, 7.5) + assert link.get_distance(j1, j2) == 7.5 + assert link.get_distance(j2, j1) == 7.5 + assert link.length == 7.5 + # Positions are left alone: the solver moves the joints on the next step + assert j2.position == (3.0, 4.0) + + def test_set_distance_rejects_bad_input(self): + j1 = RevoluteJoint("A", position=(0.0, 0.0)) + j2 = RevoluteJoint("B", position=(1.0, 0.0)) + j3 = RevoluteJoint("C", position=(2.0, 0.0)) + link = Link("AB", joints=[j1, j2]) + with pytest.raises(ValueError, match="not part of link"): + link.set_distance(j1, j3, 1.0) + with pytest.raises(ValueError, match="must be positive"): + link.set_distance(j1, j2, 0.0) + class TestOtherJoint: """Tests for Link.other_joint().""" diff --git a/tests/mechanism/test_mechanism_extended.py b/tests/mechanism/test_mechanism_extended.py index c47db10..1f4d112 100644 --- a/tests/mechanism/test_mechanism_extended.py +++ b/tests/mechanism/test_mechanism_extended.py @@ -2,6 +2,8 @@ import math +import pytest + from pylinkage.mechanism import ( DriverLink, GroundJoint, @@ -147,6 +149,38 @@ def test_set_constraints_with_fewer_values(self): mech = _build_four_bar() mech.set_constraints([1.5]) # Only set first constraint + def test_set_constraints_round_trip(self): + """get_constraints reads back what set_constraints wrote, before any step.""" + mech = _build_four_bar() + target = [c * 1.2 for c in mech.get_constraints()] + mech.set_constraints(target) + assert mech.get_constraints() == pytest.approx(target) + + @pytest.mark.parametrize("group_solver", [True, False]) + def test_set_constraints_drives_the_simulation(self, group_solver): + """The solver maintains the new lengths, not the ones the mechanism was built with.""" + mech = _build_four_bar() + mech._use_group_solver = group_solver + crank_r, coupler_l, rocker_l = 1.2, 3.6, 2.4 + mech.set_constraints([crank_r, coupler_l, rocker_l]) + + for positions in mech.step(iterations=20): + pos = dict(zip([j.id for j in mech.joints], positions, strict=True)) + assert math.dist(pos["O1"], pos["A"]) == pytest.approx(crank_r) + assert math.dist(pos["A"], pos["B"]) == pytest.approx(coupler_l) + assert math.dist(pos["O2"], pos["B"]) == pytest.approx(rocker_l) + + def test_set_constraints_survives_serialization(self): + """A zero-length step re-solves the pose so to_dict/from_dict keeps the new lengths.""" + from pylinkage.mechanism import mechanism_from_dict, mechanism_to_dict + + mech = _build_four_bar() + target = [1.2, 3.6, 2.4] + mech.set_constraints(target) + next(mech.step(iterations=1, dt=0.0)) + rebuilt = mechanism_from_dict(mechanism_to_dict(mech)) + assert rebuilt.get_constraints() == pytest.approx(target) + class TestMechanismJointPositions: """Tests for get_joint_positions() / set_joint_positions()."""