Keep the Cartesian streaming executor on its line and under its ceiling

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)

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