Hold every joint to its acceleration limit where the path bends, timing the bends the way MoveIt's TOTG does

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/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,15 +327,30 @@ 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]
 
     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
 
@@ -504,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
@@ -559,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:

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