Add the SEPTIC motion profile

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
 

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.

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

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.

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
 

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

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)
 

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)

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",