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