From 0c9ccf74087ae2b97c989a9726e12320af9d6001 Mon Sep 17 00:00:00 2001 From: Claude Date: Sun, 20 Sep 2026 14:27:12 +0000 Subject: [PATCH] Keep the Cartesian streaming executor on its line and under its ceiling MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit MOVECART and SERVOL promise the tool travels a straight line. Measured on a 160 mm diagonal it left that line by 4.7 mm and ran the TCP at 1.35x the configured speed. Three things, each verified against the real executor: Ruckig's synchronization was never set, so it defaulted to Time, which only makes the six tangent components FINISH together — each still takes its own time-optimal route there and the tangent bows. Phase holds them to one shared profile. Phase synchronization then has to survive the way a servo stream drives the executor: SERVOL repeats its target every tick, and each re-plan tests the current velocity and acceleration against a fresh profile and drops to time synchronization unless they line up exactly. Once out of phase the state drifts further out, so the next tick fails the test too. Setting the same target again is now a no-op. Set once the path was already straight; repeated every tick it bowed 4.7 mm. The configured ceilings are TCP speeds, but Ruckig bounds each component on its own, so an isotropic envelope let a diagonal run the resultant up to sqrt(3) times the limit. Under one shared profile the tangent runs along a fixed direction, so scaling the per-component ceilings by that direction's largest component over its norm makes the two agree. The same move now leaves the line by 0.000 mm and peaks at exactly the 0.160 m/s jog ceiling. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01EmiThKnCSjh69Eu7Jbh5uX --- parol6/motion/streaming_executors.py | 119 +++++++++++++++++-- tests/unit/test_cartesian_streaming_line.py | 123 ++++++++++++++++++++ 2 files changed, 235 insertions(+), 7 deletions(-) create mode 100644 tests/unit/test_cartesian_streaming_line.py diff --git a/parol6/motion/streaming_executors.py b/parol6/motion/streaming_executors.py index 0adafe4..7b40f29 100644 --- a/parol6/motion/streaming_executors.py +++ b/parol6/motion/streaming_executors.py @@ -10,12 +10,20 @@ """ import logging +import math from abc import ABC, abstractmethod import numpy as np from numba import njit from numpy.typing import NDArray -from ruckig import ControlInterface, InputParameter, OutputParameter, Result, Ruckig # type: ignore[unresolved-import, ty:unresolved-import] +from ruckig import ( # type: ignore[unresolved-import, ty:unresolved-import] + ControlInterface, + InputParameter, + OutputParameter, + Result, + Ruckig, + Synchronization, +) import parol6.PAROL6_ROBOT as PAROL6_ROBOT from parol6.config import INTERVAL_S, LIMITS @@ -449,12 +457,30 @@ def __init__(self, dt: float = INTERVAL_S): self._target_velocity_arr = np.zeros(6, dtype=np.float64) self._target_acceleration_arr = np.zeros(6, dtype=np.float64) + # Unit direction of the motion the limits are being applied to, + # and the tangent the last tick reached. _apply_limits reads the + # direction, and super().__init__() calls it, so both exist first. + self._direction = np.zeros(6, dtype=np.float64) + self._cur_tangent = np.zeros(6, dtype=np.float64) + self._delta_tangent = np.zeros(6, dtype=np.float64) + self._last_target = np.zeros(6, dtype=np.float64) + self._has_target = False + super().__init__(num_dofs=6, dt=dt) # 6-DOF: [x, y, z, wx, wy, wz] self._tangent_buf = np.zeros(6, dtype=np.float64) self._vel_np_buf = np.zeros(6, dtype=np.float64) self._world_vel_buf = np.zeros(6, dtype=np.float64) + # Ruckig's default (Time) only makes the six components FINISH + # together; each still takes its own time-optimal route there, so + # the tangent bows and the TCP leaves the straight line by + # millimetres. Phase holds them to one shared profile, which is + # what makes the interpolation the screw geodesic. Ruckig falls + # back to time synchronization by itself when the limits make a + # shared profile impossible. + self.inp.synchronization = Synchronization.Phase + # SE3 workspace buffers let the JIT pose conversions run with zero allocation. self._ref_inv_buf = np.zeros((4, 4), dtype=np.float64) self._delta_buf = np.zeros((4, 4), dtype=np.float64) @@ -485,21 +511,66 @@ def _init_state(self) -> None: self.inp.target_acceleration = self._zeros self._apply_limits() + def _set_direction(self, vec: np.ndarray) -> None: + """Point the envelope along `vec`, leaving it as it was when + `vec` is too small to take a direction from. + + A servo stream retargets every tick and the remaining delta + shrinks to nothing as the move lands; snapping back to isotropic + there would change the limits under a move still running. + """ + norm = 0.0 + for i in range(6): + norm += vec[i] * vec[i] + if norm <= 1e-24: + return + inv = 1.0 / math.sqrt(norm) + for i in range(6): + self._direction[i] = vec[i] * inv + + def _direction_scale(self, lo: int, hi: int) -> float: + """Ratio turning a per-component ceiling into a TCP-norm one. + + The configured ceilings are TCP speeds -- what the tool may + travel at, not what each axis may. Ruckig bounds each component + on its own, so an isotropic envelope lets a diagonal run the norm + up to sqrt(3) times the ceiling: a 200 mm/s limit reaches 269 + mm/s on a three-axis move. Under phase synchronization the six + components share one profile, so the tangent runs along a fixed + direction at some scalar rate -- the component Ruckig binds on is + the largest |d|, and the norm is |d| over the half. Their ratio + makes the two agree. + """ + norm = 0.0 + largest = 0.0 + for i in range(lo, hi): + v = self._direction[i] + norm += v * v + a = abs(v) + if a > largest: + largest = a + if norm <= 0.0: + return 1.0 + return largest / math.sqrt(norm) + def _apply_limits(self) -> None: """Apply current limits (with scaling) to Ruckig parameters. Uses pre-allocated numpy arrays to avoid per-tick allocations. """ - self._max_velocity_arr[:3] = self._v_lin_max * self._vel_scale - self._max_velocity_arr[3:] = self._v_ang_max * self._vel_scale + lin = self._direction_scale(0, 3) + ang = self._direction_scale(3, 6) + + self._max_velocity_arr[:3] = self._v_lin_max * self._vel_scale * lin + self._max_velocity_arr[3:] = self._v_ang_max * self._vel_scale * ang self.inp.max_velocity = self._max_velocity_arr - self._max_acceleration_arr[:3] = self._a_lin_max * self._acc_scale - self._max_acceleration_arr[3:] = self._a_ang_max * self._acc_scale + self._max_acceleration_arr[:3] = self._a_lin_max * self._acc_scale * lin + self._max_acceleration_arr[3:] = self._a_ang_max * self._acc_scale * ang self.inp.max_acceleration = self._max_acceleration_arr - self._max_jerk_arr[:3] = self._j_lin_max - self._max_jerk_arr[3:] = self._j_ang_max + self._max_jerk_arr[:3] = self._j_lin_max * lin + self._max_jerk_arr[3:] = self._j_ang_max * ang self.inp.max_jerk = self._max_jerk_arr def sync_pose(self, current_pose: np.ndarray) -> None: @@ -513,6 +584,8 @@ def sync_pose(self, current_pose: np.ndarray) -> None: current_pose: Current TCP pose as 4x4 SE3 matrix """ self.reference_pose = current_pose.copy() # avoid aliasing with cached FK + self._cur_tangent.fill(0.0) + self._has_target = False # Reset Ruckig state to origin (relative to reference) self.inp.current_position = self._zeros self.inp.current_velocity = self._zeros @@ -584,6 +657,33 @@ def set_pose_target(self, target_pose: np.ndarray) -> None: """ target_tangent = self._pose_to_tangent(target_pose) + # Re-planning a target Ruckig is already tracking costs the phase + # synchronization that keeps the TCP on its line. A re-plan tests + # the current velocity and acceleration against the new profile + # and falls back to time synchronization unless they line up + # exactly; once it has, the state drifts further out of phase and + # the next tick fails the test again. A servo stream repeats its + # target at the tick rate, so this is the common case, not an + # edge one: the same move retargeted every tick left the line by + # 4.7 mm, and left by none at all when set once. + if self._has_target: + same = True + for i in range(6): + if self._last_target[i] != target_tangent[i]: + same = False + break + if same: + self.active = True + return + self._last_target[:] = target_tangent + self._has_target = True + + # The envelope is direction-dependent (see _apply_limits), and + # the direction is the one from where the limiter is to the + # target, not the target's own bearing from the reference. + np.subtract(target_tangent, self._cur_tangent, out=self._delta_tangent) + self._set_direction(self._delta_tangent) + self.inp.control_interface = ControlInterface.Position self.inp.target_position = target_tangent self.inp.target_velocity = self._zeros # Stop at target @@ -614,6 +714,8 @@ def set_jog_velocity_1dof( else: self._target_velocity_arr[axis] = velocity + self._has_target = False + self._set_direction(self._target_velocity_arr) self.inp.control_interface = ControlInterface.Velocity self.inp.target_velocity = self._target_velocity_arr self._target_acceleration_arr.fill(0.0) @@ -655,6 +757,8 @@ def set_jog_velocity_1dof_wrf( np.dot(R.T, self._world_vel_buf[:3], self._target_velocity_arr[:3]) np.dot(R.T, self._world_vel_buf[3:], self._target_velocity_arr[3:]) + self._has_target = False + self._set_direction(self._target_velocity_arr) self.inp.control_interface = ControlInterface.Velocity self.inp.target_velocity = self._target_velocity_arr self._target_acceleration_arr.fill(0.0) @@ -700,6 +804,7 @@ def tick(self) -> tuple[np.ndarray, NDArray[np.float64], bool]: ) smoothed_pose = self._tangent_to_pose(pos) + self._cur_tangent[:] = pos self._vel_np_buf[:] = vel # Don't auto-deactivate in velocity mode - caller controls via set_jog_velocity(0) diff --git a/tests/unit/test_cartesian_streaming_line.py b/tests/unit/test_cartesian_streaming_line.py new file mode 100644 index 0000000..3ecb47e --- /dev/null +++ b/tests/unit/test_cartesian_streaming_line.py @@ -0,0 +1,123 @@ +"""The Cartesian streaming executor has to keep the TCP on its line. + +That is what MOVECART and SERVOL promise, and both drive the executor +the same way a servo stream does: the same target, repeated at the tick +rate. +""" + +import math + +import numpy as np +import pytest + +from parol6.config import LIMITS +from parol6.motion.streaming_executors import CartesianStreamingExecutor + +DT = 0.01 +# A diagonal in all three axes: a move along one axis alone cannot tell a +# straight path from a bowed one. +DELTA_M = np.array([0.12, -0.09, 0.06]) + + +def _pose(xyz): + m = np.eye(4) + m[:3, 3] = xyz + return m + + +def _sweep(retarget_every_tick: bool): + """Drive a MOVECART to completion, returning (worst off-line m, peak + TCP speed m/s, miss distance m).""" + cse = CartesianStreamingExecutor(dt=DT) + start = _pose([0.35, 0.10, 0.20]) + cse.sync_pose(start) + goal = _pose(start[:3, 3] + DELTA_M) + + a = start[:3, 3].copy() + b = goal[:3, 3].copy() + unit = (b - a) / np.linalg.norm(b - a) + + cse.set_pose_target(goal) + worst_off, peak = 0.0, 0.0 + prev = a.copy() + for _ in range(20_000): + if retarget_every_tick: + cse.set_pose_target(goal) + pose, _vel, finished = cse.tick() + p = pose[:3, 3] + rel = p - a + worst_off = max(worst_off, float(np.linalg.norm(rel - (rel @ unit) * unit))) + peak = max(peak, float(np.linalg.norm(p - prev)) / DT) + prev = p.copy() + if finished: + break + else: + pytest.fail("the move never finished") + return worst_off, peak, float(np.linalg.norm(prev - b)) + + +@pytest.mark.unit +def test_cartesian_stream_holds_its_line_and_its_tcp_speed(): + """Two things the executor owes a Cartesian move. + + The path is straight, whether the target is set once or repeated + every tick as a servo stream does — Ruckig only holds the six tangent + components to one shared profile under phase synchronization, and a + re-plan drops out of phase unless the target is left alone once set. + + The speed ceiling is a TCP speed, not a per-axis one. Ruckig bounds + each component separately, so an isotropic envelope lets a diagonal + run the resultant up to sqrt(3) times the configured limit. + """ + ceiling = LIMITS.cart.jog.velocity.linear + + for repeated in (False, True): + off, peak, miss = _sweep(retarget_every_tick=repeated) + how = "repeated every tick" if repeated else "set once" + assert off < 1e-6, f"target {how}: TCP bowed {off * 1000:.3f} mm off the line" + assert peak <= ceiling * 1.01, ( + f"target {how}: TCP ran at {peak:.4f} m/s over a {ceiling:.4f} m/s ceiling" + ) + assert miss < 1e-9, f"target {how}: stopped {miss * 1000:.4f} mm short" + + +@pytest.mark.unit +def test_cartesian_stream_rations_a_mixed_move_between_both_ceilings(): + """A move that turns spends its budget on both halves at once, so + neither the linear nor the angular ceiling may be exceeded.""" + cse = CartesianStreamingExecutor(dt=DT) + start = _pose([0.30, 0.05, 0.25]) + cse.sync_pose(start) + + goal = start.copy() + goal[:3, 3] = start[:3, 3] + DELTA_M + # A rotation about Z, well clear of the pi wrap. + angle = 0.6 + c, s = math.cos(angle), math.sin(angle) + goal[:3, :3] = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]) + + cse.set_pose_target(goal) + peak_lin, peak_ang = 0.0, 0.0 + prev_p = start[:3, 3].copy() + prev_R = start[:3, :3].copy() + for _ in range(20_000): + cse.set_pose_target(goal) + pose, _vel, finished = cse.tick() + peak_lin = max(peak_lin, float(np.linalg.norm(pose[:3, 3] - prev_p)) / DT) + # Rotation angle between consecutive orientations. + dR = prev_R.T @ pose[:3, :3] + cos = (np.trace(dR) - 1.0) / 2.0 + peak_ang = max(peak_ang, math.acos(min(1.0, max(-1.0, cos))) / DT) + prev_p = pose[:3, 3].copy() + prev_R = pose[:3, :3].copy() + if finished: + break + else: + pytest.fail("the move never finished") + + assert peak_lin <= LIMITS.cart.jog.velocity.linear * 1.01, ( + f"linear TCP speed {peak_lin:.4f} m/s over its ceiling" + ) + assert peak_ang <= LIMITS.cart.jog.velocity.angular * 1.01, ( + f"angular TCP speed {peak_ang:.4f} rad/s over its ceiling" + )