Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
7 changes: 4 additions & 3 deletions parol6/commands/system_commands.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
261 changes: 248 additions & 13 deletions parol6/motion/trajectory.py

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion parol6/server/motion_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
4 changes: 2 additions & 2 deletions parol6/server/state.py
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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"
Expand Down
60 changes: 55 additions & 5 deletions tests/integration/test_profile_commands.py
Original file line number Diff line number Diff line change
Expand Up @@ -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."""
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -194,6 +194,56 @@ 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 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) -> np.ndarray:
nonlocal dt
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()
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 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"
)

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
Expand Down
4 changes: 2 additions & 2 deletions tests/integration/test_reset_semantics.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
5 changes: 4 additions & 1 deletion tests/unit/test_dry_run_blend.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
4 changes: 2 additions & 2 deletions tests/unit/test_dry_run_record.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
3 changes: 2 additions & 1 deletion tests/unit/test_motion.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
4 changes: 2 additions & 2 deletions tests/unit/test_reset_command.py
Original file line number Diff line number Diff line change
Expand Up @@ -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())
Expand All @@ -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):
Expand Down
Loading