diff --git a/README.md b/README.md index 034966b..b2a7aa8 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,10 +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-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 18cce13..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__( @@ -1381,8 +1389,8 @@ async def select_profile(self, profile: str) -> int: rbt.select_profile("TOPPRA") Args: - profile: Motion profile type ('TOPPRA', 'RUCKIG', 'QUINTIC', 'TRAPEZOID', 'LINEAR') - Note: RUCKIG is point-to-point only; Cartesian moves will use TOPPRA. + profile: Motion profile type ('TOPPRA', 'RUCKIG', 'QUINTIC', 'SEPTIC', 'TRAPEZOID', 'LINEAR') + 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 23d20aa..547ea48 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -427,8 +427,8 @@ def select_profile(self, profile: str) -> int: """Set the motion profile (e.g. ``"TOPPRA"``). Args: - profile: Motion profile type ('TOPPRA', 'RUCKIG', 'QUINTIC', 'TRAPEZOID', 'LINEAR') - Note: RUCKIG is point-to-point only; Cartesian moves will use TOPPRA. + profile: Motion profile type ('TOPPRA', 'RUCKIG', 'QUINTIC', 'SEPTIC', 'TRAPEZOID', 'LINEAR') + 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 5e2ea44..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,7 +168,7 @@ def execute_step(self, state: ControllerState) -> ExecutionStatusCode: return ExecutionStatusCode.COMPLETED -VALID_PROFILES = frozenset(("TOPPRA", "RUCKIG", "QUINTIC", "TRAPEZOID", "LINEAR")) +VALID_PROFILES = tuple(ProfileType.__members__) @register_command(CmdType.SELECT_PROFILE) @@ -181,11 +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-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 @@ -198,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 e4e1998..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/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 fd1774b..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,10 +104,26 @@ 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 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 +_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 +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" # Point-to-point septic polynomial (C³ smooth, jerk-limited) TRAPEZOID = "trapezoid" # Trapezoidal velocity profile LINEAR = "linear" # Direct linear interpolation (no smoothing) @@ -718,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. + 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 @@ -762,7 +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() + 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. @@ -1142,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, ) @@ -1174,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. """ @@ -1193,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: @@ -1231,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 @@ -1254,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) diff --git a/parol6/server/state.py b/parol6/server/state.py index 4278dd5..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, 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 1980eb4..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, 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 9b1a8af..fe3341b 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,11 +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"]: - # Teleport to the standby pose: each profile starts from the same - # spot without paying for five planned home moves (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). + 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 # Set profile and execute move @@ -95,9 +93,9 @@ 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 - for p in ["LINEAR", "QUINTIC", "TRAPEZOID", "RUCKIG", "TOPPRA"]: - # Teleport, don't drive: five planned home moves blow the + # 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. assert client.teleport(standby) == 1 @@ -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 diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index 01abdce..a7d33f6 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, @@ -203,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 @@ -216,6 +216,47 @@ 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, + ): + """SEPTIC stays inside the velocity, acceleration and jerk limits, and + 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() + # At rest before and after, so the differences see both ends. + 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)] + + hard = LIMITS.joint.hard + v, a, j = j2_derivatives(ProfileType.SEPTIC) + for name, x, limit in [ + ("velocity", v, hard.velocity[1]), + ("acceleration", a, hard.acceleration[1]), + ("jerk", j, hard.jerk[1]), + ]: + ratio = float(np.max(np.abs(x)) / limit) + assert ratio <= 1.0, f"SEPTIC {name} at {ratio:.3f}x its limit" + + # 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}" + ) + class TestTrajectory: """Tests for Trajectory dataclass."""