From 4b46df0f742f564d01ced94e0a7d43b3926db700 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 3 Nov 2026 02:44:35 +0000 Subject: [PATCH 1/2] Time joint moves with RUCKIG by default; a timed move is TOPPRA-timed TOPPRA times paths: a cartesian move or a blended chain follows its own, and a move given a duration is a path stretched to a clock. A plain move_j reached it only because TOPPRA was the startup profile, so every joint move ran TOPPRA's jerk-free bang-bang acceleration. RUCKIG is now the startup profile, as in par6, where that acceleration step became a torque step through the inertia feedforward and rang J1 into chatter. A timed move under RUCKIG is TOPPRA-timed, so Ruckig's minimum-duration input is no longer used. An explicitly selected profile plans as before. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_016uVshJDKTz7nCqrMGVxVJU --- README.md | 8 ++-- parol6/commands/system_commands.py | 7 ++-- parol6/motion/trajectory.py | 10 +++-- parol6/server/motion_planner.py | 2 +- parol6/server/state.py | 4 +- tests/integration/test_profile_commands.py | 43 +++++++++++++++++++--- tests/integration/test_reset_semantics.py | 4 +- tests/unit/test_dry_run_blend.py | 5 ++- tests/unit/test_reset_command.py | 4 +- 9 files changed, 63 insertions(+), 24 deletions(-) diff --git a/README.md b/README.md index b2a7aa8..c859f1a 100644 --- a/README.md +++ b/README.md @@ -283,21 +283,21 @@ Performance-critical functions (unit conversions, serial frame packing, IK check Set the motion profile for all moves: ```python -client.set_profile("TOPPRA") # Default: time-optimal path-following +client.set_profile("RUCKIG") # Default: jerk-limited point-to-point ``` **Available profiles** (`SETPROFILE`): | Profile | Description | |---------|-------------| -| **TOPPRA** | Time-optimal path-following (default) | -| **RUCKIG** | Jerk-limited point-to-point motion (joint moves only) | +| **RUCKIG** | Jerk-limited point-to-point motion (default; joint moves only) | +| **TOPPRA** | Time-optimal path-following | | **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 and SEPTIC are point-to-point only and cannot follow Cartesian paths. When either 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, and so does a move given a `duration` under RUCKIG. ### Speed and acceleration diff --git a/parol6/commands/system_commands.py b/parol6/commands/system_commands.py index 3596bf1..a4ae290 100644 --- a/parol6/commands/system_commands.py +++ b/parol6/commands/system_commands.py @@ -179,15 +179,16 @@ class SelectProfileCommand(SystemCommand[SelectProfileCmd]): Format: [CmdType.SELECT_PROFILE, profile_type] Profile Types: - TOPPRA - Time-optimal path parameterization (default) - RUCKIG - Time-optimal jerk-limited (point-to-point only, joint moves only) + RUCKIG - Time-optimal jerk-limited (default; point-to-point only, joint moves only) + TOPPRA - Time-optimal path parameterization 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 and SEPTIC are point-to-point and cannot follow Cartesian - paths. Cartesian moves will use TOPPRA when either is set. + paths. Cartesian moves will use TOPPRA when either is set, as does a + timed move under RUCKIG. """ PARAMS_TYPE = SelectProfileCmd diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index 320706c..0d3920d 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -326,8 +326,8 @@ def compute_constraint_params(self, path: _BlendedPath, gridpoints: NDArray): class ProfileType(Enum): """Available trajectory profile types for motion planning.""" - TOPPRA = "toppra" # Time-optimal path following (default) - RUCKIG = "ruckig" # Point-to-point jerk-limited (can't follow Cartesian paths) + RUCKIG = "ruckig" # Point-to-point jerk-limited (default; no Cartesian paths) + TOPPRA = "toppra" # Time-optimal path following QUINTIC = "quintic" # Quintic polynomial (C² smooth, predictable shape) SEPTIC = "septic" # Point-to-point septic polynomial (C³ smooth, jerk-limited) TRAPEZOID = "trapezoid" # Trapezoidal velocity profile @@ -868,6 +868,10 @@ def __init__( ): logger.warning("RUCKIG cannot follow Cartesian paths, using TOPPRA") self.profile = ProfileType.TOPPRA + # A timed move is a path stretched to a clock, which is TOPPRA's + # parameterization. + if self.profile == ProfileType.RUCKIG and duration: + self.profile = ProfileType.TOPPRA self.velocity_frac = velocity_frac self.accel_frac = accel_frac @@ -1524,8 +1528,6 @@ 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() - if self.duration is not None: - inp.minimum_duration = self.duration result = gen.calculate(inp, solution) if result not in (Result.Working, Result.Finished): diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index 8e12f9a..1ef51f4 100644 --- a/parol6/server/motion_planner.py +++ b/parol6/server/motion_planner.py @@ -191,7 +191,7 @@ class PlannerState: # snapshot) and predicts a queued HOME; a pessimistic default would refuse # dry runs that were seeded from an already-homed robot. Homed_in: np.ndarray = field(default_factory=lambda: np.ones(8, dtype=np.uint8)) - motion_profile: str = "TOPPRA" + motion_profile: str = "RUCKIG" current_tool: str = "NONE" current_tool_variant: str = "" tcp_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0) diff --git a/parol6/server/state.py b/parol6/server/state.py index a751192..8f4be34 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -177,7 +177,7 @@ class ControllerState: # 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" + motion_profile: str = "RUCKIG" # Streaming executors for online motion (jogging/streaming) streaming_executor: StreamingExecutor = field( @@ -465,7 +465,7 @@ def reset(self) -> None: self.execution_paused = False self.execution_speed = 1.0 self.soft_error = False - self.motion_profile = "TOPPRA" + self.motion_profile = "RUCKIG" # Tool back to none self._current_tool = "NONE" diff --git a/tests/integration/test_profile_commands.py b/tests/integration/test_profile_commands.py index 5c66e1b..f8243cc 100644 --- a/tests/integration/test_profile_commands.py +++ b/tests/integration/test_profile_commands.py @@ -21,11 +21,11 @@ class TestProfileCommands: """Test motion profile get/set commands.""" def test_profile_returns_default(self, client, server_proc): - """Test GETPROFILE returns default profile (TOPPRA) after reset.""" + """Test GETPROFILE returns default profile (RUCKIG) after reset.""" client.reset_state() result = client.profile() assert result is not None - assert result == "TOPPRA" + assert result == "RUCKIG" def test_select_and_get_profile_roundtrip(self, client, server_proc): """Test setting a profile and getting it back.""" @@ -58,14 +58,14 @@ async def reported_profile(name: str) -> StatusBuffer: ) for profile in ProfileType: - if profile is ProfileType.TOPPRA: + if profile is ProfileType.RUCKIG: continue assert await writer.select_profile(profile.name) > 0 selected = await reported_profile(profile.name) assert selected.motion_profile == profile.name assert await writer.reset_state() > 0 - reset = await reported_profile("TOPPRA") - assert reset.motion_profile == "TOPPRA" + reset = await reported_profile("RUCKIG") + assert reset.motion_profile == "RUCKIG" @pytest.mark.integration @@ -194,6 +194,39 @@ def holds_the_move(status) -> bool: f"{planned[-1]:.2f} s" ) + def test_the_startup_profile_is_jerk_limited_and_a_timed_move_is_toppra_timed( + self, + ): + """A move_j under the startup profile is RUCKIG's jerk-limited shape, + and a timed one under RUCKIG is TOPPRA's: asked for less than the + optimum, it takes TOPPRA's acceleration-bound minimum.""" + 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] + target = [40.0, -60.0, 200.0, 30.0, 20.0, 150.0] + + def previewed(profile: str | None, duration: float = 0.0) -> int: + preview = DryRunRobotClient(initial_joints_deg=standby) + if profile is not None: + assert preview.select_profile(profile) == 1 + if duration: + index = preview.move_j(target, duration=duration, accel=1.0) + else: + index = preview.move_j(target, speed=1.0, accel=1.0) + record = preview.plan() + assert record.blocks[index].error is None + return record.blocks[index].rows + + ruckig, toppra = previewed("RUCKIG"), previewed("TOPPRA") + assert ruckig > toppra, "the move does not tell RUCKIG from TOPPRA" + assert previewed(None) == ruckig, ( + f"the startup profile ran {previewed(None)} rows, RUCKIG {ruckig}" + ) + assert previewed("RUCKIG", 0.1) == previewed("TOPPRA", 0.1) == toppra + assert previewed(None, 0.1) == toppra + 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 diff --git a/tests/integration/test_reset_semantics.py b/tests/integration/test_reset_semantics.py index 930db1e..5d7c402 100644 --- a/tests/integration/test_reset_semantics.py +++ b/tests/integration/test_reset_semantics.py @@ -59,7 +59,7 @@ def test_reset_state_keeps_the_protective_stop_outputs_and_homed( output = client.write_io(0, 1) assert output >= 0 and client.wait_command(output, timeout=5.0) - assert client.select_profile("RUCKIG") == 1 + assert client.select_profile("TOPPRA") == 1 assert client.set_execution_speed(0.5) == 1 try: assert client.estop() == 1 @@ -72,7 +72,7 @@ def test_reset_state_keeps_the_protective_stop_outputs_and_homed( assert client.wait_status(lambda s: s.homed and not s.enabled, timeout=2.0) io = client.io() assert io is not None and io[2] == 1, f"reset_state changed the outputs: {io}" - assert client.profile() == "TOPPRA" + assert client.profile() == "RUCKIG" assert client.execution_speed().target_scale == 1.0 assert client.reset() == 1 diff --git a/tests/unit/test_dry_run_blend.py b/tests/unit/test_dry_run_blend.py index 80b8a6a..0068342 100644 --- a/tests/unit/test_dry_run_blend.py +++ b/tests/unit/test_dry_run_blend.py @@ -56,8 +56,11 @@ def test_empty_program_is_an_empty_record(self, client): assert record.rows == 0 and record.blocks == () 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. + # W1 and W2 lie on the joint-space line from the start to W3. A + # blended chain is timed along its path, so the one move is too. single = DryRunRobotClient() + for robot in (single, client): + assert robot.select_profile("TOPPRA") == 1 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) diff --git a/tests/unit/test_reset_command.py b/tests/unit/test_reset_command.py index e3d6b25..1d5eb68 100644 --- a/tests/unit/test_reset_command.py +++ b/tests/unit/test_reset_command.py @@ -28,7 +28,7 @@ def test_reset_restores_program_state_and_keeps_the_physical(self): state.soft_error = True state.execution_paused = True state.execution_speed = 0.4 - state.motion_profile = "RUCKIG" + state.motion_profile = "TOPPRA" state._current_tool = "GRIPPER" cmd = ResetStateCommand(ResetStateCmd()) @@ -51,7 +51,7 @@ def test_reset_restores_program_state_and_keeps_the_physical(self): assert state.error is None assert not state.execution_paused assert state.execution_speed == 1.0 - assert state.motion_profile == "TOPPRA" + assert state.motion_profile == "RUCKIG" assert state._current_tool == "NONE" def test_reset_preserves_connection_state(self): From 81f5fbe69e8e21f7d34cd24b64e410811702c2a3 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Wed, 4 Nov 2026 14:40:59 +0000 Subject: [PATCH 2/2] Smooth TOPPRA's timing to the jerk limits TOPPRA's path acceleration is bang-bang. A move it times (any cartesian move or blended chain under RUCKIG or TOPPRA, and a timed move under RUCKIG) now has its path position averaged over the shortest window that ramps each joint's acceleration steps within its jerk limit: only the timing along the path changes, never the path, as in par6, where those steps reached the drives as torque steps through the inertia feedforward. Averaged, a bend TOPPRA slowed for would be crossed at the window's mean speed, so the path speed is capped at the lowest of the maximum-velocity curve within half a window of each point; and since the averaged path acceleration is spent where the joints answer it differently, up to three more solves lower the acceleration limits where the average still passes them. The segment pass takes any remainder. The jerk limit costs time: a timed wrist turn now needs more than the 2 s its test asked for, and a spline's bend slows to 37 mm/s, not under 40. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_016uVshJDKTz7nCqrMGVxVJU --- parol6/motion/trajectory.py | 251 ++++++++++++++++++++- tests/integration/test_profile_commands.py | 35 ++- tests/unit/test_dry_run_record.py | 4 +- tests/unit/test_motion.py | 3 +- 4 files changed, 272 insertions(+), 21 deletions(-) diff --git a/parol6/motion/trajectory.py b/parol6/motion/trajectory.py index 0d3920d..9689a01 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -139,6 +139,141 @@ def _tick_intervals(duration: float, dt: float) -> int: _RATE_TOLERANCE = 1e-9 +def _max_path_speed( + path: _BlendedPath, + s: NDArray[np.float64], + v_max: NDArray[np.float64], + a_max: NDArray[np.float64], +) -> NDArray[np.float64]: + """The maximum-velocity curve at path positions ``s``: every joint under + its velocity limit, and some path acceleration keeping every joint's + ``q'·u + q''·sd²`` inside its acceleration limit. Each joint confines + ``u`` to an interval that shrinks as ``sd²`` grows; they share a point + while every pair overlaps, and a pair stops overlapping past + ``(a_j·|q'_k| + a_k·|q'_j|) / |q''_j·q'_k − q''_k·q'_j|``. A joint the + path does not move is bounded by its curvature alone.""" + signed, d2 = path(s, 1), path(s, 2) + d1 = np.abs(signed) + tiny = 1e-12 + moving = d1 > tiny + with np.errstate(divide="ignore", invalid="ignore"): + sd = np.where(moving, v_max / d1, np.inf).min(axis=1) + x = np.where(~moving & (np.abs(d2) > tiny), a_max / np.abs(d2), np.inf).min( + axis=1 + ) + j, k = np.triu_indices(len(v_max), 1) + den = np.abs(d2[:, j] * signed[:, k] - d2[:, k] * signed[:, j]) + num = a_max[j] * d1[:, k] + a_max[k] * d1[:, j] + pairs = np.where(den > tiny, num / den, np.inf).min(axis=1) + return np.minimum(sd, np.sqrt(np.minimum(x, pairs))) + + +def _window_min( + values: NDArray[np.float64], times: NDArray[np.float64], reach: float +) -> NDArray[np.float64]: + """At each point, the lowest of ``values`` within ``reach`` of its time + either way.""" + near = np.abs(times[:, None] - times[None, :]) <= reach + return np.where(near, values[None, :], np.inf).min(axis=1) + + +class _PathClock: + """TOPP-RA's path timing as constant-acceleration segments between its + grid points, built from the solved grid speeds the way toppra's + ParametrizeConstAccel builds them.""" + + def __init__(self, gridpoints: NDArray, speeds: NDArray) -> None: + self.s = np.asarray(gridpoints, dtype=np.float64) + self.v = np.maximum(np.asarray(speeds, dtype=np.float64), 0.0) + ds = np.diff(self.s) + self.u = 0.5 * np.diff(self.v**2) / ds + self.t = np.concatenate( + ([0.0], np.cumsum(2.0 * ds / (self.v[:-1] + self.v[1:]))) + ) + d = np.diff(self.t) + self._area = np.concatenate( + ( + [0.0], + np.cumsum( + self.s[:-1] * d + 0.5 * self.v[:-1] * d**2 + self.u * d**3 / 6.0 + ), + ) + ) + + def _at(self, t: NDArray) -> tuple[NDArray, NDArray, NDArray]: + """Segment index, time into it, and whether ``t`` is before the + start or after the end.""" + end = self.t[-1] + k = np.clip(np.searchsorted(self.t, t, side="right") - 1, 0, len(self.u) - 1) + return k, np.clip(t, 0.0, end) - self.t[k], (t <= 0.0) | (t >= end) + + def _position_speed(self, t: NDArray) -> tuple[NDArray, NDArray]: + """The path position and speed at ``t``, at rest outside the + profile.""" + k, d, outside = self._at(t) + s = self.s[k] + self.v[k] * d + 0.5 * self.u[k] * d**2 + s = np.where(t <= 0.0, self.s[0], np.where(t >= self.t[-1], self.s[-1], s)) + return s, np.where(outside, 0.0, self.v[k] + self.u[k] * d) + + def _position_integral(self, t: NDArray) -> NDArray: + """The path position integrated from 0 to ``t``, held at rest + before the start and after the end.""" + end = self.t[-1] + k, d, _ = self._at(t) + inside = ( + self._area[k] + + self.s[k] * d + + 0.5 * self.v[k] * d**2 + + self.u[k] * d**3 / 6.0 + ) + after = self._area[-1] + self.s[-1] * (t - end) + return np.where(t <= 0.0, self.s[0] * t, np.where(t >= end, after, inside)) + + def averaged( + self, times: NDArray, window: float + ) -> tuple[NDArray, NDArray, NDArray]: + """The path position averaged over the trailing ``window``, with its + speed and acceleration: the differences of the position and speed + across the window over its width, so each step in TOPP-RA's path + acceleration becomes a ramp that wide. Only the timing along the + path changes, never the path.""" + integral = self._position_integral + s = (integral(times) - integral(times - window)) / window + s_now, v_now = self._position_speed(times) + s_then, v_then = self._position_speed(times - window) + return ( + np.clip(s, self.s[0], self.s[-1]), + (s_now - s_then) / window, + (v_now - v_then) / window, + ) + + def window_span(self, times: NDArray, window: float) -> tuple[NDArray, NDArray]: + """The stretch of path the average at ``times`` draws on.""" + return self._position_speed(times - window)[0], self._position_speed(times)[0] + + +class _ScaledJointAcceleration(constraint.LinearConstraint): + """Joint acceleration limits scaled per gridpoint.""" + + def __init__(self, limit: NDArray, scale: NDArray) -> None: + super().__init__() + self.limit = np.asarray(limit, dtype=np.float64) + self.scale = np.asarray(scale, dtype=np.float64) + self.dof = len(self.limit) + self.identical = False + self.set_discretization_type(constraint.DiscretizationType.Interpolation) + + def compute_constraint_params(self, path: _BlendedPath, gridpoints: NDArray): + ps, pss = path(gridpoints, 1), path(gridpoints, 2) + eye = np.eye(self.dof) + F = np.tile(np.vstack((eye, -eye)), (len(gridpoints), 1, 1)) + bound = self.scale[:, None] * self.limit[None, :] + g = np.hstack((bound, bound)) + return constraint.canlinear_colloc_to_interpolate( + ps, pss, np.zeros_like(ps), F, g, None, None, gridpoints + ) + + 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( @@ -1067,21 +1202,25 @@ def _build_with_prefix(self) -> Trajectory: def _build_toppra_trajectory(self) -> Trajectory: """ - Build trajectory using TOPP-RA's time-optimal path parameterization. + Build trajectory using TOPP-RA's time-optimal path parameterization, + its timing smoothed to the jerk limits. Bounded corner blends expose curvature to the timing solver while preserving the waypoint parameter and Cartesian velocity limits. """ - return self._time_path(self._blended_path()) + return self._time_path(self._blended_path(), smoothed=True) def _time_path( self, path: _BlendedPath, profile_speed: Callable[[NDArray], NDArray] | None = None, unaccelerated_ends: bool = False, + smoothed: bool = False, ) -> Trajectory: """Time the blended path within its speed envelope, optionally easing the - clock into and out of rest with zero endpoint acceleration.""" + clock into and out of rest with zero endpoint acceleration, or + averaging the path position over a window that ramps every step in + TOPP-RA's acceleration within the jerk limits.""" positions = self.joint_path.positions gridpoints = self._gridpoints(path) joint_vel_constraint = constraint.JointVelocityConstraint(self._vlim) @@ -1098,7 +1237,9 @@ def _time_path( if profile_speed is not None: constraints.append(_PathSpeedConstraint(profile_speed(gridpoints))) - try: + def solve( + constraints: list[constraint.Constraint], + ) -> tuple[algo.TOPPRA, ParametrizeConstAccel, float]: instance = algo.TOPPRA( constraints, path, @@ -1106,24 +1247,63 @@ def _time_path( 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") + return instance, jnt_traj, duration + try: + instance, jnt_traj, duration = solve(constraints) logger.debug( "TrajectoryBuilder: TOPP-RA duration=%.3f, path_len=%d", duration, len(positions), ) + window = self._smoothing_window(jnt_traj, duration) if smoothed else 0.0 + if window: + # Averaged, a narrow dip TOPP-RA takes through a bend would be + # carried at the window's mean speed. The average lands where + # the path stood half a window earlier, drawing on the speeds + # half a window either side, so capping the path speed at the + # lowest limit within half a window of each point widens the + # dip instead; the maximum-velocity curve leaves out the + # rest-to-rest ramps, slow for the boundary, not the path. + data = instance.problem_data + first = _PathClock(data.gridpoints, data.sd_vec) + limit = _max_path_speed(path, data.gridpoints, self.v_max, self.a_max) + cap = _PathSpeedConstraint(_window_min(limit, first.t, 0.5 * window)) + others = [c for c in constraints if c is not joint_acc_constraint] + scale = np.ones(len(data.gridpoints)) + # Averaged over the window, the path acceleration TOPP-RA + # held at each point is spent where the joints answer it + # differently, and along a path whose joint ratios change + # quickly (near a wrist singularity, through a blend) that + # can pass a limit TOPP-RA held. Each round lowers the + # limits on the stretch the average drew from by its + # excess; the segment pass after planning takes what is left. + for round_ in range(4): + instance, jnt_traj, duration = solve( + [*others, _ScaledJointAcceleration(self.a_max, scale), cap] + ) + window = self._smoothing_window(jnt_traj, duration) + if round_ == 3: + break + data = instance.problem_data + factor = self._acceleration_excess( + _PathClock(data.gridpoints, data.sd_vec), path, duration, window + ) + if factor is None: + break + scale = scale / factor + 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) + span = duration + ramp + window + intervals = _tick_intervals(span, self.dt) + times = np.arange(intervals) * (span / intervals) if ramp: # A C2 clock ramp removes endpoint acceleration without slowing cruise. distance = np.minimum(times, duration + ramp - times) @@ -1135,7 +1315,12 @@ def _time_path( 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) + if window: + data = instance.problem_data + clock = _PathClock(data.gridpoints, data.sd_vec) + trajectory_rad[:-1] = path(clock.averaged(times, window)[0]) + else: + trajectory_rad[:-1] = jnt_traj(times) trajectory_rad[-1] = positions[-1] if not np.all(np.isfinite(trajectory_rad)): raise RuntimeError("TOPP-RA returned non-finite joint positions") @@ -1156,6 +1341,54 @@ def _time_path( make_error(ErrorCode.TRAJ_NO_STEPS, detail=f"TOPPRA failed: {e}") ) from e + def _smoothing_window( + self, jnt_traj: ParametrizeConstAccel, duration: float + ) -> float: + """The shortest window the path position can be averaged over with + every joint's jerk inside its limit. Averaged over ``W``, a joint's + acceleration changes at ``(qdd(t) - qdd(t - W)) / W``, so ``W`` must + cover the largest change in ``qdd`` across any span that wide. A + move that cruises between its acceleration steps needs one step's + change, a bang-bang one that reverses within the window both; the + widths that fit are not one interval, so they are scanned from the + shortest up.""" + samples = 2000 + h = duration / samples + qdd = jnt_traj(np.linspace(0.0, duration, samples + 1), order=2) + swing = np.maximum(qdd.max(axis=0), 0.0) - np.minimum(qdd.min(axis=0), 0.0) + longest = float( + np.divide( + swing, self.j_max, out=np.zeros_like(swing), where=self.j_max > 0 + ).max() + ) + if longest <= 0.0: + return 0.0 + for m in range(1, math.ceil(longest / h) + 1): + rest = np.zeros((m, qdd.shape[1])) + padded = np.vstack((rest, qdd, rest)) + change = np.abs(padded[m:] - padded[:-m]).max(axis=0) + if np.all(change <= self.j_max * m * h): + return m * h + return longest + + def _acceleration_excess( + self, clock: _PathClock, path: _BlendedPath, duration: float, window: float + ) -> NDArray | None: + """By how much each gridpoint's acceleration limits must come down + for the average over ``window`` to stay inside them: the worst + excess of any sample whose average draws on it. ``None`` when no + sample is over.""" + times = np.linspace(0.0, duration + window, 2001) + s, v, u = clock.averaged(times, window) + qdd = path(s, 2) * v[:, None] ** 2 + path(s, 1) * u[:, None] + ratio = (np.abs(qdd) / self.a_max).max(axis=1) + over = ratio > 1.0 + 1e-6 + if not over.any(): + return None + lo, hi = clock.window_span(times[over], window) + drawn = (clock.s[None, :] >= lo[:, None]) & (clock.s[None, :] <= hi[:, None]) + return np.where(drawn, ratio[over][:, None], 1.0).max(axis=0) + def _build_simple_trajectory(self) -> Trajectory: """A trapezoidal path-speed envelope, slowed ahead of bends as needed.""" path = self._blended_path() diff --git a/tests/integration/test_profile_commands.py b/tests/integration/test_profile_commands.py index f8243cc..6b742af 100644 --- a/tests/integration/test_profile_commands.py +++ b/tests/integration/test_profile_commands.py @@ -199,15 +199,20 @@ def test_the_startup_profile_is_jerk_limited_and_a_timed_move_is_toppra_timed( ): """A move_j under the startup profile is RUCKIG's jerk-limited shape, and a timed one under RUCKIG is TOPPRA's: asked for less than the - optimum, it takes TOPPRA's acceleration-bound minimum.""" + optimum, it takes TOPPRA's minimum. TOPPRA's timing is smoothed to + the jerk limits: its own acceleration is bang-bang, a step a torque + feedforward turns into a torque step.""" from parol6.client.dry_run_client import DryRunRobotClient + from parol6.config import LIMITS import parol6.PAROL6_ROBOT as PAROL6_ROBOT standby = [float(v) for v in PAROL6_ROBOT.joint.standby_deg] target = [40.0, -60.0, 200.0, 30.0, 20.0, 150.0] + dt = 0.0 - def previewed(profile: str | None, duration: float = 0.0) -> int: + def previewed(profile: str | None, duration: float = 0.0) -> np.ndarray: + nonlocal dt preview = DryRunRobotClient(initial_joints_deg=standby) if profile is not None: assert preview.select_profile(profile) == 1 @@ -216,16 +221,28 @@ def previewed(profile: str | None, duration: float = 0.0) -> int: else: index = preview.move_j(target, speed=1.0, accel=1.0) record = preview.plan() - assert record.blocks[index].error is None - return record.blocks[index].rows + block = record.blocks[index] + assert block.error is None + dt = record.row_dt_s + rows = record.joints_rad[block.start_row : block.start_row + block.rows] + return np.asarray(rows, dtype=np.float64) ruckig, toppra = previewed("RUCKIG"), previewed("TOPPRA") - assert ruckig > toppra, "the move does not tell RUCKIG from TOPPRA" - assert previewed(None) == ruckig, ( - f"the startup profile ran {previewed(None)} rows, RUCKIG {ruckig}" + assert len(ruckig) != len(toppra), "the move does not tell RUCKIG from TOPPRA" + assert len(previewed(None)) == len(ruckig), ( + f"the startup profile ran {len(previewed(None))} rows, RUCKIG {len(ruckig)}" + ) + assert len(previewed("RUCKIG", 0.1)) == len(previewed("TOPPRA", 0.1)) + assert len(previewed(None, 0.1)) == len(toppra) + + # From rest to rest, the third difference of the rows is their jerk + # averaged over three rows. + rest = np.vstack([toppra[:1]] * 3 + [toppra] + [toppra[-1:]] * 3) + jerk = np.abs(np.diff(rest, 3, axis=0)).max(axis=0) / dt**3 + limit = LIMITS.joint.hard.jerk + assert np.all(jerk <= 1.05 * limit), ( + f"TOPPRA commanded {np.round(jerk / limit, 2)} of the jerk limits" ) - assert previewed("RUCKIG", 0.1) == previewed("TOPPRA", 0.1) == toppra - assert previewed(None, 0.1) == toppra def test_a_septic_cartesian_move_is_timed_along_its_path(self): """SEPTIC times a straight joint run only. From standby the wrist diff --git a/tests/unit/test_dry_run_record.py b/tests/unit/test_dry_run_record.py index 2735a8e..288faa1 100644 --- a/tests/unit/test_dry_run_record.py +++ b/tests/unit/test_dry_run_record.py @@ -130,9 +130,9 @@ def test_a_move_that_starts_with_a_wrist_turn_takes_the_duration_it_names(): """From standby a tool-frame reorientation first turns the wrist out of its singularity; a duration names the whole move, turn included.""" client = DryRunRobotClient(initial_joints_deg=HOME) - index = client.move_l([0.0, 0.0, 0.0, -15.0, 0.0, 0.0], frame="TRF", duration=2.0) + index = client.move_l([0.0, 0.0, 0.0, -15.0, 0.0, 0.0], frame="TRF", duration=3.0) record = client.plan() block = record.blocks[index] assert block.error is None, block.error # The block's first row is the pose the move starts from, at t = 0. - assert (block.rows - 1) * record.row_dt_s == pytest.approx(2.0, abs=record.row_dt_s) + assert (block.rows - 1) * record.row_dt_s == pytest.approx(3.0, abs=record.row_dt_s) diff --git a/tests/unit/test_motion.py b/tests/unit/test_motion.py index a2bee56..b4f2bdd 100644 --- a/tests/unit/test_motion.py +++ b/tests/unit/test_motion.py @@ -447,7 +447,8 @@ def test_spline_orientation_change_does_not_stop_at_waypoint(self): 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 + # Jerk-limited, the bend costs speed, but nowhere near a stop. + assert middle.min() > 30.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)