From 8e77197b1ea9e6b815f1327ac56ee9a9380eeb84 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 23 Sep 2026 06:48:11 +0000 Subject: [PATCH 1/7] Add the SEPTIC motion profile A septic time scaling of the path coordinate whose velocity, acceleration and jerk are all zero at both ends, alongside the existing profiles: the trajectory builder, the profile command, the error catalogue, the state and streaming executors, both clients and the README, with tests. It is what par6 moves on too, so the two backends offer the same profile set. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_01MozxHZWkbgHcaRGMkHnJe9 Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_016uVshJDKTz7nCqrMGVxVJU --- README.md | 3 +- parol6/client/async_client.py | 2 +- parol6/client/sync_client.py | 2 +- parol6/commands/system_commands.py | 5 ++- parol6/motion/streaming_executors.py | 2 +- parol6/motion/trajectory.py | 46 +++++++++++++++++++++- parol6/server/state.py | 2 +- parol6/utils/error_catalog.py | 2 +- tests/integration/test_profile_commands.py | 18 +++++---- tests/unit/test_motion.py | 44 +++++++++++++++++++++ 10 files changed, 111 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 034966b..b53def4 100644 --- a/README.md +++ b/README.md @@ -197,7 +197,7 @@ flowchart TB - **MotionPlanner** (`parol6.server.motion_planner`): Separate subprocess for trajectory computation (TOPPRA, IK chains) — keeps the 100 Hz loop free. Only planned moves (MoveJ, MoveL, MoveC, MoveS, MoveP) go through the planner; streaming commands (JogJ, ServoJ, etc.) execute directly in the main loop - **SegmentPlayer** (`parol6.server.segment_player`): Consumes computed trajectory segments in the control loop — indexes one waypoint per tick with zero allocation - **StreamingExecutor** (`parol6.motion.streaming_executors`): Joint-space and Cartesian Ruckig-based executors for real-time jog/servo commands -- **Motion pipeline** (`parol6.motion`): Offline trajectory generation (TOPPRA, Ruckig, Quintic, Trapezoid, Linear) and online streaming executors +- **Motion pipeline** (`parol6.motion`): Offline trajectory generation (TOPPRA, Ruckig, Quintic, Septic, Trapezoid, Linear) and online streaming executors - **Transports** (`parol6.server.transports`): `SerialTransport` (hardware, 3 Mbaud), `MockSerialTransport` (simulator via shared memory IPC) - **StatusCache** (`parol6.server.status_cache`): Change-detection cache with async IK worker for cartesian/joint enablement computation @@ -293,6 +293,7 @@ client.set_profile("TOPPRA") # Default: time-optimal path-following | **TOPPRA** | Time-optimal path-following (default) | | **RUCKIG** | Jerk-limited point-to-point motion (joint moves only) | | **QUINTIC** | C² smooth polynomial trajectories | +| **SEPTIC** | C³ smooth polynomial: jerk also starts and ends at zero, jerk-limited | | **TRAPEZOID** | Linear segments with parabolic blends | | **LINEAR** | Direct interpolation (no smoothing) | diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 18cce13..59b1655 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -1381,7 +1381,7 @@ async def select_profile(self, profile: str) -> int: rbt.select_profile("TOPPRA") Args: - profile: Motion profile type ('TOPPRA', 'RUCKIG', 'QUINTIC', 'TRAPEZOID', 'LINEAR') + profile: Motion profile type ('TOPPRA', 'RUCKIG', 'QUINTIC', 'SEPTIC', 'TRAPEZOID', 'LINEAR') Note: RUCKIG is point-to-point only; Cartesian moves will use TOPPRA. Returns: diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index 23d20aa..ae18ce2 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -427,7 +427,7 @@ def select_profile(self, profile: str) -> int: """Set the motion profile (e.g. ``"TOPPRA"``). Args: - profile: Motion profile type ('TOPPRA', 'RUCKIG', 'QUINTIC', 'TRAPEZOID', 'LINEAR') + profile: Motion profile type ('TOPPRA', 'RUCKIG', 'QUINTIC', 'SEPTIC', 'TRAPEZOID', 'LINEAR') Note: RUCKIG is point-to-point only; Cartesian moves will use TOPPRA. Returns: diff --git a/parol6/commands/system_commands.py b/parol6/commands/system_commands.py index 5e2ea44..80a83fa 100644 --- a/parol6/commands/system_commands.py +++ b/parol6/commands/system_commands.py @@ -167,7 +167,9 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: return ExecutionStatusCode.COMPLETED -VALID_PROFILES = frozenset(("TOPPRA", "RUCKIG", "QUINTIC", "TRAPEZOID", "LINEAR")) +VALID_PROFILES = frozenset( + ("TOPPRA", "RUCKIG", "QUINTIC", "SEPTIC", "TRAPEZOID", "LINEAR") +) @register_command(CmdType.SELECT_PROFILE) @@ -181,6 +183,7 @@ class SelectProfileCommand(SystemCommand[SelectProfileCmd]): TOPPRA - Time-optimal path parameterization (default) RUCKIG - Time-optimal jerk-limited (point-to-point only, joint moves only) QUINTIC - C² smooth polynomial trajectories + SEPTIC - C³ smooth polynomial: jerk also zero at both ends, jerk-limited TRAPEZOID - Linear segments with parabolic blends LINEAR - Direct interpolation (no smoothing) diff --git a/parol6/motion/streaming_executors.py b/parol6/motion/streaming_executors.py index e4e1998..6de87cd 100644 --- a/parol6/motion/streaming_executors.py +++ b/parol6/motion/streaming_executors.py @@ -6,7 +6,7 @@ - CartesianStreamingExecutor: Cartesian-space jogging and streaming Precomputed trajectories bypass these executors and go directly to the controller, -since they're already time-optimal (TOPPRA/RUCKIG) or validated (QUINTIC/TRAPEZOID). +since they're already time-optimal (TOPPRA/RUCKIG) or validated (QUINTIC/SEPTIC/TRAPEZOID). """ import logging diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index fd1774b..19fddbc 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -107,6 +107,12 @@ def _quintic_samples( _QUINTIC_PEAK_VEL = 1.875 _QUINTIC_PEAK_ACC = 10.0 / math.sqrt(3.0) +# Peaks of the unit septic s(u) = 35u⁴ − 84u⁵ + 70u⁶ − 20u⁷ on u ∈ [0, 1]: +# ds/du and |d³s/du³| at u = 1/2, |d²s/du²| at u = (5 ∓ √5)/10. +_SEPTIC_PEAK_VEL: float = 35.0 / 16.0 +_SEPTIC_PEAK_ACC: float = 84.0 * math.sqrt(5.0) / 25.0 +_SEPTIC_PEAK_JERK: float = 52.5 + # A timing solver's duration can land a hair past a whole number of ticks. _TICK_ROUNDING = 1e-6 @@ -152,6 +158,7 @@ class ProfileType(Enum): TOPPRA = "toppra" # Time-optimal path following (default) RUCKIG = "ruckig" # Point-to-point jerk-limited (can't follow Cartesian paths) QUINTIC = "quintic" # Quintic polynomial (C² smooth, predictable shape) + SEPTIC = "septic" # Septic polynomial (C³ smooth: jerk also zero at the ends) TRAPEZOID = "trapezoid" # Trapezoidal velocity profile LINEAR = "linear" # Direct linear interpolation (no smoothing) @@ -722,7 +729,7 @@ def build(self) -> Trajectory: that is not a straight run between them is timed by TOPP-RA instead. QUINTIC and TRAPEZOID run each joint on its own profile from end to end on a straight run, and the path coordinate along - any other path. + any other path. SEPTIC runs the path coordinate on every path. Returns: Trajectory ready for execution, its rows one control tick apart @@ -746,6 +753,7 @@ def build(self) -> Trajectory: if self.path_knots is not None and self.profile in ( ProfileType.LINEAR, ProfileType.QUINTIC, + ProfileType.SEPTIC, ProfileType.TRAPEZOID, ): # These profiles time a path coordinate spaced by row; spaced @@ -763,6 +771,8 @@ def build(self) -> Trajectory: if along_path: return self._build_quintic_trajectory_along_path() return self._build_quintic_trajectory_joint() + elif self.profile == ProfileType.SEPTIC: + return self._build_septic_trajectory() elif self.profile == ProfileType.TRAPEZOID: if along_path: # A trapezoid of the path coordinate is what LINEAR runs. @@ -1289,6 +1299,40 @@ def _build_quintic_trajectory_along_path(self) -> Trajectory: return self._trajectory(trajectory_rad) + def _build_septic_trajectory(self) -> Trajectory: + """ + Build trajectory with a septic polynomial on the path coordinate. + + Velocity, acceleration and jerk are all zero at both ends, so unlike + the quintic the move neither starts nor stops with a jerk step. One + scalar profile times the whole path, joint and cartesian alike, so + every joint arrives together and a joint move stays on its straight + joint-space line. On a cartesian path the profile's peak keeps the + tool under its ceiling. + """ + positions = self.joint_path.positions + # Steepest dq/ds of each joint over the path's segments. A joint move + # has one slope, its displacement; a cartesian path bends in joint + # space, and its steepest segment is what the limits have to cover. + slope = np.max(np.abs(np.diff(positions, axis=0)), axis=0) * ( + len(positions) - 1 + ) + fastest = max( + float(np.max(_SEPTIC_PEAK_VEL * slope / self.v_max)), + float(np.max(np.sqrt(_SEPTIC_PEAK_ACC * slope / self.a_max))), + float(np.max(np.cbrt(_SEPTIC_PEAK_JERK * slope / self.j_max))), + _SEPTIC_PEAK_VEL / self._cruise_cap(), + self.dt * 2, + ) + intervals = _tick_intervals(self._requested_or(fastest), self.dt) + u = np.linspace(0.0, 1.0, intervals + 1) + s = u**4 * (35.0 - 84.0 * u + 70.0 * u**2 - 20.0 * u**3) + trajectory_rad = self.joint_path.sample_many(s) + + trajectory_rad = self._enforce_segment_limits(trajectory_rad) + + return self._trajectory(trajectory_rad) + def _build_trapezoid_trajectory_joint(self) -> Trajectory: """ Build per-joint trapezoidal trajectory. diff --git a/parol6/server/state.py b/parol6/server/state.py index 4278dd5..7e7e0f3 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -175,7 +175,7 @@ class ControllerState: disabled_reason: str = "" e_stop_active: bool = False - # Motion profile for all moves (TOPPRA, RUCKIG, QUINTIC, TRAPEZOID, LINEAR) + # Motion profile for all moves (TOPPRA, RUCKIG, QUINTIC, SEPTIC, TRAPEZOID, LINEAR) # Note: RUCKIG is point-to-point only; Cartesian moves fall back to TOPPRA motion_profile: str = "TOPPRA" diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index 1980eb4..584c289 100644 --- a/parol6/utils/error_catalog.py +++ b/parol6/utils/error_catalog.py @@ -156,7 +156,7 @@ class _ErrorTemplate: title="Invalid motion profile", cause="Unrecognized motion profile: {detail}", effect="Profile not changed.", - remedy="Use one of: TOPPRA, RUCKIG, QUINTIC, TRAPEZOID, LINEAR.", + remedy="Use one of: TOPPRA, RUCKIG, QUINTIC, SEPTIC, TRAPEZOID, LINEAR.", ), ErrorCode.SYS_NOT_SIMULATOR: _ErrorTemplate( title="Simulator-only command", diff --git a/tests/integration/test_profile_commands.py b/tests/integration/test_profile_commands.py index 9b1a8af..839b751 100644 --- a/tests/integration/test_profile_commands.py +++ b/tests/integration/test_profile_commands.py @@ -24,7 +24,7 @@ def test_profile_returns_default(self, client, server_proc): def test_select_and_get_profile_roundtrip(self, client, server_proc): """Test setting a profile and getting it back.""" - for p in ["LINEAR", "QUINTIC", "TRAPEZOID", "RUCKIG", "TOPPRA"]: + for p in ["LINEAR", "QUINTIC", "SEPTIC", "TRAPEZOID", "RUCKIG", "TOPPRA"]: assert client.select_profile(p) > 0 assert client.profile() == p @@ -48,9 +48,9 @@ def test_joint_move_reaches_target_all_profiles(self, client, server_proc): target_angles = [10, -50, 190, 5, 10, 15] standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] - for p in ["LINEAR", "QUINTIC", "TRAPEZOID", "RUCKIG", "TOPPRA"]: + for p in ["LINEAR", "QUINTIC", "SEPTIC", "TRAPEZOID", "RUCKIG", "TOPPRA"]: # Teleport to the standby pose: each profile starts from the same - # spot without paying for five planned home moves (home() on a + # spot without paying for a planned home move per profile (home() on a # referenced robot is a real planned move now, not a firmware # re-home — slow CI runners blew the per-test timeout on it). assert client.teleport(standby) == 1 @@ -96,8 +96,8 @@ def test_cartesian_move_reaches_target_all_profiles(self, client, server_proc): # All profiles should work for Cartesian moves # RUCKIG falls back to TOPPRA automatically - for p in ["LINEAR", "QUINTIC", "TRAPEZOID", "RUCKIG", "TOPPRA"]: - # Teleport, don't drive: five planned home moves blow the + for p in ["LINEAR", "QUINTIC", "SEPTIC", "TRAPEZOID", "RUCKIG", "TOPPRA"]: + # Teleport, don't drive: a planned home move per profile blows the # per-test timeout on slow runners. assert client.teleport(standby) == 1 @@ -231,7 +231,9 @@ def test_servo_l_sequential(self, client, server_proc): class TestCartesianPrecision: """Test Cartesian move precision with different profiles.""" - @pytest.mark.parametrize("profile", ["TOPPRA", "LINEAR", "QUINTIC", "TRAPEZOID"]) + @pytest.mark.parametrize( + "profile", ["TOPPRA", "LINEAR", "QUINTIC", "SEPTIC", "TRAPEZOID"] + ) def test_cartesian_simple_sequence(self, client, server_proc, profile): """ Test precision of simple Cartesian moves with all profiles. @@ -342,7 +344,9 @@ def _extract_position_from_pose_matrix(pose_flat: list[float]) -> np.ndarray: class TestTCPPathAccuracy: """Test that Cartesian moves follow straight-line TCP paths.""" - @pytest.mark.parametrize("profile", ["TOPPRA", "LINEAR", "QUINTIC", "TRAPEZOID"]) + @pytest.mark.parametrize( + "profile", ["TOPPRA", "LINEAR", "QUINTIC", "SEPTIC", "TRAPEZOID"] + ) def test_cartesian_follows_straight_line(self, client, server_proc, profile): """ Verify TCP follows a straight line during Cartesian moves. diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index 01abdce..02238eb 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -179,6 +179,7 @@ def test_single_point_path(self): [ ProfileType.LINEAR, ProfileType.QUINTIC, + ProfileType.SEPTIC, ProfileType.TRAPEZOID, ProfileType.TOPPRA, ProfileType.RUCKIG, @@ -216,6 +217,49 @@ def test_trajectory_respects_joint_velocity_limits( f"{(max_vel_ratio - 1) * 100:.1f}%" ) + def test_septic_holds_its_limits_and_starts_and_stops_without_a_jerk_step( + self, simple_joint_path + ): + """SEPTIC stays inside the velocity, acceleration and jerk limits, and + its jerk ramps in and out where QUINTIC's steps straight to its peak.""" + + def derivatives(profile): + traj = TrajectoryBuilder( + joint_path=simple_joint_path, profile=profile, dt=INTERVAL_S + ).build() + q = traj.positions_rad + dt = INTERVAL_S + # At rest before and after, so the differences see both ends. + q = np.vstack([q[:1], q[:1], q, q[-1:], q[-1:]]) + v = np.diff(q, axis=0) / dt + a = np.diff(v, axis=0) / dt + return v, a, np.diff(a, axis=0) / dt + + v, a, j = derivatives(ProfileType.SEPTIC) + hard = LIMITS.joint.hard + for name, x, limit in [ + ("velocity", v, hard.velocity), + ("acceleration", a, hard.acceleration), + ("jerk", j, hard.jerk), + ]: + ratio = float(np.max(np.abs(x) / limit)) + assert ratio <= 1.0, f"SEPTIC {name} at {ratio:.3f}x its limit" + + moving = np.abs( + simple_joint_path.positions[-1] - simple_joint_path.positions[0] + ) + joint = int(np.argmax(moving)) + peak = np.max(np.abs(j[:, joint])) + for at, edge in [("start", j[:2, joint]), ("end", j[-2:, joint])]: + assert np.max(np.abs(edge)) < 0.1 * peak, ( + f"SEPTIC jerk at the {at} is {np.max(np.abs(edge)):.3f} " + f"against a peak of {peak:.3f}" + ) + # The control: the quintic's jerk is at its peak on the first sample, + # so the assertion above discriminates rather than merely passes. + _, _, jq = derivatives(ProfileType.QUINTIC) + assert np.max(np.abs(jq[:2, joint])) > 0.5 * np.max(np.abs(jq[:, joint])) + class TestTrajectory: """Tests for Trajectory dataclass.""" From b1e3dcb89558936c615e03736b2c2078408cac0e Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sun, 11 Oct 2026 21:36:06 +0000 Subject: [PATCH 2/7] Run SEPTIC point to point, as par6 does Timed as one polynomial along a cartesian path, SEPTIC sized the whole move from its steepest row. From standby the wrist's turn off its singularity leaves a few degrees of J4 in the path's first row, so a 40 mm MoveL planned 24 s against TOPPRA's 2 s; along a bent path its jerk bound did not hold either. SEPTIC now runs a straight joint run only, on the per-joint polynomial builder it shares with QUINTIC, and a cartesian path or a blended chain is timed by TOPPRA, as with RUCKIG and as par6 does. - VALID_PROFILES and the invalid-profile remedy come from ProfileType. - The async client's robot setter takes any waldoctl Robot, as the interface declares; ty 0.0.86 refuses the narrower one. - The limit test runs a move the jerk limit times, and the velocity test differentiates at the tick the rows are played at. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_016uVshJDKTz7nCqrMGVxVJU --- README.md | 4 +- parol6/client/async_client.py | 12 +- parol6/client/dry_run_client.py | 4 +- parol6/client/sync_client.py | 2 +- parol6/commands/system_commands.py | 15 +-- parol6/motion/streaming_executors.py | 2 +- parol6/motion/trajectory.py | 128 ++++++++++----------- parol6/server/state.py | 4 +- parol6/utils/error_catalog.py | 2 +- tests/integration/test_profile_commands.py | 43 +++++-- tests/unit/test_motion.py | 63 +++++----- 11 files changed, 153 insertions(+), 126 deletions(-) diff --git a/README.md b/README.md index b53def4..b2a7aa8 100644 --- a/README.md +++ b/README.md @@ -293,11 +293,11 @@ client.set_profile("TOPPRA") # Default: time-optimal path-following | **TOPPRA** | Time-optimal path-following (default) | | **RUCKIG** | Jerk-limited point-to-point motion (joint moves only) | | **QUINTIC** | C² smooth polynomial trajectories | -| **SEPTIC** | C³ smooth polynomial: jerk also starts and ends at zero, jerk-limited | +| **SEPTIC** | C³ smooth polynomial, jerk-limited: jerk also starts and ends at zero (joint moves only) | | **TRAPEZOID** | Linear segments with parabolic blends | | **LINEAR** | Direct interpolation (no smoothing) | -Note: RUCKIG is point-to-point only and cannot follow Cartesian paths. When RUCKIG is set, Cartesian moves automatically use TOPPRA instead. +Note: RUCKIG and SEPTIC are point-to-point only and cannot follow Cartesian paths. When either is set, Cartesian moves automatically use TOPPRA instead. ### Speed and acceleration diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 59b1655..d972846 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -242,6 +242,8 @@ def _create_unicast_socket(port: int, host: str) -> socket.socket: if TYPE_CHECKING: from typing import Protocol + from waldoctl import Robot as WaldoRobot + from parol6.robot import Robot class _StatusNotifier(Protocol): @@ -320,7 +322,13 @@ def robot(self) -> "Robot": return self._robot @robot.setter - def robot(self, value: "Robot | None") -> None: + def robot(self, value: "WaldoRobot | None") -> None: + # The interface takes any backend's robot; this client drives only + # its own. + if value is not None: + from parol6.robot import Robot + + assert isinstance(value, Robot) self._robot = value def __init__( @@ -1382,7 +1390,7 @@ async def select_profile(self, profile: str) -> int: Args: profile: Motion profile type ('TOPPRA', 'RUCKIG', 'QUINTIC', 'SEPTIC', 'TRAPEZOID', 'LINEAR') - Note: RUCKIG is point-to-point only; Cartesian moves will use TOPPRA. + Note: RUCKIG and SEPTIC are point-to-point only; Cartesian moves will use TOPPRA. Returns: 1 once the profile is selected, 0 when no reply arrives. diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index b0be8a6..fe6117b 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -751,7 +751,9 @@ def _dispatch(self, params: Any, method: str) -> int: idx, np.empty((0, 6)), error=make_error( - ErrorCode.SYS_PROFILE_INVALID, detail=params.profile + ErrorCode.SYS_PROFILE_INVALID, + detail=params.profile, + allowed=", ".join(VALID_PROFILES), ), ) return idx diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index ae18ce2..547ea48 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -428,7 +428,7 @@ def select_profile(self, profile: str) -> int: Args: profile: Motion profile type ('TOPPRA', 'RUCKIG', 'QUINTIC', 'SEPTIC', 'TRAPEZOID', 'LINEAR') - Note: RUCKIG is point-to-point only; Cartesian moves will use TOPPRA. + Note: RUCKIG and SEPTIC are point-to-point only; Cartesian moves will use TOPPRA. Returns: 1 once the profile is selected, 0 when no reply arrives. diff --git a/parol6/commands/system_commands.py b/parol6/commands/system_commands.py index 80a83fa..3596bf1 100644 --- a/parol6/commands/system_commands.py +++ b/parol6/commands/system_commands.py @@ -19,6 +19,7 @@ SystemCommand, ) from parol6.config import save_com_port +from parol6.motion.trajectory import ProfileType from parol6.protocol.wire import ( CmdType, ConnectHardwareCmd, @@ -167,9 +168,7 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: return ExecutionStatusCode.COMPLETED -VALID_PROFILES = frozenset( - ("TOPPRA", "RUCKIG", "QUINTIC", "SEPTIC", "TRAPEZOID", "LINEAR") -) +VALID_PROFILES = tuple(ProfileType.__members__) @register_command(CmdType.SELECT_PROFILE) @@ -183,12 +182,12 @@ class SelectProfileCommand(SystemCommand[SelectProfileCmd]): TOPPRA - Time-optimal path parameterization (default) RUCKIG - Time-optimal jerk-limited (point-to-point only, joint moves only) QUINTIC - C² smooth polynomial trajectories - SEPTIC - C³ smooth polynomial: jerk also zero at both ends, jerk-limited + SEPTIC - C³ smooth polynomial, jerk-limited (point-to-point only, joint moves only) TRAPEZOID - Linear segments with parabolic blends LINEAR - Direct interpolation (no smoothing) - Note: RUCKIG is point-to-point and cannot follow Cartesian paths. - Cartesian moves will use TOPPRA when RUCKIG is set. + Note: RUCKIG and SEPTIC are point-to-point and cannot follow Cartesian + paths. Cartesian moves will use TOPPRA when either is set. """ PARAMS_TYPE = SelectProfileCmd @@ -201,7 +200,9 @@ def do_setup(self, state: ControllerState) -> None: if profile not in VALID_PROFILES: err = ValueError(f"Invalid profile '{self.p.profile}'") err.robot_error = make_error( # type: ignore[attr-defined, ty:unresolved-attribute] - ErrorCode.SYS_PROFILE_INVALID, detail=self.p.profile + ErrorCode.SYS_PROFILE_INVALID, + detail=self.p.profile, + allowed=", ".join(VALID_PROFILES), ) raise err diff --git a/parol6/motion/streaming_executors.py b/parol6/motion/streaming_executors.py index 6de87cd..9257aac 100644 --- a/parol6/motion/streaming_executors.py +++ b/parol6/motion/streaming_executors.py @@ -6,7 +6,7 @@ - CartesianStreamingExecutor: Cartesian-space jogging and streaming Precomputed trajectories bypass these executors and go directly to the controller, -since they're already time-optimal (TOPPRA/RUCKIG) or validated (QUINTIC/SEPTIC/TRAPEZOID). +since TrajectoryBuilder has already timed them within the joint limits. """ import logging diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index 19fddbc..24994d2 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -15,6 +15,7 @@ import logging import math +from collections.abc import Callable from dataclasses import dataclass from enum import Enum @@ -103,11 +104,21 @@ def _quintic_samples( return q0 + (q1 - q0) * s * s * s * (10.0 - 15.0 * s + 6.0 * s * s) +def _septic_samples( + times: NDArray[np.float64], q0: float, q1: float, duration: float +) -> NDArray[np.float64]: + """Sample a septic profile from ``q0`` to ``q1``, at rest, unaccelerated + and without jerk at both ends.""" + s = np.clip(times / duration, 0.0, 1.0) + s2 = s * s + return q0 + (q1 - q0) * s2 * s2 * (35.0 - 84.0 * s + 70.0 * s2 - 20.0 * s2 * s) + + # Peak ds/dt and d²s/dt² of a quintic from s=0 to s=1 over one second. _QUINTIC_PEAK_VEL = 1.875 _QUINTIC_PEAK_ACC = 10.0 / math.sqrt(3.0) -# Peaks of the unit septic s(u) = 35u⁴ − 84u⁵ + 70u⁶ − 20u⁷ on u ∈ [0, 1]: +# Peaks of the septic s(u) = 35u⁴ − 84u⁵ + 70u⁶ − 20u⁷ over one second: # ds/du and |d³s/du³| at u = 1/2, |d²s/du²| at u = (5 ∓ √5)/10. _SEPTIC_PEAK_VEL: float = 35.0 / 16.0 _SEPTIC_PEAK_ACC: float = 84.0 * math.sqrt(5.0) / 25.0 @@ -158,7 +169,7 @@ class ProfileType(Enum): TOPPRA = "toppra" # Time-optimal path following (default) RUCKIG = "ruckig" # Point-to-point jerk-limited (can't follow Cartesian paths) QUINTIC = "quintic" # Quintic polynomial (C² smooth, predictable shape) - SEPTIC = "septic" # Septic polynomial (C³ smooth: jerk also zero at the ends) + SEPTIC = "septic" # Point-to-point septic polynomial (C³ smooth, jerk-limited) TRAPEZOID = "trapezoid" # Trapezoidal velocity profile LINEAR = "linear" # Direct linear interpolation (no smoothing) @@ -725,11 +736,11 @@ def build(self) -> Trajectory: at the control rate. No interpolation of the original joint path is needed since TOPP-RA's trajectory already provides smooth, continuous positions. - RUCKIG runs point to point, from the first row to the last; a path - that is not a straight run between them is timed by TOPP-RA - instead. QUINTIC and TRAPEZOID run each joint on its own profile - from end to end on a straight run, and the path coordinate along - any other path. SEPTIC runs the path coordinate on every path. + RUCKIG and SEPTIC run point to point, from the first row to the + last; a path that is not a straight run between them is timed by + TOPP-RA instead. QUINTIC and TRAPEZOID run each joint on its own + profile from end to end on a straight run, and the path coordinate + along any other path. Returns: Trajectory ready for execution, its rows one control tick apart @@ -753,7 +764,6 @@ def build(self) -> Trajectory: if self.path_knots is not None and self.profile in ( ProfileType.LINEAR, ProfileType.QUINTIC, - ProfileType.SEPTIC, ProfileType.TRAPEZOID, ): # These profiles time a path coordinate spaced by row; spaced @@ -770,9 +780,19 @@ def build(self) -> Trajectory: elif self.profile == ProfileType.QUINTIC: if along_path: return self._build_quintic_trajectory_along_path() - return self._build_quintic_trajectory_joint() - elif self.profile == ProfileType.SEPTIC: - return self._build_septic_trajectory() + return self._build_polynomial_trajectory_joint( + _quintic_samples, + self._compute_joint_duration_polynomial( + _QUINTIC_PEAK_VEL, _QUINTIC_PEAK_ACC + ), + ) + elif self.profile == ProfileType.SEPTIC and not along_path: + return self._build_polynomial_trajectory_joint( + _septic_samples, + self._compute_joint_duration_polynomial( + _SEPTIC_PEAK_VEL, _SEPTIC_PEAK_ACC, _SEPTIC_PEAK_JERK + ), + ) elif self.profile == ProfileType.TRAPEZOID: if along_path: # A trapezoid of the path coordinate is what LINEAR runs. @@ -1152,7 +1172,7 @@ def _requested_or(self, fastest: float) -> float: return fastest if self.duration < fastest: logger.warning( - "Extending duration from %.3fs to %.3fs to respect velocity/acceleration limits", + "Extending duration from %.3fs to %.3fs to respect the joint limits", self.duration, fastest, ) @@ -1184,16 +1204,18 @@ def _compute_joint_duration_trapezoid(self) -> float: return max(max_duration, self.dt * 2) - def _compute_joint_duration_quintic(self) -> float: + def _compute_joint_duration_polynomial( + self, peak_vel: float, peak_acc: float, peak_jerk: float = 0.0 + ) -> float: """ - Compute duration for joint paths using quintic polynomial profile. - - For quintic polynomials with zero-velocity endpoints: - - Peak velocity at t=T/2: v_peak = 1.875 * delta / T - - Peak acceleration at t=T*(3-sqrt(3))/6: a_peak = 5.77 * delta / T² + Compute duration for joint paths using a polynomial profile. - For velocity limit: T = 1.875 * delta / v_max - For acceleration limit: T = sqrt(5.77 * delta / a_max) + Each peak of the unit polynomial (ds/dt, d²s/dt² and, for a profile + that holds the jerk limit, |d³s/dt³|), scaled by a joint's + displacement, has to fit that joint's limit: + T >= peak_vel * delta / v_max + T >= sqrt(peak_acc * delta / a_max) + T >= cbrt(peak_jerk * delta / j_max) Returns the maximum duration across all joints. """ @@ -1203,16 +1225,21 @@ def _compute_joint_duration_quintic(self) -> float: total_delta = np.abs(positions[-1] - positions[0]) - time_vel = _QUINTIC_PEAK_VEL * total_delta / self.v_max + time_vel = peak_vel * total_delta / self.v_max with np.errstate(divide="ignore", invalid="ignore"): time_acc = np.where( self.a_max > 0, - np.sqrt(_QUINTIC_PEAK_ACC * total_delta / self.a_max), + np.sqrt(peak_acc * total_delta / self.a_max), + 0.0, + ) + time_jerk = np.where( + self.j_max > 0, + np.cbrt(peak_jerk * total_delta / self.j_max), 0.0, ) - time_per_joint = np.maximum(time_vel, time_acc) + time_per_joint = np.maximum(np.maximum(time_vel, time_acc), time_jerk) return max(float(np.max(time_per_joint)), self.dt * 2) def _compute_duration_along_rows(self) -> float: @@ -1241,17 +1268,24 @@ def _compute_duration_along_rows(self) -> float: return max(float(np.sum(segment_times)), self.dt * 2) - def _build_quintic_trajectory_joint(self) -> Trajectory: + def _build_polynomial_trajectory_joint( + self, + samples: Callable[ + [NDArray[np.float64], float, float, float], NDArray[np.float64] + ], + fastest: float, + ) -> Trajectory: """ - Build per-joint quintic trajectory. + Build per-joint polynomial trajectory. - Each joint independently follows a quintic polynomial profile, - synchronized to finish at the same time. + Each joint independently follows the polynomial ``samples``, + synchronized to finish at the same time: over ``fastest``, the + shortest duration the joint limits allow, or a longer one asked for. """ start_pos = self.joint_path.positions[0] end_pos = self.joint_path.positions[-1] - duration = self._requested_or(self._compute_joint_duration_quintic()) + duration = self._requested_or(fastest) intervals = _tick_intervals(duration, self.dt) duration = intervals * self.dt @@ -1264,9 +1298,7 @@ def _build_quintic_trajectory_joint(self) -> Trajectory: trajectory_rad[:, j] = start_pos[j] continue - trajectory_rad[:, j] = _quintic_samples( - times, start_pos[j], end_pos[j], duration - ) + trajectory_rad[:, j] = samples(times, start_pos[j], end_pos[j], duration) trajectory_rad = self._enforce_segment_limits(trajectory_rad) @@ -1299,40 +1331,6 @@ def _build_quintic_trajectory_along_path(self) -> Trajectory: return self._trajectory(trajectory_rad) - def _build_septic_trajectory(self) -> Trajectory: - """ - Build trajectory with a septic polynomial on the path coordinate. - - Velocity, acceleration and jerk are all zero at both ends, so unlike - the quintic the move neither starts nor stops with a jerk step. One - scalar profile times the whole path, joint and cartesian alike, so - every joint arrives together and a joint move stays on its straight - joint-space line. On a cartesian path the profile's peak keeps the - tool under its ceiling. - """ - positions = self.joint_path.positions - # Steepest dq/ds of each joint over the path's segments. A joint move - # has one slope, its displacement; a cartesian path bends in joint - # space, and its steepest segment is what the limits have to cover. - slope = np.max(np.abs(np.diff(positions, axis=0)), axis=0) * ( - len(positions) - 1 - ) - fastest = max( - float(np.max(_SEPTIC_PEAK_VEL * slope / self.v_max)), - float(np.max(np.sqrt(_SEPTIC_PEAK_ACC * slope / self.a_max))), - float(np.max(np.cbrt(_SEPTIC_PEAK_JERK * slope / self.j_max))), - _SEPTIC_PEAK_VEL / self._cruise_cap(), - self.dt * 2, - ) - intervals = _tick_intervals(self._requested_or(fastest), self.dt) - u = np.linspace(0.0, 1.0, intervals + 1) - s = u**4 * (35.0 - 84.0 * u + 70.0 * u**2 - 20.0 * u**3) - trajectory_rad = self.joint_path.sample_many(s) - - trajectory_rad = self._enforce_segment_limits(trajectory_rad) - - return self._trajectory(trajectory_rad) - def _build_trapezoid_trajectory_joint(self) -> Trajectory: """ Build per-joint trapezoidal trajectory. diff --git a/parol6/server/state.py b/parol6/server/state.py index 7e7e0f3..a751192 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -175,8 +175,8 @@ class ControllerState: disabled_reason: str = "" e_stop_active: bool = False - # Motion profile for all moves (TOPPRA, RUCKIG, QUINTIC, SEPTIC, TRAPEZOID, LINEAR) - # Note: RUCKIG is point-to-point only; Cartesian moves fall back to TOPPRA + # Motion profile for all moves, a ProfileType name. + # RUCKIG and SEPTIC are point-to-point only; Cartesian moves fall back to TOPPRA motion_profile: str = "TOPPRA" # Streaming executors for online motion (jogging/streaming) diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index 584c289..5dab25e 100644 --- a/parol6/utils/error_catalog.py +++ b/parol6/utils/error_catalog.py @@ -156,7 +156,7 @@ class _ErrorTemplate: title="Invalid motion profile", cause="Unrecognized motion profile: {detail}", effect="Profile not changed.", - remedy="Use one of: TOPPRA, RUCKIG, QUINTIC, SEPTIC, TRAPEZOID, LINEAR.", + remedy="Use one of: {allowed}.", ), ErrorCode.SYS_NOT_SIMULATOR: _ErrorTemplate( title="Simulator-only command", diff --git a/tests/integration/test_profile_commands.py b/tests/integration/test_profile_commands.py index 839b751..fe3341b 100644 --- a/tests/integration/test_profile_commands.py +++ b/tests/integration/test_profile_commands.py @@ -49,10 +49,8 @@ def test_joint_move_reaches_target_all_profiles(self, client, server_proc): standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] for p in ["LINEAR", "QUINTIC", "SEPTIC", "TRAPEZOID", "RUCKIG", "TOPPRA"]: - # Teleport to the standby pose: each profile starts from the same - # spot without paying for a planned home move per profile (home() on a - # referenced robot is a real planned move now, not a firmware - # re-home — slow CI runners blew the per-test timeout on it). + # Teleport, don't drive: a planned home move per profile blows the + # per-test timeout on slow runners. assert client.teleport(standby) == 1 # Set profile and execute move @@ -95,7 +93,7 @@ def test_cartesian_move_reaches_target_all_profiles(self, client, server_proc): standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] # All profiles should work for Cartesian moves - # RUCKIG falls back to TOPPRA automatically + # RUCKIG and SEPTIC fall back to TOPPRA automatically for p in ["LINEAR", "QUINTIC", "SEPTIC", "TRAPEZOID", "RUCKIG", "TOPPRA"]: # Teleport, don't drive: a planned home move per profile blows the # per-test timeout on slow runners. @@ -165,6 +163,33 @@ def holds_the_move(status) -> bool: f"{planned[-1]:.2f} s" ) + def test_a_septic_cartesian_move_is_timed_along_its_path(self): + """SEPTIC times a straight joint run only. From standby the wrist + sits at its singularity, and a cartesian move leaves it in the + path's first row: a few degrees of J4 and J6 the tool never sees. + Timed as one polynomial from that row's slope, the whole move would run at + the turn's pace, a few centimetres taking ten times what TOPPRA + takes.""" + from parol6.client.dry_run_client import DryRunRobotClient + + import parol6.PAROL6_ROBOT as PAROL6_ROBOT + + standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] + + def previewed(profile: str) -> float: + preview = DryRunRobotClient(initial_joints_deg=standby) + assert preview.select_profile(profile) == 1 + index = preview.move_l([40.0, 0.0, 40.0, 0.0, 0.0, 0.0], rel=True) + record = preview.plan() + assert record.blocks[index].error is None + return record.blocks[index].rows * record.row_dt_s + + septic, toppra = previewed("SEPTIC"), previewed("TOPPRA") + # The wrist's turn ahead of the path is a joint move, run as SEPTIC. + assert septic < 1.5 * toppra, ( + f"SEPTIC planned {septic:.2f} s for a move TOPPRA runs in {toppra:.2f} s" + ) + @pytest.mark.parametrize("profile", ["LINEAR", "QUINTIC", "TRAPEZOID"]) def test_a_short_timed_move_takes_its_whole_duration(self, profile): """A move timed to 50 ms reaches its target 50 ms in, not a control @@ -231,9 +256,7 @@ def test_servo_l_sequential(self, client, server_proc): class TestCartesianPrecision: """Test Cartesian move precision with different profiles.""" - @pytest.mark.parametrize( - "profile", ["TOPPRA", "LINEAR", "QUINTIC", "SEPTIC", "TRAPEZOID"] - ) + @pytest.mark.parametrize("profile", ["TOPPRA", "LINEAR", "QUINTIC", "TRAPEZOID"]) def test_cartesian_simple_sequence(self, client, server_proc, profile): """ Test precision of simple Cartesian moves with all profiles. @@ -344,9 +367,7 @@ def _extract_position_from_pose_matrix(pose_flat: list[float]) -> np.ndarray: class TestTCPPathAccuracy: """Test that Cartesian moves follow straight-line TCP paths.""" - @pytest.mark.parametrize( - "profile", ["TOPPRA", "LINEAR", "QUINTIC", "SEPTIC", "TRAPEZOID"] - ) + @pytest.mark.parametrize("profile", ["TOPPRA", "LINEAR", "QUINTIC", "TRAPEZOID"]) def test_cartesian_follows_straight_line(self, client, server_proc, profile): """ Verify TCP follows a straight line during Cartesian moves. diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index 02238eb..a7d33f6 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -204,9 +204,8 @@ def test_trajectory_respects_joint_velocity_limits( for i in range(len(traj.steps)): steps_to_rad(traj.steps[i], trajectory_rad[i]) - # Compute velocities via finite difference - dt = traj.duration / (len(traj) - 1) - velocities_rad = np.diff(trajectory_rad, axis=0) / dt + # Rows are one control tick apart + velocities_rad = np.diff(trajectory_rad, axis=0) / INTERVAL_S # Check against velocity limits (with small tolerance for numerical error) v_max_rad = LIMITS.joint.hard.velocity @@ -218,47 +217,45 @@ def test_trajectory_respects_joint_velocity_limits( ) def test_septic_holds_its_limits_and_starts_and_stops_without_a_jerk_step( - self, simple_joint_path + self, ): """SEPTIC stays inside the velocity, acceleration and jerk limits, and - its jerk ramps in and out where QUINTIC's steps straight to its peak.""" - - def derivatives(profile): - traj = TrajectoryBuilder( - joint_path=simple_joint_path, profile=profile, dt=INTERVAL_S + its jerk ramps in and out where QUINTIC's steps straight to its peak. + The move is short enough that the jerk limit is what sets its + duration.""" + start = np.array([0.0, -1.57, 3.14, 0.0, 0.0, 0.0]) + path = JointPath.interpolate(start, start + [0, 0.05, 0, 0, 0, 0], 50) + + def j2_derivatives(profile): + q = TrajectoryBuilder( + joint_path=path, profile=profile, dt=INTERVAL_S ).build() - q = traj.positions_rad - dt = INTERVAL_S # At rest before and after, so the differences see both ends. - q = np.vstack([q[:1], q[:1], q, q[-1:], q[-1:]]) - v = np.diff(q, axis=0) / dt - a = np.diff(v, axis=0) / dt - return v, a, np.diff(a, axis=0) / dt + q = np.pad(q.positions_rad[:, 1], 2, mode="edge") + return [np.diff(q, n=k) / INTERVAL_S**k for k in (1, 2, 3)] - v, a, j = derivatives(ProfileType.SEPTIC) hard = LIMITS.joint.hard + v, a, j = j2_derivatives(ProfileType.SEPTIC) for name, x, limit in [ - ("velocity", v, hard.velocity), - ("acceleration", a, hard.acceleration), - ("jerk", j, hard.jerk), + ("velocity", v, hard.velocity[1]), + ("acceleration", a, hard.acceleration[1]), + ("jerk", j, hard.jerk[1]), ]: - ratio = float(np.max(np.abs(x) / limit)) + ratio = float(np.max(np.abs(x)) / limit) assert ratio <= 1.0, f"SEPTIC {name} at {ratio:.3f}x its limit" - moving = np.abs( - simple_joint_path.positions[-1] - simple_joint_path.positions[0] - ) - joint = int(np.argmax(moving)) - peak = np.max(np.abs(j[:, joint])) - for at, edge in [("start", j[:2, joint]), ("end", j[-2:, joint])]: - assert np.max(np.abs(edge)) < 0.1 * peak, ( - f"SEPTIC jerk at the {at} is {np.max(np.abs(edge)):.3f} " - f"against a peak of {peak:.3f}" + # Jerk on the first and last samples against its peak: the septic's + # ramps from zero, the quintic's steps straight to its peak. + for profile, ramps in [ + (ProfileType.SEPTIC, True), + (ProfileType.QUINTIC, False), + ]: + j = j2_derivatives(profile)[2] + edge = max(np.max(np.abs(j[:2])), np.max(np.abs(j[-2:]))) + assert (edge < 0.5 * np.max(np.abs(j))) == ramps, ( + f"{profile.name} jerk at its ends is {edge:.3f} " + f"against a peak of {np.max(np.abs(j)):.3f}" ) - # The control: the quintic's jerk is at its peak on the first sample, - # so the assertion above discriminates rather than merely passes. - _, _, jq = derivatives(ProfileType.QUINTIC) - assert np.max(np.abs(jq[:2, joint])) > 0.5 * np.max(np.abs(jq[:, joint])) class TestTrajectory: From e35b2cec852d0f223cca67f81845738c662c2e22 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sun, 11 Oct 2026 22:33:32 +0000 Subject: [PATCH 3/7] Hold every joint to its acceleration limit as the rows are played Played one row per tick, a bend between rows is a change of joint velocity within a tick, however smoothly a profile times the path coordinate. _enforce_segment_limits sized its acceleration stretch from the velocity-limited segment time, usually far under a tick, so it seldom stretched anything, and TOPP-RA's rows were never checked: a 40 mm MoveL from standby ran J4 at 2.6-4.2x the hard acceleration limit and a blended joint chain its corner at 4-5x, under every path-following profile. Each segment between rows now gets the shortest time, never under a tick, that keeps every joint under its velocity limit and its velocity change between neighbouring segments within a tick of its acceleration limit: a forward and a backward pass over the rows, repeated until neither lowers a rate. TOPP-RA's rows go through it too. A wrist turn ahead of a cartesian path keeps the rest row it ends on, so a joint the path drives back the way the turn came does not reverse within a tick. The async client's robot setter takes any waldoctl Robot, as the interface declares; ty 0.0.86 refuses the narrower one. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_016uVshJDKTz7nCqrMGVxVJU --- parol6/client/async_client.py | 10 ++- parol6/motion/trajectory.py | 151 ++++++++++++++++++++++++---------- tests/unit/test_motion.py | 72 +++++++++++++++- 3 files changed, 186 insertions(+), 47 deletions(-) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 18cce13..f551573 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -242,6 +242,8 @@ def _create_unicast_socket(port: int, host: str) -> socket.socket: if TYPE_CHECKING: from typing import Protocol + from waldoctl import Robot as WaldoRobot + from parol6.robot import Robot class _StatusNotifier(Protocol): @@ -320,7 +322,13 @@ def robot(self) -> "Robot": return self._robot @robot.setter - def robot(self, value: "Robot | None") -> None: + def robot(self, value: "WaldoRobot | None") -> None: + # The interface takes any backend's robot; this client drives only + # its own. + if value is not None: + from parol6.robot import Robot + + assert isinstance(value, Robot) self._robot = value def __init__( diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index fd1774b..d27bf3d 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -19,6 +19,7 @@ from enum import Enum import numpy as np +from numba import njit from numpy.typing import NDArray from ruckig import InputParameter, OutputParameter, Result, Ruckig # type: ignore[unresolved-import, ty:unresolved-import] @@ -119,6 +120,75 @@ def _tick_intervals(duration: float, dt: float) -> int: return max(1, math.ceil(duration / dt - _TICK_ROUNDING)) +# Relative slack on a segment rate: below it a rate is taken as settled. +_RATE_TOLERANCE = 1e-9 + + +@njit(cache=True) +def _segment_rates( + deltas: NDArray[np.float64], + v_max: NDArray[np.float64], + dv_max: NDArray[np.float64], + rate_max: float, +) -> NDArray[np.float64]: + """The fastest rate (one over its time) each segment between rows can + be run at: no faster than ``rate_max``, every joint under ``v_max``, + and every joint's velocity changing by at most ``dv_max`` from one + segment to the next, from rest into the first and into rest out of the + last. + + A joint that its two neighbouring segments move in opposite directions + turns at the row between them, and each side takes half the change. + Every other bound caps a segment's rate by its own limits or by a + rising function of a neighbour's rate, so lowering the rates until no + bound is broken reaches the fastest timing that breaks none: a forward + pass for a joint speeding up into a segment, a backward pass for one + slowing into the next, repeated while either still lowers a rate. + """ + n, dof = deltas.shape + rates = np.full(n, rate_max) + for i in range(n): + for j in range(dof): + d = abs(deltas[i, j]) + if d > 0.0: + rates[i] = min(rates[i], v_max[j] / d) + if i == 0 or i == n - 1: + rates[i] = min(rates[i], dv_max[j] / d) + for i in range(n - 1): + for j in range(dof): + a = deltas[i, j] + b = deltas[i + 1, j] + if a * b < 0.0: + rates[i] = min(rates[i], 0.5 * dv_max[j] / abs(a)) + rates[i + 1] = min(rates[i + 1], 0.5 * dv_max[j] / abs(b)) + elif a == 0.0 and b != 0.0: + rates[i + 1] = min(rates[i + 1], dv_max[j] / abs(b)) + elif b == 0.0 and a != 0.0: + rates[i] = min(rates[i], dv_max[j] / abs(a)) + lowered = True + while lowered: + lowered = False + for i in range(n - 1): + for j in range(dof): + a = deltas[i, j] + b = deltas[i + 1, j] + if a * b > 0.0: + cap = (abs(a) * rates[i] + dv_max[j]) / abs(b) + if cap < rates[i + 1] * (1.0 - _RATE_TOLERANCE): + rates[i + 1] = cap + lowered = True + for i in range(n - 2, -1, -1): + for j in range(dof): + a = deltas[i, j] + b = deltas[i + 1, j] + if a * b > 0.0: + cap = (abs(b) * rates[i + 1] + dv_max[j]) / abs(a) + if cap < rates[i] * (1.0 - _RATE_TOLERANCE): + rates[i] = cap + lowered = True + return rates + + class _LinearPath: """Piecewise linear path wrapper for TOPPRA compatibility. @@ -838,7 +908,7 @@ def _build_with_prefix(self) -> Trajectory: # turn left none, as any request shorter than the move can be does. path_duration = None if self.duration: - remaining = self.duration - (len(turn) - 1) * self.dt + remaining = self.duration - len(turn) * self.dt path_duration = remaining if remaining > 0 else None path = TrajectoryBuilder( joint_path=JointPath(positions=self.joint_path.positions[p:]), @@ -854,7 +924,11 @@ def _build_with_prefix(self) -> Trajectory: constant_tool_speed=self.constant_tool_speed, ).build() return self._trajectory( - np.concatenate([turn.positions_rad, path.positions_rad[1:]]) + # The turn's last row and the path's first are the same pose; + # played twice, the wrist is at rest for the tick between, and a + # joint the path drives back the way the turn came does not + # reverse within one. + np.concatenate([turn.positions_rad, path.positions_rad]) ) def _build_toppra_trajectory(self) -> Trajectory: @@ -953,7 +1027,10 @@ def _build_toppra_trajectory(self) -> Trajectory: trajectory_rad = np.empty((intervals + 1, 6), dtype=np.float64) trajectory_rad[:-1] = jnt_traj(times) trajectory_rad[-1] = jnt_traj(duration) - duration = intervals * self.dt + # TOPP-RA bounds acceleration along a path it takes as straight + # between rows; the bends between them are the rows' own. + trajectory_rad = self._enforce_segment_limits(trajectory_rad) + duration = len(trajectory_rad) * self.dt logger.debug( "TrajectoryBuilder: output_samples=%d, duration=%.3f", @@ -1059,11 +1136,20 @@ def _enforce_segment_limits( self, trajectory_rad: NDArray[np.float64] ) -> NDArray[np.float64]: """ - Enforce velocity limits by locally stretching segments that exceed limits. - - Walks through each segment and checks if joint velocities exceed limits. - Where they do, stretches that segment's time. This handles singularities - and wrist flips by slowing only where necessary, not globally. + Slow the rows down wherever a joint would pass its velocity or + acceleration limit as the player plays them, one row per tick. + + A profile times the path coordinate, but the rows follow the path's + own shape: a bend between rows, a junction in a blended chain or the + step a wrist turn leaves behind is a change of joint velocity + within one tick, however smoothly the coordinate itself is timed. + Each segment between rows is given the shortest time, never less + than a tick, that keeps every joint under its velocity limit and + its velocity change from one segment to the next within a tick of + its acceleration limit (:func:`_segment_rates`). The rows are then + resampled a tick apart along that timing; a stretch of segments + all at least a tick long, sampled a tick apart, changes velocity + no faster than its segments do. Args: trajectory_rad: Joint positions in radians, shape (N, 6), one @@ -1078,51 +1164,26 @@ def _enforce_segment_limits( if n_points < 2: return trajectory_rad - initial_dt = self.dt - duration = (n_points - 1) * initial_dt - - deltas = np.diff(trajectory_rad, axis=0) # (N-1, 6) - - # Minimum time per segment to respect velocity limits: - # max(|delta[j]| / v_max[j]) over joints - min_segment_times = np.max(np.abs(deltas) / self.v_max, axis=1) # (N-1,) - segment_times = np.maximum(min_segment_times, initial_dt) - - # Approximate acceleration check: is the velocity change between - # adjacent segments feasible at the times they take once slowed for - # velocity? Measured at the original spacing instead, a segment - # slowed for velocity counts its excess twice. - if n_points > 2: - velocities = deltas / segment_times[:, np.newaxis] - spans = 0.5 * (segment_times[:-1] + segment_times[1:]) - accel = np.diff(velocities, axis=0) / spans[:, np.newaxis] # (N-2, 6) - accel_times = np.zeros(n_points - 1) - for i in range(len(accel)): - max_accel_ratio = np.max(np.abs(accel[i]) / self.a_max) - if max_accel_ratio > 1.0: - # Spread the extra time across adjacent segments - stretch = np.sqrt(max_accel_ratio) - accel_times[i] = max(accel_times[i], min_segment_times[i] * stretch) - accel_times[i + 1] = max( - accel_times[i + 1], min_segment_times[i + 1] * stretch - ) - segment_times = np.maximum(segment_times, accel_times) - - new_duration = float(np.sum(segment_times)) - if new_duration <= duration * 1.001: # No significant change + rates = _segment_rates( + np.diff(trajectory_rad, axis=0), + self.v_max, + self.a_max * self.dt, + 1.0 / self.dt, + ) + if np.all(rates * self.dt >= 1.0 - _RATE_TOLERANCE): return trajectory_rad - logger.warning( + cumulative_times = np.zeros(n_points) + cumulative_times[1:] = np.cumsum(1.0 / rates) + duration = (n_points - 1) * self.dt + new_duration = float(cumulative_times[-1]) + logger.debug( "Extending duration from %.3fs to %.3fs (%.1f%% increase) to respect velocity/acceleration limits", duration, new_duration, (new_duration / duration - 1) * 100, ) - # Resample at control rate with the new per-segment timing - cumulative_times = np.zeros(n_points) - cumulative_times[1:] = np.cumsum(segment_times) - intervals = _tick_intervals(new_duration, self.dt) output_times = np.arange(intervals + 1) * (new_duration / intervals) diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index 01abdce..750733d 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -3,8 +3,10 @@ import numpy as np import pytest -from parol6.config import INTERVAL_S, LIMITS, steps_to_rad +import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6.config import INTERVAL_S, LIMITS, PATH_SAMPLES, steps_to_rad from parol6.motion import JointPath, ProfileType, Trajectory, TrajectoryBuilder +from parol6.motion.geometry import LineSegment from parol6.motion.trajectory import ( IK_MAX_JOINT_STEP_RAD, _ik_branch_hop, @@ -216,6 +218,74 @@ def test_trajectory_respects_joint_velocity_limits( f"{(max_vel_ratio - 1) * 100:.1f}%" ) + @pytest.mark.parametrize( + "profile", + [ + ProfileType.TOPPRA, + ProfileType.LINEAR, + ProfileType.QUINTIC, + ProfileType.TRAPEZOID, + ], + ) + def test_a_path_that_bends_is_played_within_the_acceleration_limit(self, profile): + """Played one row per tick, a bend between rows is a change of joint + velocity within a tick, however smoothly the profile times the path + coordinate: the corner where a blended chain turns from J1 to J2, + and the J4 step a cartesian move from standby leaves after turning + the wrist off its singularity. Each is slowed until no joint + accelerates past the fraction of its limit the move asks for.""" + start = np.array([0.0, -1.57, 3.14, 0.0, 0.5, 0.0]) + corner = start + [0.3, 0, 0, 0, 0, 0] + chain = JointPath( + positions=np.vstack( + [ + JointPath.interpolate(start, corner, 50).positions, + JointPath.interpolate( + corner, corner + [0, 0.3, 0, 0, 0, 0], 50 + ).positions[1:], + ] + ) + ) + standby = np.deg2rad(PAROL6_ROBOT.joint.standby_deg) + here = np.asarray(PAROL6_ROBOT.robot.fkine(standby)).copy() + there = here.copy() + there[:3, 3] += [0.04, 0.0, 0.04] + poses = np.empty((PATH_SAMPLES, 4, 4)) + LineSegment(here, there).sample_into(poses, 0.0, 1.0, 0) + line = JointPath.from_poses(poses, standby) + frac = 0.5 + cartesian = { + "cart_vel_limit": LIMITS.cart.hard.velocity.linear * frac, + "cart_acc_limit": LIMITS.cart.hard.acceleration.linear * frac, + "path_knots": line.knots, + } + + for name, path, extra in [ + ("blended corner", chain, {}), + ("MoveL from standby", line, cartesian), + ]: + rows = ( + TrajectoryBuilder( + joint_path=path, + profile=profile, + velocity_frac=frac, + accel_frac=frac, + dt=INTERVAL_S, + **extra, + ) + .build() + .positions_rad + ) + rows = np.pad(rows, ((1, 1), (0, 0)), mode="edge") + accel = np.diff(rows, n=2, axis=0) / INTERVAL_S**2 + ratio = float( + np.max(np.abs(accel) / (LIMITS.joint.hard.acceleration * frac)) + ) + assert ratio <= 1.0, ( + f"{profile.name}, {name}: a joint at {ratio:.2f}x the " + "acceleration asked for" + ) + class TestTrajectory: """Tests for Trajectory dataclass.""" From 624e1784748586aa85827ad0a0c30e1229a88e82 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Mon, 12 Oct 2026 15:18:44 +0000 Subject: [PATCH 4/7] Give TOPP-RA the path's bends, as MoveIt's TOTG does A piecewise-linear path tells TOPP-RA its second derivative is zero, so it timed every bend as a straight run, and slowing the sampled rows afterwards could only slow them: a sharp joint-space corner took 4.26 s against 1.44 s for its two legs run apart. As TOTG blends its waypoints, each corner is now a parabolic blend in the path's own parameter, no further from the row than half the finest motor step, so the solver sees the curvature and times it: the corner takes 1.42 s within the limits. - Gridpoints are toppra's proposal joined with the rows, every blend's ends and the midpoints between, and the solution is sampled with ParametrizeConstAccel. - A requested duration stretches the time-optimal solution evenly instead of TOPPRAsd, whose fixed speed margin clamped the narrow blends to a stop. - The row pass stays as a safeguard; it now fires on 1 of 14 measured moves, not 10. - A process move is slowed evenly, keeping its one tool speed, and a requested duration the bends overran is met by the fastest timing slowed evenly when it fits. Co-Authored-By: Claude Opus 5.5 Co-Authored-By: Codex Claude-Session: https://claude.ai/code/session_016uVshJDKTz7nCqrMGVxVJU --- parol6/motion/trajectory.py | 254 +++++++++++++++++++++++++----------- tests/unit/test_motion.py | 26 ++++ 2 files changed, 202 insertions(+), 78 deletions(-) diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index d27bf3d..cdc2a24 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -23,15 +23,12 @@ from numpy.typing import NDArray from ruckig import InputParameter, OutputParameter, Result, Ruckig # type: ignore[unresolved-import, ty:unresolved-import] -from scipy.interpolate import PPoly - -import toppra as ta import toppra.algorithm as algo import toppra.constraint as constraint -from toppra.interpolator import SplineInterpolator +from toppra.interpolator import propose_gridpoints import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from parol6.config import INTERVAL_S, LIMITS, rad_to_steps +from parol6.config import INTERVAL_S, LIMITS, rad_to_steps, steps_to_rad_scalar from parol6.motion.geometry import _rotation_angle, cartesian_path_knots from parol6.utils.error_catalog import make_error from parol6.utils.error_codes import ErrorCode @@ -124,6 +121,16 @@ def _tick_intervals(duration: float, dt: float) -> int: _RATE_TOLERANCE = 1e-9 +def _slowed(rows: NDArray[np.float64], count: int) -> NDArray[np.float64]: + """``rows``, a tick apart, slowed evenly onto ``count`` rows a tick + apart: every velocity, and every change of one, shrinks by the same + factor.""" + times = np.linspace(0.0, len(rows) - 1, count) + return np.column_stack( + [np.interp(times, np.arange(len(rows)), rows[:, j]) for j in range(6)] + ) + + @njit(cache=True) def _segment_rates( deltas: NDArray[np.float64], @@ -189,31 +196,83 @@ def _segment_rates( return rates -class _LinearPath: - """Piecewise linear path wrapper for TOPPRA compatibility. +# Half the finest joint's motor step bounds every blend below the arm's +# command resolution, also used by _within_a_step to recognize no motion. +_BLEND_MAX_DEV_RAD = 0.5 * min(float(steps_to_rad_scalar(1, j)) for j in range(6)) - Wraps a scipy PPoly (degree-1) to satisfy the toppra path interface: - __call__(s, order), .dof, .path_interval. Linear segments prevent - overshoot between waypoints — critical near wrist singularities where - cubic spline bulge amplifies orientation error. - """ - __slots__ = ("_pp", "dof", "path_interval") +class _BlendedPath: + """C1 parabolic corner blends in the original waypoint parameter. - def __init__(self, ppoly: PPoly, dof: int) -> None: - self._pp = ppoly - self.dof = dof - self.path_interval = [float(ppoly.x[0]), float(ppoly.x[-1])] + The largest departure from the original path is h*|delta_slope|/4 at the corner. + Half-interval widths keep neighbouring blends from overlapping, while + retaining the parameter needed by the Cartesian speed constraints. + """ + + def __init__( + self, knots: NDArray[np.float64], positions: NDArray[np.float64] + ) -> None: + self._knots = knots + self._positions = positions + self.dof = positions.shape[1] + self.path_interval = (float(knots[0]), float(knots[-1])) + intervals = np.diff(knots) + self.slopes = np.diff(positions, axis=0) / intervals[:, None] + delta = np.diff(self.slopes, axis=0) + magnitude = np.max(np.abs(delta), axis=1) + # Sub-nanoradian slope changes are roundoff, far below a motor step. + corners = np.flatnonzero(magnitude > 1e-9) + half_width = np.minimum( + 0.5 * np.minimum(intervals[:-1], intervals[1:])[corners], + 4.0 * _BLEND_MAX_DEV_RAD / magnitude[corners], + ) + self.blend_starts = knots[corners + 1] - half_width + self.blend_ends = knots[corners + 1] + half_width + self._blend_slopes = self.slopes[corners] + self._blend_positions = ( + positions[corners + 1] - self._blend_slopes * half_width[:, None] + ) + self._curvature = delta[corners] / (2.0 * half_width[:, None]) def __call__(self, s_in: float | NDArray, order: int = 0) -> NDArray[np.float64]: - scalar = np.isscalar(s_in) - s = np.atleast_1d(np.asarray(s_in, dtype=float)) - if order <= 1: - result = self._pp(s, order) + s = np.asarray(s_in, dtype=np.float64) + flat = s.reshape(-1) + segments = np.clip( + np.searchsorted(self._knots, flat, side="right") - 1, + 0, + len(self.slopes) - 1, + ) + if order == 0: + result = ( + self._positions[segments] + + self.slopes[segments] * (flat - self._knots[segments])[:, None] + ) + elif order == 1: + result = self.slopes[segments].copy() + elif order == 2: + result = np.zeros((len(flat), self.dof)) else: - # Second+ derivative of a piecewise-linear path is zero - result = np.zeros((len(s), self.dof)) - return result[0] if scalar and result.ndim > 1 else result + raise ValueError(f"Unsupported path derivative order: {order}") + + if len(self.blend_starts): + blends = np.maximum( + np.searchsorted(self.blend_starts, flat, side="right") - 1, 0 + ) + inside = (flat >= self.blend_starts[blends]) & ( + flat <= self.blend_ends[blends] + ) + indices = blends[inside] + offset = (flat[inside] - self.blend_starts[indices])[:, None] + curvature = self._curvature[indices] + if order == 0: + result[inside] = self._blend_positions[indices] + offset * ( + self._blend_slopes[indices] + 0.5 * offset * curvature + ) + elif order == 1: + result[inside] = self._blend_slopes[indices] + offset * curvature + else: + result[inside] = curvature + return result.reshape(s.shape + (self.dof,)) class ProfileType(Enum): @@ -797,6 +856,28 @@ def build(self) -> Trajectory: Returns: Trajectory ready for execution, its rows one control tick apart """ + trajectory = self._build() + if not self.duration: + return trajectory + asked = _tick_intervals(self.duration, self.dt) + 1 + if len(trajectory) <= asked: + return trajectory + # Slowed at its bends, the move overran a duration a fastest timing + # may still fit: bent rows slowed evenly change velocity no faster. + requested, self.duration = self.duration, None + fastest = self._build().positions_rad + self.duration = requested + if len(fastest) > asked: + logger.warning( + "Extending duration from %.3fs to %.3fs to respect the joint limits", + requested, + len(fastest) * self.dt, + ) + return self._trajectory(fastest) + return self._trajectory(_slowed(fastest, asked)) + + def _build(self) -> Trajectory: + """The trajectory as the selected profile times it.""" positions = self.joint_path.positions # A path that goes nowhere is done where it stands: there is no # distance for a timing solver to spread over, and every profile @@ -935,9 +1016,8 @@ def _build_toppra_trajectory(self) -> Trajectory: """ Build trajectory using TOPP-RA's time-optimal path parameterization. - Uses piecewise linear interpolation through waypoints (no overshoot) - and computes time-optimal velocity profile respecting joint limits - and optional Cartesian velocity limits. + Bounded corner blends expose curvature to the timing solver while + preserving the waypoint parameter and Cartesian velocity limits. """ positions = self.joint_path.positions if self.path_knots is not None: @@ -957,16 +1037,7 @@ def _build_toppra_trajectory(self) -> Trajectory: ss_waypoints = np.linspace(0.0, 1.0, len(positions)) n_points = len(positions) - # Piecewise linear PPoly — prevents cubic spline overshoot that - # amplifies orientation error near wrist singularities - n_seg = n_points - 1 - dof = positions.shape[1] - c = np.zeros((2, n_seg, dof)) - for i in range(n_seg): - dx = ss_waypoints[i + 1] - ss_waypoints[i] - c[0, i, :] = (positions[i + 1] - positions[i]) / dx - c[1, i, :] = positions[i] - path = _LinearPath(PPoly(c, ss_waypoints), dof) + path = _BlendedPath(ss_waypoints, positions) joint_vel_constraint = constraint.JointVelocityConstraint(self._vlim) joint_acc_constraint = constraint.JointAccelerationConstraint(self._alim) @@ -980,39 +1051,55 @@ def _build_toppra_trajectory(self) -> Trajectory: if cart_constraint is not None: constraints.append(cart_constraint) if self.constant_tool_speed: - constraints.append(self._build_path_speed_cap(path, positions, c[0])) + constraints.append(self._build_path_speed_cap(path, positions, path.slopes)) try: - # Use evenly-spaced gridpoints - TOPPRA docs recommend "at least a few times - # the number of waypoints". Auto-selection can cluster points near - # discontinuities, causing TOPPRAsd to produce incorrect durations. - n_gridpoints = n_points * 3 - gridpoints = np.linspace(0.0, 1.0, n_gridpoints) - - if self.duration is not None and self.duration > 0: - instance = algo.TOPPRAsd(constraints, path, gridpoints=gridpoints) - instance.set_desired_duration(self.duration) - jnt_traj = instance.compute_trajectory() - if jnt_traj is not None: - duration = self.duration - logger.debug( - "TrajectoryBuilder: TOPPRAsd target_duration=%.3f, path_len=%d", - duration, - n_points, - ) - else: - # Fall back to time-optimal if TOPPRAsd fails - logger.warning("TOPPRAsd failed, trying time-optimal TOPPRA") - instance = algo.TOPPRA(constraints, path, gridpoints=gridpoints) - jnt_traj = instance.compute_trajectory() - else: - instance = algo.TOPPRA(constraints, path, gridpoints=gridpoints) - jnt_traj = instance.compute_trajectory() + # Adaptive samples alone can miss a sub-step blend; its boundaries + # and centre must constrain the solver even on a long straight run. + # Samples one half-width outside keep boundary curvature from + # constraining an entire neighbouring straight segment. + half_widths = 0.5 * (path.blend_ends - path.blend_starts) + gridpoints = np.unique( + np.concatenate( + [ + propose_gridpoints(path), + ss_waypoints, + path.blend_starts, + path.blend_ends, + np.clip(path.blend_starts - half_widths, *path.path_interval), + np.clip(path.blend_ends + half_widths, *path.path_interval), + ] + ) + ) + # A straight span between two slow bends needs an interior speed + # to accelerate and brake; curvature-based sampling can omit it. + gridpoints = np.sort( + np.concatenate([gridpoints, 0.5 * (gridpoints[:-1] + gridpoints[1:])]) + ) + # Shared boundaries can differ by roundoff; the same tolerance + # as repeated input knots avoids singular solver intervals. + gridpoints = gridpoints[ + np.concatenate(([True], np.diff(gridpoints) > 1e-12)) + ] + gridpoints[[0, -1]] = path.path_interval + + # Uniformly stretching a rest-to-rest solution preserves its + # limits. TOPPRAsd's fixed subtraction from squared speed can + # instead stall at narrow blends and yield an infinite duration. + instance = algo.TOPPRA( + constraints, + path, + gridpoints=gridpoints, + parametrizer="ParametrizeConstAccel", + ) + jnt_traj = instance.compute_trajectory() - if not isinstance(jnt_traj, SplineInterpolator): + if jnt_traj is None: raise RuntimeError("TOPP-RA failed to compute trajectory") - duration = float(jnt_traj.duration) + duration = float(jnt_traj.path_interval[1] - jnt_traj.path_interval[0]) + if not np.isfinite(duration) or duration <= 0: + raise RuntimeError("TOPP-RA returned an invalid duration") logger.debug( "TrajectoryBuilder: TOPP-RA duration=%.3f, path_len=%d", @@ -1022,13 +1109,14 @@ def _build_toppra_trajectory(self) -> Trajectory: # One row per tick, the solved trajectory slowed onto a whole # number of ticks and ending exactly at its endpoint. - intervals = _tick_intervals(duration, self.dt) + intervals = _tick_intervals(self._requested_or(duration), self.dt) times = np.arange(intervals) * (duration / intervals) trajectory_rad = np.empty((intervals + 1, 6), dtype=np.float64) trajectory_rad[:-1] = jnt_traj(times) - trajectory_rad[-1] = jnt_traj(duration) - # TOPP-RA bounds acceleration along a path it takes as straight - # between rows; the bends between them are the rows' own. + trajectory_rad[-1] = positions[-1] + if not np.all(np.isfinite(trajectory_rad)): + raise RuntimeError("TOPP-RA returned non-finite joint positions") + # Discretized solver constraints can miss extrema between gridpoints. trajectory_rad = self._enforce_segment_limits(trajectory_rad) duration = len(trajectory_rad) * self.dt @@ -1164,13 +1252,23 @@ def _enforce_segment_limits( if n_points < 2: return trajectory_rad - rates = _segment_rates( - np.diff(trajectory_rad, axis=0), - self.v_max, - self.a_max * self.dt, - 1.0 / self.dt, - ) - if np.all(rates * self.dt >= 1.0 - _RATE_TOLERANCE): + deltas = np.diff(trajectory_rad, axis=0) + if self.constant_tool_speed: + # A process move holds one tool speed: slowed evenly, by the + # most any segment needs, not only where it bends. + at_rest = np.zeros((1, 6)) + turns = np.diff(np.vstack([at_rest, deltas, at_rest]), axis=0) + stretch = max( + 1.0, + float(np.max(np.abs(deltas) / (self.v_max * self.dt))), + float(np.max(np.abs(turns) / (self.a_max * self.dt**2))), + ) + rates = np.full(len(deltas), 1.0 / (stretch * self.dt)) + else: + rates = _segment_rates( + deltas, self.v_max, self.a_max * self.dt, 1.0 / self.dt + ) + if rates.min() * self.dt >= 1.0 - _RATE_TOLERANCE: return trajectory_rad cumulative_times = np.zeros(n_points) @@ -1391,7 +1489,7 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory: return self._trajectory(trajectory_rad) def _build_cart_vel_constraint( - self, path: ta.SplineInterpolator | _LinearPath, ss_waypoints: NDArray + self, path: _BlendedPath, ss_waypoints: NDArray ) -> constraint.JointVelocityConstraintVarying | None: """ Build Cartesian velocity constraint for TOPP-RA using path-tangent method. @@ -1484,7 +1582,7 @@ def vlim_func(s: float) -> NDArray: def _build_path_speed_cap( self, - path: _LinearPath, + path: _BlendedPath, positions: NDArray[np.float64], slopes: NDArray[np.float64], ) -> constraint.Constraint: diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index 750733d..21eb984 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -218,6 +218,32 @@ def test_trajectory_respects_joint_velocity_limits( f"{(max_vel_ratio - 1) * 100:.1f}%" ) + def test_toppra_corner_is_no_slower_than_stopping_between_legs(self): + start = np.array([0.0, -1.57, 3.14, 0.0, 0.5, 0.0]) + corner = start + [0.3, 0, 0, 0, 0, 0] + end = corner + [0, 0.3, 0, 0, 0, 0] + legs = [ + JointPath.interpolate(start, corner, 50), + JointPath.interpolate(corner, end, 50), + ] + chain = JointPath( + positions=np.vstack([legs[0].positions, legs[1].positions[1:]]) + ) + frac = 0.5 + separate_duration = sum( + TrajectoryBuilder(leg, ProfileType.TOPPRA, accel_frac=frac).build().duration + for leg in legs + ) + trajectory = TrajectoryBuilder( + chain, ProfileType.TOPPRA, accel_frac=frac + ).build() + + assert trajectory.duration <= separate_duration + rows = np.pad(trajectory.positions_rad, ((1, 1), (0, 0)), mode="edge") + acceleration = np.diff(rows, n=2, axis=0) / INTERVAL_S**2 + assert np.all(np.abs(acceleration) <= frac * LIMITS.joint.hard.acceleration) + np.testing.assert_array_equal(trajectory.positions_rad[[0, -1]], [start, end]) + @pytest.mark.parametrize( "profile", [ From fce3006562c8121e070c78c5d839093065ef048e Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 13 Oct 2026 15:46:56 +0000 Subject: [PATCH 5/7] Size corner blends by the control tick and time every profile on them Reviews found the half-step blends made TOPP-RA crawl through blended MoveJ chains, left LINEAR/TRAPEZOID/QUINTIC to a post-pass that timed bent paths up to 3.6x slower than feasible, and let process moves dip at every corner. - Blends are sized by the tick the rows are played at, and every along-path profile samples the blended path. - A process move's speed cap includes the bends, so it holds one feasible tool speed under every profile. - A requested duration is applied once in build(), as a uniform stretch of the fastest timing; the post-pass runs once on the final rows, prefix seam included. - Corner blends turn the tool at a continuous rate, so a leg that turns the tool no longer forces a constant-speed move to crawl. - Cartesian constraints are evaluated vectorized over fewer gridpoints: MoveL from standby plans in 34 ms, down from 213 ms. - _segment_rates is compiled at warmup. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_016uVshJDKTz7nCqrMGVxVJU --- parol6/motion/geometry.py | 32 +- parol6/motion/trajectory.py | 741 +++++++++++-------------------- parol6/utils/warmup.py | 2 + tests/unit/test_dry_run_blend.py | 28 +- tests/unit/test_motion.py | 168 ++++--- 5 files changed, 383 insertions(+), 588 deletions(-) diff --git a/parol6/motion/geometry.py b/parol6/motion/geometry.py index ae7717f..5810dee 100644 --- a/parol6/motion/geometry.py +++ b/parol6/motion/geometry.py @@ -317,6 +317,7 @@ def tangent(self, t: float) -> NDArray[np.float64]: def _cubic_blend_into( entry_pose: NDArray[np.float64], + corner_pose: NDArray[np.float64], exit_pose: NDArray[np.float64], p1: NDArray[np.float64], p2: NDArray[np.float64], @@ -326,7 +327,8 @@ def _cubic_blend_into( """Write a cubic Bézier blend zone into a pre-allocated buffer. Position follows the cubic through the control points - (entry, p1, p2, exit); orientation is the geodesic from entry to exit. + (entry, p1, p2, exit); orientation follows the spherical quadratic + through (entry, corner, exit), matching the adjoining legs' rates. """ E = entry_pose[:3, 3] X = exit_pose[:3, 3] @@ -334,7 +336,21 @@ def _cubic_blend_into( n_total = out.shape[0] + skip t = np.linspace(0.0, 1.0, n_total)[skip:] - batch_se3_interp(entry_pose, exit_pose, t, out) + entry, corner, exit_ = Rotation.from_matrix( + np.stack([pose[:3, :3] for pose in (entry_pose, corner_pose, exit_pose)]) + ) + # Matching the position's 2r tangents avoids angular-rate jumps at the joins. + entering = entry * Rotation.from_rotvec( + np.outer(t, (entry.inv() * corner).as_rotvec()) + ) + leaving = corner * Rotation.from_rotvec( + np.outer(t, (corner.inv() * exit_).as_rotvec()) + ) + out[:, :3, :3] = ( + entering + * Rotation.from_rotvec(t[:, None] * (entering.inv() * leaving).as_rotvec()) + ).as_matrix() + out[:, 3, :] = [0.0, 0.0, 0.0, 1.0] omt = 1.0 - t out[:, :3, 3] = ( @@ -466,7 +482,9 @@ def build_blended_path( segments[seg_idx + 1].sample(t_out, exit_buf) counts.append( _intervals( - 2.0 * clamped[seg_idx], _rotation_angle(entry_buf, exit_buf) + 2.0 * clamped[seg_idx], + _rotation_angle(entry_buf, seg.end) + + _rotation_angle(seg.end, exit_buf), ) ) if samples_per_segment is None: @@ -488,7 +506,13 @@ def build_blended_path( p1 = entry_buf[:3, 3] + handle_m * seg.tangent(t_a) p2 = exit_buf[:3, 3] - handle_m * nxt.tangent(t_b) _cubic_blend_into( - entry_buf, exit_buf, p1, p2, out[row : row + n_write], skip=skip + entry_buf, + seg.end, + exit_buf, + p1, + p2, + out[row : row + n_write], + skip=skip, ) row += n_write diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index cdc2a24..3b35326 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -15,25 +15,27 @@ import logging import math +from collections.abc import Callable from dataclasses import dataclass from enum import Enum import numpy as np from numba import njit from numpy.typing import NDArray +from scipy.interpolate import PPoly from ruckig import InputParameter, OutputParameter, Result, Ruckig # type: ignore[unresolved-import, ty:unresolved-import] import toppra.algorithm as algo import toppra.constraint as constraint -from toppra.interpolator import propose_gridpoints +from toppra.parametrizer import ParametrizeConstAccel import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from parol6.config import INTERVAL_S, LIMITS, rad_to_steps, steps_to_rad_scalar +from parol6.config import INTERVAL_S, LIMITS, rad_to_steps from parol6.motion.geometry import _rotation_angle, cartesian_path_knots from parol6.utils.error_catalog import make_error from parol6.utils.error_codes import ErrorCode from parol6.utils.errors import IKError, TrajectoryPlanningError -from parol6.utils.joint_limits import TRAVEL_MAX_RAD, TRAVEL_MIN_RAD +from parol6.utils.joint_limits import HALF_STEP_RAD, TRAVEL_MAX_RAD, TRAVEL_MIN_RAD from pinokin import Damping, IKSolver, se3_interp @@ -122,13 +124,21 @@ def _tick_intervals(duration: float, dt: float) -> int: def _slowed(rows: NDArray[np.float64], count: int) -> NDArray[np.float64]: - """``rows``, a tick apart, slowed evenly onto ``count`` rows a tick - apart: every velocity, and every change of one, shrinks by the same - factor.""" - times = np.linspace(0.0, len(rows) - 1, count) - return np.column_stack( - [np.interp(times, np.arange(len(rows)), rows[:, j]) for j in range(6)] - ) + """Uniformly stretch tick-spaced rows without increasing their played limits.""" + return JointPath(positions=rows).sample_many(np.linspace(0.0, 1.0, count)) + + +def _distinct(values: NDArray[np.float64]) -> NDArray[np.bool_]: + # Seidel treats intervals below 1e-10 as singular. + return np.concatenate(([True], np.diff(values) > 1e-9)) + + +def _resample( + rows: NDArray[np.float64], knots: NDArray[np.float64], at: NDArray[np.float64] +) -> NDArray[np.float64]: + indices = np.clip(np.searchsorted(knots, at, side="right") - 1, 0, len(knots) - 2) + fraction = ((at - knots[indices]) / (knots[indices + 1] - knots[indices]))[:, None] + return rows[indices] + fraction * (rows[indices + 1] - rows[indices]) @njit(cache=True) @@ -152,6 +162,9 @@ def _segment_rates( pass for a joint speeding up into a segment, a backward pass for one slowing into the next, repeated while either still lowers a rate. """ + padded = np.zeros((len(deltas) + 2, deltas.shape[1])) + padded[1:-1] = deltas + deltas = padded n, dof = deltas.shape rates = np.full(n, rate_max) for i in range(n): @@ -159,8 +172,6 @@ def _segment_rates( d = abs(deltas[i, j]) if d > 0.0: rates[i] = min(rates[i], v_max[j] / d) - if i == 0 or i == n - 1: - rates[i] = min(rates[i], dv_max[j] / d) for i in range(n - 1): for j in range(dof): a = deltas[i, j] @@ -193,86 +204,85 @@ def _segment_rates( if cap < rates[i] * (1.0 - _RATE_TOLERANCE): rates[i] = cap lowered = True - return rates - - -# Half the finest joint's motor step bounds every blend below the arm's -# command resolution, also used by _within_a_step to recognize no motion. -_BLEND_MAX_DEV_RAD = 0.5 * min(float(steps_to_rad_scalar(1, j)) for j in range(6)) + return rates[1:-1] class _BlendedPath: - """C1 parabolic corner blends in the original waypoint parameter. - - The largest departure from the original path is h*|delta_slope|/4 at the corner. - Half-interval widths keep neighbouring blends from overlapping, while - retaining the parameter needed by the Cartesian speed constraints. - """ + """C1 parabolic blends bounded by motor resolution and tick-sized corner cutting.""" def __init__( - self, knots: NDArray[np.float64], positions: NDArray[np.float64] + self, + knots: NDArray[np.float64], + positions: NDArray[np.float64], + a_max: NDArray[np.float64], + dt: float, ) -> None: - self._knots = knots - self._positions = positions self.dof = positions.shape[1] self.path_interval = (float(knots[0]), float(knots[-1])) intervals = np.diff(knots) self.slopes = np.diff(positions, axis=0) / intervals[:, None] delta = np.diff(self.slopes, axis=0) magnitude = np.max(np.abs(delta), axis=1) - # Sub-nanoradian slope changes are roundoff, far below a motor step. corners = np.flatnonzero(magnitude > 1e-9) + with np.errstate(divide="ignore"): + # Playback itself cuts corners by up to a*dt²/4. + tick_width = dt**2 * np.min(a_max / np.abs(delta[corners]), axis=1) half_width = np.minimum( 0.5 * np.minimum(intervals[:-1], intervals[1:])[corners], - 4.0 * _BLEND_MAX_DEV_RAD / magnitude[corners], - ) - self.blend_starts = knots[corners + 1] - half_width - self.blend_ends = knots[corners + 1] + half_width - self._blend_slopes = self.slopes[corners] - self._blend_positions = ( - positions[corners + 1] - self._blend_slopes * half_width[:, None] + np.maximum( + 4.0 * float(np.min(HALF_STEP_RAD)) / magnitude[corners], tick_width + ), ) - self._curvature = delta[corners] / (2.0 * half_width[:, None]) - - def __call__(self, s_in: float | NDArray, order: int = 0) -> NDArray[np.float64]: - s = np.asarray(s_in, dtype=np.float64) - flat = s.reshape(-1) + starts = knots[corners + 1] - half_width + ends = knots[corners + 1] + half_width + unblended = np.delete(knots, corners + 1) + breaks = np.unique(np.concatenate([unblended, starts, ends])) + at = breaks[:-1] segments = np.clip( - np.searchsorted(self._knots, flat, side="right") - 1, - 0, - len(self.slopes) - 1, + np.searchsorted(knots, at, side="right") - 1, 0, len(self.slopes) - 1 ) - if order == 0: - result = ( - self._positions[segments] - + self.slopes[segments] * (flat - self._knots[segments])[:, None] - ) - elif order == 1: - result = self.slopes[segments].copy() - elif order == 2: - result = np.zeros((len(flat), self.dof)) - else: - raise ValueError(f"Unsupported path derivative order: {order}") - - if len(self.blend_starts): - blends = np.maximum( - np.searchsorted(self.blend_starts, flat, side="right") - 1, 0 + coefficients = np.zeros((3, len(at), self.dof)) + coefficients[1] = self.slopes[segments] + coefficients[2] = ( + positions[segments] + + self.slopes[segments] * (at - knots[segments])[:, None] + ) + if len(corners): + blends = np.maximum(np.searchsorted(starts, at, side="right") - 1, 0) + inside = (at >= starts[blends]) & (at < ends[blends]) + selected = blends[inside] + curvature = delta[corners[selected]] / (2.0 * half_width[selected, None]) + offset = (at[inside] - starts[selected])[:, None] + slope = self.slopes[corners[selected]] + position = ( + positions[corners[selected] + 1] - slope * half_width[selected, None] ) - inside = (flat >= self.blend_starts[blends]) & ( - flat <= self.blend_ends[blends] + coefficients[0, inside] = 0.5 * curvature + coefficients[1, inside] = slope + offset * curvature + coefficients[2, inside] = position + offset * ( + slope + 0.5 * offset * curvature ) - indices = blends[inside] - offset = (flat[inside] - self.blend_starts[indices])[:, None] - curvature = self._curvature[indices] - if order == 0: - result[inside] = self._blend_positions[indices] + offset * ( - self._blend_slopes[indices] + 0.5 * offset * curvature - ) - elif order == 1: - result[inside] = self._blend_slopes[indices] + offset * curvature - else: - result[inside] = curvature - return result.reshape(s.shape + (self.dof,)) + self._pp = PPoly(coefficients, breaks) + + @property + def breaks(self) -> NDArray[np.float64]: + return self._pp.x + + def __call__(self, s: float | NDArray, order: int = 0) -> NDArray[np.float64]: + return self._pp(s, order) + + +class _PathSpeedConstraint(constraint.LinearConstraint): + """A scalar path-speed ceiling expressed directly as a bound on squared speed.""" + + def __init__(self, caps: float | NDArray[np.float64]) -> None: + super().__init__() + self.caps = caps + + def compute_constraint_params(self, path: _BlendedPath, gridpoints: NDArray): + bounds = np.zeros((len(gridpoints), 2)) + bounds[:, 1] = np.square(self.caps) + return None, None, None, None, None, None, bounds class ProfileType(Enum): @@ -763,8 +773,8 @@ class TrajectoryBuilder: """ Converts JointPath to executable Trajectory. - Uses TOPP-RA to compute maximum allowable path speed, then applies - the selected motion profile (clamped to TOPP-RA limits). + Along-path profiles use TOPP-RA to time a blended path within the + selected profile's speed envelope and the joint limits. All limits come from PAROL6_ROBOT config - no hardcoded fallbacks. """ @@ -792,7 +802,7 @@ def __init__( velocity_frac: Scale joint velocity limits (0.0-1.0), default 1.0 accel_frac: Scale joint acceleration limits (0.0-1.0), default 1.0 jerk_frac: Scale jerk limits (0.0-1.0), default 1.0 - duration: Override duration (stretches profile if longer than TOPP-RA min) + duration: Uniformly stretch the final rows when longer than their fastest timing dt: Control loop time step cart_vel_limit: Cartesian linear velocity limit in m/s (for Cartesian commands) cart_acc_limit: Cartesian linear acceleration limit in m/s² (for Cartesian commands) @@ -840,41 +850,35 @@ def __init__( self._alim = np.column_stack([-self.a_max, self.a_max]) def build(self) -> Trajectory: - """ - Generate time-parameterized trajectory. - - Uses TOPP-RA to compute time-optimal trajectory, then samples it directly - at the control rate. No interpolation of the original joint path is needed - since TOPP-RA's trajectory already provides smooth, continuous positions. - - RUCKIG runs point to point, from the first row to the last; a path - that is not a straight run between them is timed by TOPP-RA - instead. QUINTIC and TRAPEZOID run each joint on its own profile - from end to end on a straight run, and the path coordinate along - any other path. - - Returns: - Trajectory ready for execution, its rows one control tick apart + """Time the fastest selected profile, enforce played joint limits across + the complete move, then uniformly stretch to a longer requested duration. + RUCKIG keeps its own timing and ignores a requested duration. """ trajectory = self._build() - if not self.duration: - return trajectory - asked = _tick_intervals(self.duration, self.dt) + 1 - if len(trajectory) <= asked: + if trajectory.duration == 0.0: return trajectory - # Slowed at its bends, the move overran a duration a fastest timing - # may still fit: bent rows slowed evenly change velocity no faster. - requested, self.duration = self.duration, None - fastest = self._build().positions_rad - self.duration = requested - if len(fastest) > asked: - logger.warning( - "Extending duration from %.3fs to %.3fs to respect the joint limits", - requested, - len(fastest) * self.dt, + rows = self._enforce_segment_limits(trajectory.positions_rad) + fastest = (len(rows) - 1) * self.dt + if self.duration and self.profile != ProfileType.RUCKIG: + if self.duration < fastest: + logger.warning( + "Extending duration from %.3fs to %.3fs to respect the joint limits", + self.duration, + fastest, + ) + elif self.duration > fastest: + rows = _slowed(rows, _tick_intervals(self.duration, self.dt) + 1) + elif len(rows) > len(trajectory): + duration = (len(trajectory) - 1) * self.dt + level = logging.WARNING if fastest > 1.1 * duration else logging.DEBUG + logger.log( + level, + "Extending duration from %.3fs to %.3fs (%.1f%% increase) to respect velocity/acceleration limits", + duration, + fastest, + (fastest / duration - 1) * 100, ) - return self._trajectory(fastest) - return self._trajectory(_slowed(fastest, asked)) + return self._trajectory(rows) def _build(self) -> Trajectory: """The trajectory as the selected profile times it.""" @@ -894,17 +898,6 @@ def _build(self) -> Trajectory: if self.joint_path.prefix > 0: return self._build_with_prefix() - if self.path_knots is not None and self.profile in ( - ProfileType.LINEAR, - ProfileType.QUINTIC, - ProfileType.TRAPEZOID, - ): - # These profiles time a path coordinate spaced by row; spaced - # by the tool's own distance instead, their cruise is one tool - # speed, as TOPPRA holds it through the knots. - self.joint_path = self._resampled_by_knots(self.path_knots) - self.path_knots = None - along_path = self._is_cartesian_path() or not self._is_point_to_point() if self.profile == ProfileType.RUCKIG and not along_path: return self._build_ruckig_trajectory() @@ -932,18 +925,25 @@ def _trajectory(self, trajectory_rad: NDArray[np.float64]) -> Trajectory: positions_rad=trajectory_rad, ) - def _resampled_by_knots(self, knots: NDArray[np.float64]) -> JointPath: - """The joint path resampled at even steps of the path parameter the - knots give each row (the tool's normalized distance), as many rows - as it had. Repeated knots keep their first row.""" + def _blended_path(self) -> _BlendedPath: positions = self.joint_path.positions - knots = np.asarray(knots, dtype=np.float64) - keep = np.concatenate(([True], np.diff(knots) > 1e-12)) - at = np.linspace(0.0, 1.0, len(positions)) - resampled = np.empty_like(positions) - for j in range(positions.shape[1]): - resampled[:, j] = np.interp(at, knots[keep], positions[keep, j]) - return JointPath(positions=resampled) + if self.path_knots is not None: + knots = np.asarray(self.path_knots, dtype=np.float64) + else: + # Bezier sampling density must not create artificial speed changes. + knots = np.concatenate( + ([0.0], np.cumsum(np.linalg.norm(np.diff(positions, axis=0), axis=1))) + ) + knots /= knots[-1] + keep = _distinct(knots) + if np.count_nonzero(keep) < 2: + raise TrajectoryPlanningError( + make_error( + ErrorCode.TRAJ_NO_STEPS, + detail="the path covers no distance to time", + ) + ) + return _BlendedPath(knots[keep], positions[keep], self.a_max, self.dt) @staticmethod def _within_a_step(positions: NDArray[np.float64]) -> bool: @@ -983,33 +983,21 @@ def _build_with_prefix(self) -> Trajectory: accel_frac=self.accel_frac, jerk_frac=self.jerk_frac, dt=self.dt, - ).build() - # A requested duration is the whole move's: the path gets what the - # turn's motion leaves of it, and runs as fast as it can when the - # turn left none, as any request shorter than the move can be does. - path_duration = None - if self.duration: - remaining = self.duration - len(turn) * self.dt - path_duration = remaining if remaining > 0 else None + )._build() path = TrajectoryBuilder( joint_path=JointPath(positions=self.joint_path.positions[p:]), profile=self.profile, velocity_frac=self.velocity_frac, accel_frac=self.accel_frac, jerk_frac=self.jerk_frac, - duration=path_duration, dt=self.dt, cart_vel_limit=self.cart_vel_limit, cart_acc_limit=self.cart_acc_limit, path_knots=self.path_knots, constant_tool_speed=self.constant_tool_speed, - ).build() + )._build() return self._trajectory( - # The turn's last row and the path's first are the same pose; - # played twice, the wrist is at rest for the tick between, and a - # joint the path drives back the way the turn came does not - # reverse within one. - np.concatenate([turn.positions_rad, path.positions_rad]) + np.concatenate([turn.positions_rad, path.positions_rad[1:]]) ) def _build_toppra_trajectory(self) -> Trajectory: @@ -1019,26 +1007,16 @@ def _build_toppra_trajectory(self) -> Trajectory: Bounded corner blends expose curvature to the timing solver while preserving the waypoint parameter and Cartesian velocity limits. """ - positions = self.joint_path.positions - if self.path_knots is not None: - # Waypoints that cover no distance would give a zero-width - # segment; the path keeps the first of any such run. - keep = np.concatenate(([True], np.diff(self.path_knots) > 1e-12)) - positions = positions[keep] - ss_waypoints = np.asarray(self.path_knots, dtype=np.float64)[keep] - if len(positions) < 2: - raise TrajectoryPlanningError( - make_error( - ErrorCode.TRAJ_NO_STEPS, - detail="the path covers no tool distance to time", - ) - ) - else: - ss_waypoints = np.linspace(0.0, 1.0, len(positions)) - n_points = len(positions) - - path = _BlendedPath(ss_waypoints, positions) + return self._time_path(self._blended_path()) + def _time_path( + self, + path: _BlendedPath, + profile_speed: Callable[[NDArray], NDArray] | None = None, + ) -> Trajectory: + """Time the blended path within the selected profile's speed envelope.""" + positions = self.joint_path.positions + gridpoints = self._gridpoints(path) joint_vel_constraint = constraint.JointVelocityConstraint(self._vlim) joint_acc_constraint = constraint.JointAccelerationConstraint(self._alim) constraints: list[constraint.Constraint] = [ @@ -1046,46 +1024,14 @@ def _build_toppra_trajectory(self) -> Trajectory: joint_acc_constraint, ] - if self.cart_vel_limit is not None and self.cart_vel_limit > 0: - cart_constraint = self._build_cart_vel_constraint(path, ss_waypoints) - if cart_constraint is not None: - constraints.append(cart_constraint) + if self._is_cartesian_path(): + constraints.append(self._build_cart_vel_constraint(path, gridpoints)) if self.constant_tool_speed: - constraints.append(self._build_path_speed_cap(path, positions, path.slopes)) + constraints.append(self._build_path_speed_cap(path)) + if profile_speed is not None: + constraints.append(_PathSpeedConstraint(profile_speed(gridpoints))) try: - # Adaptive samples alone can miss a sub-step blend; its boundaries - # and centre must constrain the solver even on a long straight run. - # Samples one half-width outside keep boundary curvature from - # constraining an entire neighbouring straight segment. - half_widths = 0.5 * (path.blend_ends - path.blend_starts) - gridpoints = np.unique( - np.concatenate( - [ - propose_gridpoints(path), - ss_waypoints, - path.blend_starts, - path.blend_ends, - np.clip(path.blend_starts - half_widths, *path.path_interval), - np.clip(path.blend_ends + half_widths, *path.path_interval), - ] - ) - ) - # A straight span between two slow bends needs an interior speed - # to accelerate and brake; curvature-based sampling can omit it. - gridpoints = np.sort( - np.concatenate([gridpoints, 0.5 * (gridpoints[:-1] + gridpoints[1:])]) - ) - # Shared boundaries can differ by roundoff; the same tolerance - # as repeated input knots avoids singular solver intervals. - gridpoints = gridpoints[ - np.concatenate(([True], np.diff(gridpoints) > 1e-12)) - ] - gridpoints[[0, -1]] = path.path_interval - - # Uniformly stretching a rest-to-rest solution preserves its - # limits. TOPPRAsd's fixed subtraction from squared speed can - # instead stall at narrow blends and yield an infinite duration. instance = algo.TOPPRA( constraints, path, @@ -1097,27 +1043,24 @@ def _build_toppra_trajectory(self) -> Trajectory: if jnt_traj is None: raise RuntimeError("TOPP-RA failed to compute trajectory") - duration = float(jnt_traj.path_interval[1] - jnt_traj.path_interval[0]) + assert isinstance(jnt_traj, ParametrizeConstAccel) + duration = float(jnt_traj.duration) if not np.isfinite(duration) or duration <= 0: raise RuntimeError("TOPP-RA returned an invalid duration") logger.debug( "TrajectoryBuilder: TOPP-RA duration=%.3f, path_len=%d", duration, - n_points, + len(positions), ) - # One row per tick, the solved trajectory slowed onto a whole - # number of ticks and ending exactly at its endpoint. - intervals = _tick_intervals(self._requested_or(duration), self.dt) + intervals = _tick_intervals(duration, self.dt) times = np.arange(intervals) * (duration / intervals) trajectory_rad = np.empty((intervals + 1, 6), dtype=np.float64) trajectory_rad[:-1] = jnt_traj(times) trajectory_rad[-1] = positions[-1] if not np.all(np.isfinite(trajectory_rad)): raise RuntimeError("TOPP-RA returned non-finite joint positions") - # Discretized solver constraints can miss extrema between gridpoints. - trajectory_rad = self._enforce_segment_limits(trajectory_rad) duration = len(trajectory_rad) * self.dt logger.debug( @@ -1136,49 +1079,35 @@ def _build_toppra_trajectory(self) -> Trajectory: ) from e def _build_simple_trajectory(self) -> Trajectory: - """ - Build the LINEAR profile: constant velocity along the path with ramps - at the acceleration limit at either end. - - The path coordinate runs a trapezoid whose cruise is the fastest the - steepest joint allows and whose ramps are at that joint's - acceleration limit, so the profile never steps its velocity. The - limits come from the path's own length, segment by segment, so a - wrist flip or a reconfiguration mid-path costs the time it takes - rather than being averaged away by the endpoint delta. On a - cartesian path the cruise also keeps the tool under its ceiling. - """ + """A trapezoidal path-speed envelope, slowed ahead of bends as needed.""" + path = self._blended_path() vmax_s, amax_s, _ = self._compute_s_profile_limits() - vmax_s = min(vmax_s, self._cruise_cap()) - if not np.isfinite(vmax_s) or not np.isfinite(amax_s): - vmax_s, amax_s = 1.0, 1.0 - - profile_duration = _trapezoid_duration(1.0, vmax_s, amax_s) - duration = max(profile_duration, self.duration or 0.0, self.dt * 2) - intervals = _tick_intervals(duration, self.dt) - duration = intervals * self.dt - times = np.arange(intervals + 1) * self.dt - profile_s = _trapezoid_samples( - times * (profile_duration / duration), 0.0, 1.0, vmax_s, amax_s - ) - trajectory_rad = self.joint_path.sample_many(profile_s) + vmax_s = min(vmax_s, self._cruise_cap(path)) + if not self._is_cartesian_path() and self._is_point_to_point(): + duration = _trapezoid_duration(1.0, vmax_s, amax_s) + intervals = _tick_intervals(duration, self.dt) + profile_s = _trapezoid_samples( + np.linspace(0.0, duration, intervals + 1), 0.0, 1.0, vmax_s, amax_s + ) + return self._trajectory(path(profile_s)) - trajectory_rad = self._enforce_segment_limits(trajectory_rad) + def speed(s: NDArray) -> NDArray: + return np.sqrt(np.minimum(vmax_s**2, 2.0 * amax_s * np.minimum(s, 1.0 - s))) - return self._trajectory(trajectory_rad) + return self._time_path(path, speed) def _is_cartesian_path(self) -> bool: """Check if this is a Cartesian path (has Cartesian velocity limits set).""" return self.cart_vel_limit is not None and self.cart_vel_limit > 0 - def _cruise_cap(self) -> float: + def _cruise_cap(self, path: _BlendedPath) -> float: """The ``ds/dt`` ceiling on a path timed through its path coordinate: on a process move, one tool speed the steepest stretch and the tool ceiling allow; on any other cartesian path, the tool ceiling alone, the joints slowing only where they must; none off one.""" if self.constant_tool_speed: - return self._row_speed_cap() - return self._tool_speed_cap() + return self._path_speed_cap(path) + return self._tool_speed_limit(path) def _compute_s_profile_limits(self) -> tuple[float, float, float]: """ @@ -1224,13 +1153,10 @@ def _enforce_segment_limits( self, trajectory_rad: NDArray[np.float64] ) -> NDArray[np.float64]: """ - Slow the rows down wherever a joint would pass its velocity or - acceleration limit as the player plays them, one row per tick. + Trim residual velocity and acceleration excess in the final played + rows, including the seam after a wrist-turn prefix. Continuous + timing can miss extrema between gridpoints and across that seam. - A profile times the path coordinate, but the rows follow the path's - own shape: a bend between rows, a junction in a blended chain or the - step a wrist turn leaves behind is a change of joint velocity - within one tick, however smoothly the coordinate itself is timed. Each segment between rows is given the shortest time, never less than a tick, that keeps every joint under its velocity limit and its velocity change from one segment to the next within a tick of @@ -1253,60 +1179,18 @@ def _enforce_segment_limits( return trajectory_rad deltas = np.diff(trajectory_rad, axis=0) - if self.constant_tool_speed: - # A process move holds one tool speed: slowed evenly, by the - # most any segment needs, not only where it bends. - at_rest = np.zeros((1, 6)) - turns = np.diff(np.vstack([at_rest, deltas, at_rest]), axis=0) - stretch = max( - 1.0, - float(np.max(np.abs(deltas) / (self.v_max * self.dt))), - float(np.max(np.abs(turns) / (self.a_max * self.dt**2))), - ) - rates = np.full(len(deltas), 1.0 / (stretch * self.dt)) - else: - rates = _segment_rates( - deltas, self.v_max, self.a_max * self.dt, 1.0 / self.dt - ) + rates = _segment_rates(deltas, self.v_max, self.a_max * self.dt, 1.0 / self.dt) if rates.min() * self.dt >= 1.0 - _RATE_TOLERANCE: return trajectory_rad cumulative_times = np.zeros(n_points) cumulative_times[1:] = np.cumsum(1.0 / rates) - duration = (n_points - 1) * self.dt new_duration = float(cumulative_times[-1]) - logger.debug( - "Extending duration from %.3fs to %.3fs (%.1f%% increase) to respect velocity/acceleration limits", - duration, - new_duration, - (new_duration / duration - 1) * 100, - ) intervals = _tick_intervals(new_duration, self.dt) output_times = np.arange(intervals + 1) * (new_duration / intervals) - new_trajectory = np.empty((intervals + 1, 6), dtype=np.float64) - for j in range(6): - new_trajectory[:, j] = np.interp( - output_times, cumulative_times, trajectory_rad[:, j] - ) - - return new_trajectory - - def _requested_or(self, fastest: float) -> float: - """The requested duration, stretched to ``fastest`` when shorter: - a duration no joint can keep is not kept by overrunning its - limits.""" - if not self.duration: - return fastest - if self.duration < fastest: - logger.warning( - "Extending duration from %.3fs to %.3fs to respect velocity/acceleration limits", - self.duration, - fastest, - ) - return fastest - return self.duration + return _resample(trajectory_rad, cumulative_times, output_times) def _compute_joint_duration_trapezoid(self) -> float: """ @@ -1364,32 +1248,6 @@ def _compute_joint_duration_quintic(self) -> float: time_per_joint = np.maximum(time_vel, time_acc) return max(float(np.max(time_per_joint)), self.dt * 2) - def _compute_duration_along_rows(self) -> float: - """ - Compute duration for a path followed row by row, from per-segment - joint requirements. - - This properly handles singularities and wrist flips by analyzing - the maximum joint movement required in each path segment, not just - the total start-to-end displacement. - - Returns the sum of minimum segment times, ensuring the path can be - traversed without violating joint velocity limits at any point. - """ - positions = self.joint_path.positions - if len(positions) < 2: - return self.dt * 2 - - # Per-segment time from the max joint movement within each segment; - # summing these handles singularities/wrist flips that total - # start-to-end displacement would miss. - deltas = np.diff(positions, axis=0) # (N-1, 6) - segment_times = np.max(np.abs(deltas) / self.v_max, axis=1) # (N-1,) - - segment_times = np.maximum(segment_times, self.dt) - - return max(float(np.sum(segment_times)), self.dt * 2) - def _build_quintic_trajectory_joint(self) -> Trajectory: """ Build per-joint quintic trajectory. @@ -1400,7 +1258,7 @@ def _build_quintic_trajectory_joint(self) -> Trajectory: start_pos = self.joint_path.positions[0] end_pos = self.joint_path.positions[-1] - duration = self._requested_or(self._compute_joint_duration_quintic()) + duration = self._compute_joint_duration_quintic() intervals = _tick_intervals(duration, self.dt) duration = intervals * self.dt @@ -1417,36 +1275,23 @@ def _build_quintic_trajectory_joint(self) -> Trajectory: times, start_pos[j], end_pos[j], duration ) - trajectory_rad = self._enforce_segment_limits(trajectory_rad) - return self._trajectory(trajectory_rad) def _build_quintic_trajectory_along_path(self) -> Trajectory: - """ - Build a quintic trajectory of the path coordinate. - - The path is followed row by row along a quintic profile of s, with - local slowdown where velocity limits would be exceeded. On a - cartesian path the profile's peak keeps the tool under its ceiling. - """ - if self.duration: - duration = self.duration - else: - # Use per-segment analysis to handle singularities and wrist flips - duration = self._compute_duration_along_rows() - duration = max(duration, _QUINTIC_PEAK_VEL / self._cruise_cap()) - - intervals = _tick_intervals(duration, self.dt) - duration = intervals * self.dt - times = np.arange(intervals + 1) * self.dt - - profile_s = _quintic_samples(times, 0.0, 1.0, duration) - - trajectory_rad = self.joint_path.sample_many(profile_s) - - trajectory_rad = self._enforce_segment_limits(trajectory_rad) - - return self._trajectory(trajectory_rad) + """A quintic path-speed envelope, or a steady cruise for process moves.""" + if self.constant_tool_speed: + return self._build_simple_trajectory() + path = self._blended_path() + vmax_s, amax_s, _ = self._compute_s_profile_limits() + duration = max( + _QUINTIC_PEAK_VEL / min(vmax_s, self._cruise_cap(path)), + np.sqrt(_QUINTIC_PEAK_ACC / amax_s), + self.dt * 2, + ) + times = np.linspace(0.0, 1.0, 1001) + profile_s = _quintic_samples(times, 0.0, 1.0, 1.0) + profile_v = 30.0 * times**2 * (1.0 - times) ** 2 / duration + return self._time_path(path, lambda s: np.interp(s, profile_s, profile_v)) def _build_trapezoid_trajectory_joint(self) -> Trajectory: """ @@ -1458,7 +1303,7 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory: start_pos = self.joint_path.positions[0] end_pos = self.joint_path.positions[-1] - duration = self._requested_or(self._compute_joint_duration_trapezoid()) + duration = self._compute_joint_duration_trapezoid() intervals = _tick_intervals(duration, self.dt) duration = intervals * self.dt @@ -1484,170 +1329,82 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory: self.a_max[j], ) - trajectory_rad = self._enforce_segment_limits(trajectory_rad) - return self._trajectory(trajectory_rad) - def _build_cart_vel_constraint( - self, path: _BlendedPath, ss_waypoints: NDArray - ) -> constraint.JointVelocityConstraintVarying | None: - """ - Build Cartesian velocity constraint for TOPP-RA using path-tangent method. - - Uses the path tangent (dq/ds) to compute accurate Cartesian velocity limits. - At each path point s: - - cart_vel = J_lin @ q_dot = J_lin @ (dq/ds * s_dot) - - ||cart_vel|| = ||J_lin @ dq/ds|| * |s_dot| - - For ||cart_vel|| <= v_max: |s_dot| <= v_max / ||J_lin @ dq/ds|| - - This is more accurate than the column-norm method as it considers the - actual direction of motion along the path. - - Args: - path: The spline path through joint space - ss_waypoints: Path parameter values at each waypoint - - Returns: - JointVelocityConstraintVarying with path-dependent limits, or None if error - """ - if self.cart_vel_limit is None or self.cart_vel_limit <= 0: - return None - - try: - robot = PAROL6_ROBOT.robot - - # cart_vel_limit is already in m/s (SI units) - v_max_m_s = self.cart_vel_limit - # Scaled joint limits respect the user's velocity_frac - v_max_joint = self.v_max - - # Pre-allocate; vlim_func is called once per gridpoint - vlim_buffer = np.empty((6, 2), dtype=np.float64) - _jac_buf = np.zeros((6, 6), dtype=np.float64, order="F") - - def vlim_func(s: float) -> NDArray: - """Compute velocity limits at path position s using path tangent.""" - q = path(s) - dq_ds = path(s, 1) # Path tangent (first derivative) - - # Linear (translational) part of the Jacobian is the first 3 rows - robot.jacob0_into(q, _jac_buf) - J_lin = _jac_buf[:3, :] - - cart_vel_per_sdot = np.linalg.norm(J_lin @ dq_ds) - - if cart_vel_per_sdot < 1e-6: - # Near-zero path tangent (at waypoint or singular), use joint limits - vlim_buffer[:, 0] = -v_max_joint - vlim_buffer[:, 1] = v_max_joint - return vlim_buffer.copy() - - max_sdot = v_max_m_s / cart_vel_per_sdot - - # The Cartesian constraint limits s_dot, not individual joint velocities. - # We scale ALL joint velocity limits uniformly by the ratio of - # (Cartesian-limited s_dot) / (fastest achievable s_dot from joint limits). - # - # This ensures the path velocity respects the Cartesian limit while - # keeping joints at their relative proportions. - abs_dq_ds = np.abs(dq_ds) - - with np.errstate(divide="ignore", invalid="ignore"): - s_dot_per_joint = np.where( - abs_dq_ds > 1e-9, - v_max_joint / abs_dq_ds, - np.inf, - ) - - # The binding joint limit determines max achievable s_dot - s_dot_from_joints = float(np.min(s_dot_per_joint)) - - if max_sdot < s_dot_from_joints and s_dot_from_joints > 0: - # Cartesian constraint is the tighter one; scale all limits down - scale = max_sdot / s_dot_from_joints - q_dot_max = v_max_joint * scale - else: - q_dot_max = v_max_joint - - vlim_buffer[:, 0] = -q_dot_max - vlim_buffer[:, 1] = q_dot_max - - return vlim_buffer.copy() - - return constraint.JointVelocityConstraintVarying(vlim_func) - - except Exception as e: - logger.warning("Failed to build Cartesian velocity constraint: %s", e) - return None - - def _build_path_speed_cap( - self, - path: _BlendedPath, - positions: NDArray[np.float64], - slopes: NDArray[np.float64], - ) -> constraint.Constraint: - """:meth:`_path_speed_cap` as a TOPP-RA constraint. Holding every - stretch to it is what a constant tool speed costs; on a process - move that is the point rather than the price.""" - cap = self._path_speed_cap(positions, slopes) - vlim_buffer = np.empty((6, 2), dtype=np.float64) - - def vlim_func(s: float) -> NDArray: - dq_ds = np.abs(path(s, 1)) - q_dot_max = np.maximum(dq_ds * cap, 1e-6) - vlim_buffer[:, 0] = -q_dot_max - vlim_buffer[:, 1] = q_dot_max - return vlim_buffer.copy() - - return constraint.JointVelocityConstraintVarying(vlim_func) - - def _row_slopes(self) -> tuple[NDArray[np.float64], NDArray[np.float64]]: - """The rows each segment starts at, and each segment's ``dq/ds``, - for a path whose rows are evenly spaced in its parameter, as the - profiles that time ``s`` directly take it.""" - positions = self.joint_path.positions - return positions[:-1], np.diff(positions, axis=0) * (len(positions) - 1) + def _gridpoints(self, path: _BlendedPath) -> NDArray[np.float64]: + breaks = path.breaks + widths = np.diff(breaks) + midpoints = 0.5 * (breaks[:-1] + breaks[1:]) + curvature = np.max(np.abs(path(midpoints, 2)), axis=1) + divisions = np.maximum(1, np.ceil(widths / 0.05)).astype(int) + divisions = np.maximum( + divisions, np.ceil(np.sqrt(0.5 * curvature * widths**2 / 1e-4)).astype(int) + ) + divisions = np.maximum(divisions, np.where(curvature > 1e-9, 2, 1)) + points = np.concatenate( + [ + np.linspace(left, right, int(count) + 1) + for left, right, count in zip(breaks[:-1], breaks[1:], divisions) + ] + ) + curved = curvature > 1e-9 + # Narrow bends must not constrain an entire adjacent straight interval. + outside = np.concatenate( + [ + breaks[:-1][curved] - widths[curved] / 2, + breaks[1:][curved] + widths[curved] / 2, + ] + ) + points = np.unique( + np.concatenate([points, np.clip(outside, *path.path_interval)]) + ) + midpoints = 0.5 * (points[:-1] + points[1:]) + straight = np.max(np.abs(path(midpoints, 2)), axis=1) < 1e-9 + points = np.sort(np.concatenate([points, midpoints[straight]])) + points = points[_distinct(points)] + points[[0, -1]] = path.path_interval + return points - def _row_speed_cap(self) -> float: - """:meth:`_path_speed_cap` over the rows (:meth:`_row_slopes`).""" - return self._path_speed_cap(*self._row_slopes()) + def _build_cart_vel_constraint( + self, path: _BlendedPath, gridpoints: NDArray[np.float64] + ) -> _PathSpeedConstraint: + assert self.cart_vel_limit is not None + positions = path(gridpoints) + slopes = path(gridpoints, 1) + speeds = self._tool_speeds(positions, slopes) + with np.errstate(divide="ignore"): + caps = self.cart_vel_limit / speeds + return _PathSpeedConstraint(caps) - def _tool_speed_cap(self) -> float: - """:meth:`_tool_speed_limit` over the rows (:meth:`_row_slopes`).""" - return self._tool_speed_limit(*self._row_slopes()) + def _build_path_speed_cap(self, path: _BlendedPath) -> _PathSpeedConstraint: + return _PathSpeedConstraint(self._path_speed_cap(path)) - def _tool_speed_limit( + def _tool_speeds( self, positions: NDArray[np.float64], slopes: NDArray[np.float64] - ) -> float: - """The fastest ``ds/dt`` that keeps the tool under the cartesian - ceiling wherever it moves fastest per unit of path; unbounded off - a cartesian path, or on one where the tool only turns. - ``positions`` are the rows the path runs through, one per segment - start in ``slopes``.""" - if not self._is_cartesian_path(): - return np.inf - assert self.cart_vel_limit is not None + ) -> NDArray[np.float64]: robot = PAROL6_ROBOT.robot jac = np.zeros((6, 6), dtype=np.float64, order="F") - fastest = 0.0 + speeds = np.empty(len(slopes)) for i in range(len(slopes)): robot.jacob0_into(positions[i], jac) - fastest = max(fastest, float(np.linalg.norm(jac[:3, :] @ slopes[i]))) + speeds[i] = np.linalg.norm(jac[:3, :] @ slopes[i]) + return speeds + + def _tool_speed_limit(self, path: _BlendedPath) -> float: + if not self._is_cartesian_path(): + return np.inf + assert self.cart_vel_limit is not None + fastest = float( + np.max(self._tool_speeds(path(path.breaks), path(path.breaks, 1))) + ) return self.cart_vel_limit / fastest if fastest > 1e-9 else np.inf - def _path_speed_cap( - self, positions: NDArray[np.float64], slopes: NDArray[np.float64] - ) -> float: - """One ``ds/dt`` ceiling for the whole path: the fastest constant the - steepest stretch allows under the joint limits, and under the - cartesian ceiling (:meth:`_tool_speed_limit`). ``positions`` are - the rows the path runs through, one per segment start in - ``slopes``.""" + def _path_speed_cap(self, path: _BlendedPath) -> float: + """One feasible cruise speed under joint velocity, bend acceleration and tool limits.""" with np.errstate(divide="ignore", invalid="ignore"): - per_joint = np.where( - np.abs(slopes) > 1e-9, self.v_max / np.abs(slopes), np.inf - ) - cap = min(float(np.min(per_joint)), self._tool_speed_limit(positions, slopes)) + velocity = np.min(self.v_max / np.abs(path.slopes)) + bend = np.sqrt(np.min(self.a_max / np.abs(path(path.breaks, 2)))) + cap = min(float(velocity), float(bend), self._tool_speed_limit(path)) if not np.isfinite(cap) or cap <= 0.0: raise TrajectoryPlanningError( make_error( diff --git a/parol6/utils/warmup.py b/parol6/utils/warmup.py index e43ffc2..63b5d7e 100644 --- a/parol6/utils/warmup.py +++ b/parol6/utils/warmup.py @@ -40,6 +40,7 @@ _tangent_to_pose_jit, below_speed, ) +from parol6.motion.trajectory import _segment_rates from parol6.protocol.wire import ( _pack_bitfield, _pack_positions, @@ -129,6 +130,7 @@ def _progress(label: str) -> None: speed_steps_to_rad_scalar(0.0, 0) speed_rad_to_steps(dummy_6f, out_6i) speed_rad_to_steps_scalar(0.0, 0) + _segment_rates(np.zeros((2, 6)), np.ones(6), np.ones(6), 100.0) _progress("joint conversions") # parol6/utils/ik.py diff --git a/tests/unit/test_dry_run_blend.py b/tests/unit/test_dry_run_blend.py index 19c0998..249296f 100644 --- a/tests/unit/test_dry_run_blend.py +++ b/tests/unit/test_dry_run_blend.py @@ -55,19 +55,43 @@ def test_empty_program_is_an_empty_record(self, client): record = client.plan() assert record.rows == 0 and record.blocks == () - def test_blended_chain_is_longer_than_one_move(self, client): + def test_a_chain_along_one_line_takes_as_long_as_the_one_move(self, client): + # W1 and W2 lie on the joint-space line from the start to W3. single = DryRunRobotClient() single.move_j(angles=W3, speed=0.3, r=0) client.move_j(angles=W1, speed=0.3, r=10) client.move_j(angles=W2, speed=0.3, r=10) client.move_j(angles=W3, speed=0.3, r=0) - assert client.plan().duration_s > single.plan().duration_s + chain, one = client.plan(), single.plan() + assert abs(chain.duration_s - one.duration_s) <= one.row_dt_s def test_state_updated_after_blend(self, client): client.move_j(angles=W1, speed=0.5, r=10) client.move_j(angles=W2, speed=0.5, r=0) np.testing.assert_allclose(client.angles(), W2, atol=0.5) + def test_turning_process_move_takes_comparable_time_to_a_line_chain(self): + start = [-50.0, 250.0, 200.0, 90.0, 0.0, 90.0] + corner = [50.0, 250.0, 200.0, 90.0, 0.0, 180.0] + end = [50.0, 250.0, 300.0, 90.0, 0.0, 180.0] + positioned = DryRunRobotClient() + positioned.move_j(pose=start, speed=0.5) + assert all(block.error is None for block in positioned.plan().blocks) + initial = positioned.angles() + + process = DryRunRobotClient(initial_joints_deg=initial) + lines = DryRunRobotClient(initial_joints_deg=initial) + for robot in (process, lines): + robot.select_profile("TOPPRA") + process.move_p([corner, end], speed=0.25) + lines.move_l(corner, speed=0.25, r=5.0) + lines.move_l(end, speed=0.25) + process_plan, line_plan = process.plan(), lines.plan() + for plan in (process_plan, line_plan): + assert all(block.error is None for block in plan.blocks) + # A continuous turn must not force the entire process path to crawl. + assert process_plan.duration_s <= 2.0 * line_plan.duration_s + def test_pause_holds_the_program_until_resume(self): slow = DryRunRobotClient(initial_joints_deg=W0) assert slow.set_execution_speed(0.5) == 1 diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index 21eb984..8318e99 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -4,15 +4,29 @@ import pytest import parol6.PAROL6_ROBOT as PAROL6_ROBOT -from parol6.config import INTERVAL_S, LIMITS, PATH_SAMPLES, steps_to_rad +from parol6.client.dry_run_client import DryRunRobotClient +from parol6.config import INTERVAL_S, LIMITS, steps_to_rad from parol6.motion import JointPath, ProfileType, Trajectory, TrajectoryBuilder -from parol6.motion.geometry import LineSegment from parol6.motion.trajectory import ( IK_MAX_JOINT_STEP_RAD, _ik_branch_hop, ) +def _played_rows(client: DryRunRobotClient) -> np.ndarray: + record = client.plan() + assert not [block.error for block in record.blocks if block.error] + # TickIndex decimates rows; acceleration needs every control tick. + return np.concatenate([chunk.q_rad for chunk in client._chunks if len(chunk.q_rad)]) + + +def _assert_played_acceleration(rows: np.ndarray, fraction: float) -> None: + at_rest = np.pad(rows, ((1, 1), (0, 0)), mode="edge") + acceleration = np.diff(at_rest, n=2, axis=0) / INTERVAL_S**2 + ratio = np.max(np.abs(acceleration) / (fraction * LIMITS.joint.hard.acceleration)) + assert ratio <= 1.0, f"Played acceleration reached {ratio:.3f}x the requested limit" + + class TestJointPath: """Tests for JointPath dataclass.""" @@ -218,99 +232,73 @@ def test_trajectory_respects_joint_velocity_limits( f"{(max_vel_ratio - 1) * 100:.1f}%" ) - def test_toppra_corner_is_no_slower_than_stopping_between_legs(self): - start = np.array([0.0, -1.57, 3.14, 0.0, 0.5, 0.0]) - corner = start + [0.3, 0, 0, 0, 0, 0] - end = corner + [0, 0.3, 0, 0, 0, 0] - legs = [ - JointPath.interpolate(start, corner, 50), - JointPath.interpolate(corner, end, 50), - ] - chain = JointPath( - positions=np.vstack([legs[0].positions, legs[1].positions[1:]]) + @pytest.mark.parametrize("profile", ["TOPPRA", "LINEAR", "TRAPEZOID", "QUINTIC"]) + def test_blended_chain_is_no_slower_than_stops_and_respects_acceleration( + self, profile + ): + start = [90.0, -80.0, 190.0, 0.0, 30.0, 180.0] + corner = [110.0, -80.0, 190.0, 0.0, 30.0, 180.0] + end = [110.0, -65.0, 190.0, 0.0, 30.0, 180.0] + stopped_rows = 0 + for radius in (0.0, 5.0, 20.0, 50.0): + client = DryRunRobotClient(initial_joints_deg=start) + client.select_profile(profile) + client.move_j(corner, speed=0.5, accel=0.5, r=radius) + client.move_j(end, speed=0.5, accel=0.5) + rows = _played_rows(client) + if radius == 0: + stopped_rows = len(rows) + else: + assert len(rows) <= stopped_rows, ( + profile, + radius, + len(rows), + stopped_rows, + ) + _assert_played_acceleration(rows, 0.5) + np.testing.assert_allclose( + np.degrees(rows[[0, -1]]), [start, end], atol=0.01 + ) + + client = DryRunRobotClient( + initial_joints_deg=list(PAROL6_ROBOT.joint.standby_deg) ) - frac = 0.5 - separate_duration = sum( - TrajectoryBuilder(leg, ProfileType.TOPPRA, accel_frac=frac).build().duration - for leg in legs + client.select_profile(profile) + pose = client.pose() + client.move_l( + [pose[0] + 40, pose[1], pose[2] + 40, *pose[3:]], speed=0.5, accel=0.5 ) - trajectory = TrajectoryBuilder( - chain, ProfileType.TOPPRA, accel_frac=frac - ).build() - - assert trajectory.duration <= separate_duration - rows = np.pad(trajectory.positions_rad, ((1, 1), (0, 0)), mode="edge") - acceleration = np.diff(rows, n=2, axis=0) / INTERVAL_S**2 - assert np.all(np.abs(acceleration) <= frac * LIMITS.joint.hard.acceleration) - np.testing.assert_array_equal(trajectory.positions_rad[[0, -1]], [start, end]) - - @pytest.mark.parametrize( - "profile", - [ - ProfileType.TOPPRA, - ProfileType.LINEAR, - ProfileType.QUINTIC, - ProfileType.TRAPEZOID, - ], - ) - def test_a_path_that_bends_is_played_within_the_acceleration_limit(self, profile): - """Played one row per tick, a bend between rows is a change of joint - velocity within a tick, however smoothly the profile times the path - coordinate: the corner where a blended chain turns from J1 to J2, - and the J4 step a cartesian move from standby leaves after turning - the wrist off its singularity. Each is slowed until no joint - accelerates past the fraction of its limit the move asks for.""" - start = np.array([0.0, -1.57, 3.14, 0.0, 0.5, 0.0]) - corner = start + [0.3, 0, 0, 0, 0, 0] - chain = JointPath( - positions=np.vstack( + _assert_played_acceleration(_played_rows(client), 0.5) + + @pytest.mark.parametrize("profile", ["TOPPRA", "LINEAR", "TRAPEZOID", "QUINTIC"]) + def test_process_move_holds_a_feasible_tool_speed(self, profile): + for wrist_tilt in (0.0, 0.4): + start = np.array(PAROL6_ROBOT.joint.standby_deg, dtype=float) + start[4] += np.degrees(wrist_tilt) + client = DryRunRobotClient(initial_joints_deg=start.tolist()) + client.select_profile(profile) + pose = client.pose() + client.move_p( [ - JointPath.interpolate(start, corner, 50).positions, - JointPath.interpolate( - corner, corner + [0, 0.3, 0, 0, 0, 0], 50 - ).positions[1:], + [pose[0] + 50, pose[1], *pose[2:]], + [pose[0] + 50, pose[1] + 50, *pose[2:]], ] ) - ) - standby = np.deg2rad(PAROL6_ROBOT.joint.standby_deg) - here = np.asarray(PAROL6_ROBOT.robot.fkine(standby)).copy() - there = here.copy() - there[:3, 3] += [0.04, 0.0, 0.04] - poses = np.empty((PATH_SAMPLES, 4, 4)) - LineSegment(here, there).sample_into(poses, 0.0, 1.0, 0) - line = JointPath.from_poses(poses, standby) - frac = 0.5 - cartesian = { - "cart_vel_limit": LIMITS.cart.hard.velocity.linear * frac, - "cart_acc_limit": LIMITS.cart.hard.acceleration.linear * frac, - "path_knots": line.knots, - } - - for name, path, extra in [ - ("blended corner", chain, {}), - ("MoveL from standby", line, cartesian), - ]: - rows = ( - TrajectoryBuilder( - joint_path=path, - profile=profile, - velocity_frac=frac, - accel_frac=frac, - dt=INTERVAL_S, - **extra, - ) - .build() - .positions_rad - ) - rows = np.pad(rows, ((1, 1), (0, 0)), mode="edge") - accel = np.diff(rows, n=2, axis=0) / INTERVAL_S**2 - ratio = float( - np.max(np.abs(accel) / (LIMITS.joint.hard.acceleration * frac)) - ) - assert ratio <= 1.0, ( - f"{profile.name}, {name}: a joint at {ratio:.2f}x the " - "acceleration asked for" - ) + rows = _played_rows(client) + _assert_played_acceleration(rows, 0.5) + tcp = np.array([PAROL6_ROBOT.robot.fkine(q)[:3, 3] for q in rows]) + distance = np.linalg.norm(np.diff(tcp, axis=0), axis=1) + progress = np.cumsum(distance) + # A wrist reconfiguration contributes time but no tool travel. + cruise = distance[ + (progress > 0.2 * progress[-1]) & (progress < 0.8 * progress[-1]) + ] + assert cruise.min() / cruise.max() >= 0.9, (profile, wrist_tilt) + moving = np.flatnonzero(distance > 1e-6) + assert (moving[-1] - moving[0] + 1) * INTERVAL_S < 3.0 + if wrist_tilt: + middle = distance[int(0.2 * len(distance)) : int(0.8 * len(distance))] + assert middle.min() / middle.max() >= 0.9 class TestTrajectory: From 149c2ca99ab367d9427748a95db79cac55f18003 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 13 Oct 2026 23:50:57 +0000 Subject: [PATCH 6/7] Honour a requested duration under RUCKIG, including its TOPPRA fallback A bent path under RUCKIG is timed by TOPPRA, so exempting the RUCKIG profile from the duration stretch dropped the request: 5 s ran in 1 s. Ruckig itself now gets the duration as its minimum. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_016uVshJDKTz7nCqrMGVxVJU --- parol6/motion/trajectory.py | 5 +++-- tests/unit/test_dry_run_blend.py | 3 +++ tests/unit/test_motion.py | 21 +++++++++++++++------ 3 files changed, 21 insertions(+), 8 deletions(-) diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index 3b35326..583664f 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -852,14 +852,13 @@ def __init__( def build(self) -> Trajectory: """Time the fastest selected profile, enforce played joint limits across the complete move, then uniformly stretch to a longer requested duration. - RUCKIG keeps its own timing and ignores a requested duration. """ trajectory = self._build() if trajectory.duration == 0.0: return trajectory rows = self._enforce_segment_limits(trajectory.positions_rad) fastest = (len(rows) - 1) * self.dt - if self.duration and self.profile != ProfileType.RUCKIG: + if self.duration: if self.duration < fastest: logger.warning( "Extending duration from %.3fs to %.3fs to respect the joint limits", @@ -1439,6 +1438,8 @@ def _build_ruckig_trajectory(self) -> Trajectory: inp.max_velocity = self.v_max.tolist() inp.max_acceleration = self.a_max.tolist() inp.max_jerk = self.j_max.tolist() + if self.duration is not None: + inp.minimum_duration = self.duration # Pre-size the buffer from an estimated duration; the loop below # grows it if Ruckig runs longer than expected. diff --git a/tests/unit/test_dry_run_blend.py b/tests/unit/test_dry_run_blend.py index 249296f..80b8a6a 100644 --- a/tests/unit/test_dry_run_blend.py +++ b/tests/unit/test_dry_run_blend.py @@ -63,6 +63,9 @@ def test_a_chain_along_one_line_takes_as_long_as_the_one_move(self, client): client.move_j(angles=W2, speed=0.3, r=10) client.move_j(angles=W3, speed=0.3, r=0) chain, one = client.plan(), single.plan() + for plan in (chain, one): + assert plan.rows > 0 + assert all(block.error is None for block in plan.blocks) assert abs(chain.duration_s - one.duration_s) <= one.row_dt_s def test_state_updated_after_blend(self, client): diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index 8318e99..e0bbfbe 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -112,20 +112,29 @@ def test_build_linear_profile(self, simple_joint_path): # Steps should be int32 assert trajectory.steps.dtype == np.int32 - def test_build_quintic_profile(self, simple_joint_path): - """QUINTIC profile should produce smooth trajectory.""" + @pytest.mark.parametrize( + "profile,duration,bent", + [(ProfileType.QUINTIC, 3.0, False), (ProfileType.RUCKIG, 5.0, True)], + ) + def test_build_profile_with_requested_duration( + self, simple_joint_path, profile, duration, bent + ): + if bent: + start, end = simple_joint_path.positions[[0, -1]] + mid = (start + end) / 2 + mid[0] += 0.1 + simple_joint_path = JointPath(positions=np.array([start, mid, end])) builder = TrajectoryBuilder( joint_path=simple_joint_path, - profile=ProfileType.QUINTIC, - duration=3.0, # Long duration to stay within velocity limits + profile=profile, + duration=duration, dt=INTERVAL_S, ) trajectory = builder.build() - assert isinstance(trajectory, Trajectory) assert len(trajectory) > 0 - assert trajectory.duration >= 3.0 + assert trajectory.duration >= duration def test_build_trapezoid_profile(self, simple_joint_path): """TRAPEZOID profile should produce trajectory with plateau.""" From 060588580a0ecf36197c4599ca54d7f7393a660d Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 14 Oct 2026 01:32:26 +0000 Subject: [PATCH 7/7] Re-time requested durations smoothly and keep timing independent of sampling - A requested duration re-times the fastest rows through a C2 spline, so acceleration scales down smoothly instead of arriving in pulses. - QUINTIC along a path eases acceleration in and out at both ends. - Blends on densely sampled smooth paths reach half the knot interval, so process-move timing no longer depends on the sampling pitch. - MoveS eases its turn rate to zero at each waypoint along the geodesic, so it no longer nearly stops where the tool's turn changes. - A timed RUCKIG move lands on the requested tick count without a spurious extension warning. - A blend's curvature holds at its trailing break, so TOP-PRA's own rows stay within the acceleration limit. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_016uVshJDKTz7nCqrMGVxVJU --- parol6/motion/geometry.py | 10 ++- parol6/motion/trajectory.py | 142 ++++++++++++++++++----------------- tests/unit/test_motion.py | 144 ++++++++++++++++++++++++++++++++++++ 3 files changed, 228 insertions(+), 68 deletions(-) diff --git a/parol6/motion/geometry.py b/parol6/motion/geometry.py index 5810dee..3ad6673 100644 --- a/parol6/motion/geometry.py +++ b/parol6/motion/geometry.py @@ -528,7 +528,8 @@ def build_spline_path( Position is a natural cubic spline per axis over chord-length knots: uniform knots overshoot between unevenly spaced waypoints, and a natural end cannot swing wide of the first and last segments as - not-a-knot can. Orientation slerps between the waypoints' rotations. + not-a-knot can. Orientation slerps along each segment's geodesic with + smoothstep easing, giving zero angular velocity at every waypoint. Both run on one schedule, the distance along the path on the combined metric sqrt(t² + (w·θ)²), sampled at ``PATH_STEP_MM``: a waypoint that @@ -583,7 +584,12 @@ def build_spline_path( out = np.zeros((len(u), 4, 4), dtype=np.float64) out[:, 3, 3] = 1.0 - out[:, :3, :3] = Slerp(knots, rotations)(u).as_matrix() + segment = np.clip(np.searchsorted(knots, u, side="right") - 1, 0, len(knots) - 2) + span = knots[segment + 1] - knots[segment] + progress = (u - knots[segment]) / span + # Zero endpoint rates avoid angular-velocity corners without leaving the geodesics. + eased = progress**2 * (3.0 - 2.0 * progress) + out[:, :3, :3] = Slerp(knots, rotations)(knots[segment] + eased * span).as_matrix() if position is None: out[:, :3, 3] = points[0] / 1000.0 else: diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index 583664f..b8f5912 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -22,8 +22,8 @@ import numpy as np from numba import njit from numpy.typing import NDArray -from scipy.interpolate import PPoly -from ruckig import InputParameter, OutputParameter, Result, Ruckig # type: ignore[unresolved-import, ty:unresolved-import] +from scipy.interpolate import PPoly, make_interp_spline +from ruckig import InputParameter, Result, Ruckig, Trajectory as RuckigTrajectory # type: ignore[unresolved-import, ty:unresolved-import] import toppra.algorithm as algo import toppra.constraint as constraint @@ -124,8 +124,13 @@ def _tick_intervals(duration: float, dt: float) -> int: def _slowed(rows: NDArray[np.float64], count: int) -> NDArray[np.float64]: - """Uniformly stretch tick-spaced rows without increasing their played limits.""" - return JointPath(positions=rows).sample_many(np.linspace(0.0, 1.0, count)) + """Retiming of rest-to-rest rows with continuous velocity and acceleration.""" + spline = make_interp_spline( + np.linspace(0.0, 1.0, len(rows)), rows, k=3, bc_type="clamped" + ) + slowed = spline(np.linspace(0.0, 1.0, count)) + slowed[[0, -1]] = rows[[0, -1]] + return slowed def _distinct(values: NDArray[np.float64]) -> NDArray[np.bool_]: @@ -208,7 +213,7 @@ def _segment_rates( class _BlendedPath: - """C1 parabolic blends bounded by motor resolution and tick-sized corner cutting.""" + """C1 parabolic blends bounded by chord sag and sharp-corner deviation.""" def __init__( self, @@ -224,14 +229,27 @@ def __init__( delta = np.diff(self.slopes, axis=0) magnitude = np.max(np.abs(delta), axis=1) corners = np.flatnonzero(magnitude > 1e-9) + span = 0.5 * (intervals[:-1] + intervals[1:]) + curvature = np.abs(delta) / span[:, None] + neighbours = ( + np.maximum( + np.pad(curvature[:-1], ((1, 0), (0, 0))), + np.pad(curvature[1:], ((0, 1), (0, 0))), + ) + if len(delta) + else curvature + ) + width = np.minimum(intervals[:-1], intervals[1:]) + # A two-sided curvature bound permits chord sag, not isolated sharp turns. + sag = 2.0 * neighbours * width[:, None] ** 2 / 8.0 + deviation = np.maximum(HALF_STEP_RAD, a_max * dt**2 / 4.0) + sag with np.errstate(divide="ignore"): - # Playback itself cuts corners by up to a*dt²/4. - tick_width = dt**2 * np.min(a_max / np.abs(delta[corners]), axis=1) + allowed_width = np.min( + 4.0 * deviation[corners] / np.abs(delta[corners]), axis=1 + ) half_width = np.minimum( - 0.5 * np.minimum(intervals[:-1], intervals[1:])[corners], - np.maximum( - 4.0 * float(np.min(HALF_STEP_RAD)) / magnitude[corners], tick_width - ), + 0.5 * width[corners], + allowed_width, ) starts = knots[corners + 1] - half_width ends = knots[corners + 1] + half_width @@ -263,12 +281,16 @@ def __init__( slope + 0.5 * offset * curvature ) self._pp = PPoly(coefficients, breaks) + self._blend_ends = ends @property def breaks(self) -> NDArray[np.float64]: return self._pp.x def __call__(self, s: float | NDArray, order: int = 0) -> NDArray[np.float64]: + if order == 2: + # TOPP-RA must constrain the last stage inside each bend. + s = np.where(np.isin(s, self._blend_ends), np.nextafter(s, -np.inf), s) return self._pp(s, order) @@ -802,16 +824,17 @@ def __init__( velocity_frac: Scale joint velocity limits (0.0-1.0), default 1.0 accel_frac: Scale joint acceleration limits (0.0-1.0), default 1.0 jerk_frac: Scale jerk limits (0.0-1.0), default 1.0 - duration: Uniformly stretch the final rows when longer than their fastest timing + duration: Requested time between endpoint rows, rounded up to control + ticks; longer moves are smoothly retimed, subject to joint limits. dt: Control loop time step cart_vel_limit: Cartesian linear velocity limit in m/s (for Cartesian commands) cart_acc_limit: Cartesian linear acceleration limit in m/s² (for Cartesian commands) path_knots: Path-parameter value of each joint waypoint, strictly increasing from 0 to 1 — cumulative tool distance for a cartesian path, so that a constant ``ds/dt`` is a constant - tool speed. ``None`` spaces the waypoints evenly. + tool speed. ``None`` uses normalized cumulative joint-space distance. constant_tool_speed: Hold the whole path to one ``ds/dt``, the - fastest the steepest stretch and the cartesian ceiling allow, + fastest joint velocity, bend acceleration and the tool ceiling allow, rather than running each stretch as fast as it can (what a process move promises). """ @@ -851,7 +874,7 @@ def __init__( def build(self) -> Trajectory: """Time the fastest selected profile, enforce played joint limits across - the complete move, then uniformly stretch to a longer requested duration. + the complete move, then smoothly retime to a longer requested duration. """ trajectory = self._build() if trajectory.duration == 0.0: @@ -859,14 +882,17 @@ def build(self) -> Trajectory: rows = self._enforce_segment_limits(trajectory.positions_rad) fastest = (len(rows) - 1) * self.dt if self.duration: - if self.duration < fastest: + requested_intervals = _tick_intervals(self.duration, self.dt) + if requested_intervals < len(rows) - 1: logger.warning( "Extending duration from %.3fs to %.3fs to respect the joint limits", self.duration, fastest, ) - elif self.duration > fastest: - rows = _slowed(rows, _tick_intervals(self.duration, self.dt) + 1) + elif requested_intervals > len(rows) - 1: + rows = self._enforce_segment_limits( + _slowed(rows, requested_intervals + 1) + ) elif len(rows) > len(trajectory): duration = (len(trajectory) - 1) * self.dt level = logging.WARNING if fastest > 1.1 * duration else logging.DEBUG @@ -1012,8 +1038,10 @@ def _time_path( self, path: _BlendedPath, profile_speed: Callable[[NDArray], NDArray] | None = None, + unaccelerated_ends: bool = False, ) -> Trajectory: - """Time the blended path within the selected profile's speed envelope.""" + """Time the blended path within its speed envelope, optionally easing the + clock into and out of rest with zero endpoint acceleration.""" positions = self.joint_path.positions gridpoints = self._gridpoints(path) joint_vel_constraint = constraint.JointVelocityConstraint(self._vlim) @@ -1053,8 +1081,19 @@ def _time_path( len(positions), ) - intervals = _tick_intervals(duration, self.dt) - times = np.arange(intervals) * (duration / intervals) + ramp = min(10.0 * self.dt, duration / 4.0) if unaccelerated_ends else 0.0 + intervals = _tick_intervals(duration + ramp, self.dt) + times = np.arange(intervals) * ((duration + ramp) / intervals) + if ramp: + # A C2 clock ramp removes endpoint acceleration without slowing cruise. + distance = np.minimum(times, duration + ramp - times) + u = np.clip(distance / ramp, 0.0, 1.0) + eased = ramp * (u**3 - 0.5 * u**4) + times = np.where( + times < ramp, + eased, + np.where(times > duration, duration - eased, times - ramp / 2.0), + ) trajectory_rad = np.empty((intervals + 1, 6), dtype=np.float64) trajectory_rad[:-1] = jnt_traj(times) trajectory_rad[-1] = positions[-1] @@ -1101,8 +1140,8 @@ def _is_cartesian_path(self) -> bool: def _cruise_cap(self, path: _BlendedPath) -> float: """The ``ds/dt`` ceiling on a path timed through its path coordinate: - on a process move, one tool speed the steepest stretch and the tool - ceiling allow; on any other cartesian path, the tool ceiling alone, + on a process move, one speed bounded by joint velocity, bend acceleration + and the tool ceiling; on any other cartesian path, the tool ceiling alone, the joints slowing only where they must; none off one.""" if self.constant_tool_speed: return self._path_speed_cap(path) @@ -1277,7 +1316,7 @@ def _build_quintic_trajectory_joint(self) -> Trajectory: return self._trajectory(trajectory_rad) def _build_quintic_trajectory_along_path(self) -> Trajectory: - """A quintic path-speed envelope, or a steady cruise for process moves.""" + """Quintic speed envelope with unaccelerated ends, or steady process cruise.""" if self.constant_tool_speed: return self._build_simple_trajectory() path = self._blended_path() @@ -1290,7 +1329,9 @@ def _build_quintic_trajectory_along_path(self) -> Trajectory: times = np.linspace(0.0, 1.0, 1001) profile_s = _quintic_samples(times, 0.0, 1.0, 1.0) profile_v = 30.0 * times**2 * (1.0 - times) ** 2 / duration - return self._time_path(path, lambda s: np.interp(s, profile_s, profile_v)) + return self._time_path( + path, lambda s: np.interp(s, profile_s, profile_v), unaccelerated_ends=True + ) def _build_trapezoid_trajectory_joint(self) -> Trajectory: """ @@ -1423,7 +1464,7 @@ def _build_ruckig_trajectory(self) -> Trajectory: n_dofs = 6 gen = Ruckig(n_dofs, self.dt) inp = InputParameter(n_dofs) - out = OutputParameter(n_dofs) + solution = RuckigTrajectory(n_dofs) start_pos = self.joint_path.positions[0] end_pos = self.joint_path.positions[-1] @@ -1441,44 +1482,13 @@ def _build_ruckig_trajectory(self) -> Trajectory: if self.duration is not None: inp.minimum_duration = self.duration - # Pre-size the buffer from an estimated duration; the loop below - # grows it if Ruckig runs longer than expected. - est_duration = self._estimate_simple_duration() - max_iters = int(est_duration / self.dt) + 500 # generous margin - trajectory_rad = np.empty((max_iters, n_dofs), dtype=np.float64) - - # Row 0 is the start, played on the first tick like every profile's: - # Ruckig's first output is already a tick along. - trajectory_rad[0] = start_pos - count = 1 - result = Result.Working - - while result == Result.Working: - result = gen.update(inp, out) - if count >= len(trajectory_rad): - new_buf = np.empty((len(trajectory_rad) * 2, n_dofs), dtype=np.float64) - new_buf[:count] = trajectory_rad[:count] - trajectory_rad = new_buf - trajectory_rad[count] = out.new_position - count += 1 - out.pass_to_input(inp) - - if result == Result.Error: - raise RuntimeError("Ruckig failed to compute trajectory") - - return self._trajectory(trajectory_rad[:count]) - - def _estimate_simple_duration(self) -> float: - """Estimate minimum duration based on joint velocity limits. - - With adaptive time distribution, each segment gets time proportional - to its joint movement, so total duration is sum of per-segment times. - """ - positions = self.joint_path.positions - if len(positions) < 2: - return self.dt * 2 - - deltas = np.diff(positions, axis=0) # (N-1, 6) - segment_times = np.max(np.abs(deltas) / self.v_max, axis=1) # (N-1,) + result = gen.calculate(inp, solution) + if result not in (Result.Working, Result.Finished): + raise RuntimeError(f"Ruckig failed to compute trajectory: {result}") - return max(float(np.sum(segment_times)), self.dt * 2) + # Sampling the continuous solution avoids an extra update at a tick boundary. + intervals = _tick_intervals(solution.duration, self.dt) + times = np.linspace(0.0, solution.duration, intervals + 1) + trajectory_rad = np.array([solution.at_time(t)[0] for t in times]) + trajectory_rad[[0, -1]] = [start_pos, end_pos] + return self._trajectory(trajectory_rad) diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index e0bbfbe..f47dc8f 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -1,9 +1,13 @@ """Unit tests for parol6.motion trajectory pipeline.""" +import logging +import math + import numpy as np import pytest import parol6.PAROL6_ROBOT as PAROL6_ROBOT +import parol6.motion.geometry as geometry from parol6.client.dry_run_client import DryRunRobotClient from parol6.config import INTERVAL_S, LIMITS, steps_to_rad from parol6.motion import JointPath, ProfileType, Trajectory, TrajectoryBuilder @@ -310,6 +314,146 @@ def test_process_move_holds_a_feasible_tool_speed(self, profile): assert middle.min() / middle.max() >= 0.9 +class TestDryRunTimingRegressions: + START = [90.0, -80.0, 190.0, 0.0, 30.0, 180.0] + + @pytest.mark.parametrize( + "profile", ["QUINTIC", "TOPPRA", "LINEAR", "TRAPEZOID", "RUCKIG"] + ) + def test_duration_stretch_scales_acceleration_smoothly(self, profile): + planned = [] + for duration in (0.0, 5.0): + client = DryRunRobotClient(initial_joints_deg=self.START) + client.select_profile(profile) + target = [100.0, *self.START[1:]] + if profile == "RUCKIG": + client.move_j(target, speed=1.0, accel=1.0, duration=duration / 2, r=10) + target = [100.0, -65.0, *self.START[2:]] + client.move_j(target, speed=1.0, accel=1.0, duration=duration / 2) + else: + client.move_j(target, speed=1.0, accel=1.0, duration=duration) + rows = _played_rows(client) + _assert_played_acceleration(rows, 1.0) + np.testing.assert_allclose(rows[-1], np.radians(target), atol=1e-12) + planned.append(rows) + + fast, slow = planned + assert len(slow) == round(5.0 / INTERVAL_S) + 1 + np.testing.assert_array_equal(slow[[0, -1]], fast[[0, -1]]) + stretch = (len(slow) - 1) / (len(fast) - 1) + fast_peak = np.max(np.abs(np.diff(fast, n=2, axis=0))) / INTERVAL_S**2 + slow_peak = np.max(np.abs(np.diff(slow, n=2, axis=0))) / INTERVAL_S**2 + assert slow_peak <= 1.3 * fast_peak / stretch**2 + + def test_quintic_paths_ease_acceleration_at_both_ends(self): + for chain in (False, True): + client = DryRunRobotClient(initial_joints_deg=self.START) + client.select_profile("QUINTIC") + if chain: + client.move_j([110.0, *self.START[1:]], speed=0.5, accel=0.5, r=5) + client.move_j([110.0, -65.0, *self.START[2:]], speed=0.5, accel=0.5) + else: + pose = client.pose() + client.move_l( + [pose[0] + 60, pose[1], pose[2] - 40, *pose[3:]], + speed=0.5, + accel=0.5, + ) + rows = _played_rows(client) + _assert_played_acceleration(rows, 0.5) + acceleration = ( + np.abs( + np.diff(np.pad(rows, ((1, 1), (0, 0)), mode="edge"), n=2, axis=0) + ) + / INTERVAL_S**2 + ) + assert acceleration[[0, -1]].max() < 0.001 * acceleration.max() + assert acceleration[[1, -2]].max() < 0.02 * acceleration.max() + + def test_process_timing_is_stable_across_sampling_pitches(self, monkeypatch): + for square in (False, True): + durations = [] + for pitch in (4.0, 2.0, 1.0, 0.5): + monkeypatch.setattr(geometry, "PATH_STEP_MM", pitch) + client = DryRunRobotClient(initial_joints_deg=self.START) + client.select_profile("TOPPRA") + pose = client.pose() + offsets = ( + [(60, 0), (60, 60), (0, 60), (0, 0)] + if square + else [(10 * k, 15 if k % 2 else 0) for k in range(1, 11)] + ) + client.move_p( + [[pose[0] + x, pose[1] + y, *pose[2:]] for x, y in offsets], + speed=0.5, + accel=0.5, + ) + rows = _played_rows(client) + _assert_played_acceleration(rows, 0.5) + durations.append(len(rows) * INTERVAL_S) + assert max(durations) / min(durations) < 1.08 + assert max(durations) < (5.5 if square else 8.0) + + def test_spline_orientation_change_does_not_stop_at_waypoint(self): + client = DryRunRobotClient(initial_joints_deg=self.START) + client.select_profile("TOPPRA") + pose = client.pose() + corner = [pose[0] + 60, pose[1], *pose[2:]] + end = [pose[0] + 60, pose[1] + 60, pose[2], pose[3], pose[4], pose[5] + 30] + client.move_s([corner, end], speed=0.5) + rows = _played_rows(client) + _assert_played_acceleration(rows, 0.5) + tcp = np.array([PAROL6_ROBOT.robot.fkine(q)[:3, 3] for q in rows]) * 1000 + speeds = np.linalg.norm(np.diff(tcp, axis=0), axis=1) / INTERVAL_S + middle = speeds[len(speeds) // 5 : 4 * len(speeds) // 5] + assert middle.min() > 40.0 + assert np.linalg.norm(tcp - np.array(corner[:3]), axis=1).min() < 1.0 + np.testing.assert_allclose(tcp[-1], end[:3], atol=0.1) + + def test_timed_ruckig_uses_requested_ticks_without_extension_warning(self, caplog): + target = [120.0, -80.0, 180.0, 0.0, 0.0, 180.0] + for duration in (2.0, 3.0, 3.333, 4.0, 4.005, 7.77): + caplog.clear() + client = DryRunRobotClient() + client.select_profile("RUCKIG") + with caplog.at_level(logging.WARNING, logger="parol6.motion.trajectory"): + client.move_j(target, duration=duration, accel=1.0) + rows = _played_rows(client) + assert len(rows) == math.ceil(duration / INTERVAL_S - 1e-6) + 1 + assert not [ + record + for record in caplog.records + if "Extending duration" in record.message + ] + np.testing.assert_allclose(rows[-1], np.radians(target), atol=1e-12) + _assert_played_acceleration(rows, 1.0) + + def test_corner_timing_respects_acceleration_before_the_post_pass( + self, monkeypatch + ): + raw_ratios = [] + enforce = TrajectoryBuilder._enforce_segment_limits + + def observe(builder, rows): + acceleration = ( + np.diff(np.pad(rows, ((1, 1), (0, 0)), mode="edge"), n=2, axis=0) + / INTERVAL_S**2 + ) + raw_ratios.append(float(np.max(np.abs(acceleration) / builder.a_max))) + return enforce(builder, rows) + + monkeypatch.setattr(TrajectoryBuilder, "_enforce_segment_limits", observe) + start = np.array([0.0, -1.57, 3.14, 0.0, 0.5, 0.0]) + corner = start + [0.0, 0.3, 0.0, 0.0, 0.0, 0.0] + end = corner + [0.3, 0.0, 0.0, 0.0, 0.0, 0.0] + client = DryRunRobotClient(initial_joints_deg=np.degrees(start).tolist()) + client.select_profile("TOPPRA") + client.move_j(np.degrees(corner).tolist(), speed=1.0, accel=0.5, r=1.0) + client.move_j(np.degrees(end).tolist(), speed=1.0, accel=0.5) + _assert_played_acceleration(_played_rows(client), 0.5) + assert max(raw_ratios) <= 1.0 + + class TestTrajectory: """Tests for Trajectory dataclass."""