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/geometry.py b/parol6/motion/geometry.py index ae7717f..3ad6673 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 @@ -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: 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..320706c 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -15,19 +15,19 @@ 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 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] -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.parametrizer import ParametrizeConstAccel import parol6.PAROL6_ROBOT as PAROL6_ROBOT from parol6.config import INTERVAL_S, LIMITS, rad_to_steps @@ -35,7 +35,7 @@ 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 @@ -103,10 +103,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 @@ -119,31 +135,192 @@ def _tick_intervals(duration: float, dt: float) -> int: return max(1, math.ceil(duration / dt - _TICK_ROUNDING)) -class _LinearPath: - """Piecewise linear path wrapper for TOPPRA compatibility. +# Relative slack on a segment rate: below it a rate is taken as settled. +_RATE_TOLERANCE = 1e-9 + + +def _slowed(rows: NDArray[np.float64], count: int) -> NDArray[np.float64]: + """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_]: + # Seidel treats intervals below 1e-10 as singular. + return np.concatenate(([True], np.diff(values) > 1e-9)) - 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. + +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) +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. """ + 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): + for j in range(dof): + d = abs(deltas[i, j]) + if d > 0.0: + rates[i] = min(rates[i], v_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[1:-1] + + +class _BlendedPath: + """C1 parabolic blends bounded by chord sag and sharp-corner deviation.""" - __slots__ = ("_pp", "dof", "path_interval") + def __init__( + self, + knots: NDArray[np.float64], + positions: NDArray[np.float64], + a_max: NDArray[np.float64], + dt: float, + ) -> None: + 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) + 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"): + allowed_width = np.min( + 4.0 * deviation[corners] / np.abs(delta[corners]), axis=1 + ) + half_width = np.minimum( + 0.5 * width[corners], + allowed_width, + ) + 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(knots, at, side="right") - 1, 0, len(self.slopes) - 1 + ) + 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] + ) + coefficients[0, inside] = 0.5 * curvature + coefficients[1, inside] = slope + offset * curvature + coefficients[2, inside] = position + offset * ( + slope + 0.5 * offset * curvature + ) + self._pp = PPoly(coefficients, breaks) + self._blend_ends = ends - 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])] + @property + def breaks(self) -> NDArray[np.float64]: + return self._pp.x - 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) - 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 + 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) + + +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): @@ -152,6 +329,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) @@ -634,8 +812,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. """ @@ -663,16 +841,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: Override duration (stretches profile if longer than TOPP-RA min) + 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). """ @@ -711,21 +890,46 @@ def __init__( self._alim = np.column_stack([-self.a_max, self.a_max]) def build(self) -> Trajectory: + """Time the fastest selected profile, enforce played joint limits across + the complete move, then smoothly retime to a longer requested duration. """ - 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. + 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: + 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 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 + 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(rows) - 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. + def _build(self) -> Trajectory: + """The trajectory as the selected profile times it. - Returns: - Trajectory ready for execution, its rows one control tick apart + 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. """ positions = self.joint_path.positions # A path that goes nowhere is done where it stands: there is no @@ -743,17 +947,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() @@ -762,7 +955,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. @@ -781,18 +986,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: @@ -832,27 +1044,19 @@ 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) - 1) * 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( np.concatenate([turn.positions_rad, path.positions_rad[1:]]) ) @@ -861,39 +1065,21 @@ 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: - # 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) - - # 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) + return self._time_path(self._blended_path()) + def _time_path( + self, + path: _BlendedPath, + profile_speed: Callable[[NDArray], NDArray] | None = None, + unaccelerated_ends: bool = False, + ) -> Trajectory: + """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) joint_acc_constraint = constraint.JointAccelerationConstraint(self._alim) constraints: list[constraint.Constraint] = [ @@ -901,59 +1087,55 @@ 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, c[0])) + constraints.append(self._build_path_speed_cap(path)) + if profile_speed is not None: + constraints.append(_PathSpeedConstraint(profile_speed(gridpoints))) 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() - - if not isinstance(jnt_traj, SplineInterpolator): + instance = algo.TOPPRA( + constraints, + path, + gridpoints=gridpoints, + parametrizer="ParametrizeConstAccel", + ) + jnt_traj = instance.compute_trajectory() + + if jnt_traj is None: raise RuntimeError("TOPP-RA failed to compute trajectory") + 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(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] = jnt_traj(duration) - duration = intervals * self.dt + trajectory_rad[-1] = positions[-1] + if not np.all(np.isfinite(trajectory_rad)): + raise RuntimeError("TOPP-RA returned non-finite joint positions") + duration = len(trajectory_rad) * self.dt logger.debug( "TrajectoryBuilder: output_samples=%d, duration=%.3f", @@ -971,49 +1153,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, + 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._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]: """ @@ -1059,11 +1227,17 @@ 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. + 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. + + 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,76 +1252,19 @@ 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 + deltas = np.diff(trajectory_rad, axis=0) + 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 - logger.warning( - "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) + cumulative_times[1:] = np.cumsum(1.0 / rates) + new_duration = float(cumulative_times[-1]) 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: """ @@ -1174,16 +1291,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,56 +1312,39 @@ 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: - """ - 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: + def _build_polynomial_trajectory_joint( + self, + samples: Callable[ + [NDArray[np.float64], float, float, float], NDArray[np.float64] + ], + duration: 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 together after ``duration``. """ start_pos = self.joint_path.positions[0] end_pos = self.joint_path.positions[-1] - duration = self._requested_or(self._compute_joint_duration_quintic()) - intervals = _tick_intervals(duration, self.dt) duration = intervals * self.dt times = np.arange(intervals + 1) * self.dt @@ -1254,40 +1356,27 @@ 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 = self._enforce_segment_limits(trajectory_rad) + trajectory_rad[:, j] = samples(times, start_pos[j], end_pos[j], duration) 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) + """Quintic speed envelope with unaccelerated ends, or steady process cruise.""" + 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), unaccelerated_ends=True + ) def _build_trapezoid_trajectory_joint(self) -> Trajectory: """ @@ -1299,7 +1388,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 @@ -1325,170 +1414,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: ta.SplineInterpolator | _LinearPath, 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: _LinearPath, - 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( @@ -1508,7 +1509,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] @@ -1523,45 +1524,16 @@ 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() - - # 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,) - - return max(float(np.sum(segment_times)), self.dt * 2) + if self.duration is not None: + inp.minimum_duration = self.duration + + result = gen.calculate(inp, solution) + if result not in (Result.Working, Result.Finished): + raise RuntimeError(f"Ruckig failed to compute trajectory: {result}") + + # 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/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/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/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_dry_run_blend.py b/tests/unit/test_dry_run_blend.py index 19c0998..80b8a6a 100644 --- a/tests/unit/test_dry_run_blend.py +++ b/tests/unit/test_dry_run_blend.py @@ -55,19 +55,46 @@ 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() + 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): 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 01abdce..a2bee56 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -1,8 +1,14 @@ """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 from parol6.motion.trajectory import ( @@ -11,6 +17,20 @@ ) +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.""" @@ -96,20 +116,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.""" @@ -179,6 +208,7 @@ def test_single_point_path(self): [ ProfileType.LINEAR, ProfileType.QUINTIC, + ProfileType.SEPTIC, ProfileType.TRAPEZOID, ProfileType.TOPPRA, ProfileType.RUCKIG, @@ -203,9 +233,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 +245,255 @@ def test_trajectory_respects_joint_velocity_limits( f"{(max_vel_ratio - 1) * 100:.1f}%" ) + @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) + ) + 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 + ) + _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( + [ + [pose[0] + 50, pose[1], *pose[2:]], + [pose[0] + 50, pose[1] + 50, *pose[2:]], + ] + ) + 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 + + 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 TestDryRunTimingRegressions: + START = [90.0, -80.0, 190.0, 0.0, 30.0, 180.0] + + @pytest.mark.parametrize( + "profile", ["QUINTIC", "SEPTIC", "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."""