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/2] 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/2] 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: