Add batched trapezoidal and Double-S trajectory planning - #566
Conversation
 |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
There was a problem hiding this comment.
Pull request overview
This PR introduces a new batched trajectory-planning capability (trapezoidal and jerk-limited Double‑S profiles) with Torch and Warp backends, integrates it into the existing MotionGenerator planner registry, and extends Cartesian straight-line planning with continuous batched IK path solving (notably for OPW via Warp).
Changes:
- Add
TrapezoidalPlanner(+ Warp backend) with constraint projection, synchronized multi-joint timing, and sampling (fixed-count / fixed-time). - Add continuous batched Cartesian path IK support (
Robot.compute_ik_path()andOPWSolver.get_ik_path()), plus tutorial + benchmark scripts. - Add comprehensive tests and update docs + agent context references.
Reviewed changes
Copilot reviewed 19 out of 19 changed files in this pull request and generated 4 comments.
Show a summary per file
| File | Description |
|---|---|
| tests/utils/test_opw_path_kernel.py | Adds a unit test validating OPW Warp path branch continuity selection. |
| tests/sim/planners/test_trapezoidal_planner.py | Adds extensive correctness/golden tests for trapezoidal and Double‑S planning across Torch/Warp. |
| tests/lab/scripts/test_trapezoidal_planner_tutorial.py | Tests tutorial utilities (Cartesian line planning, derivatives, plotting, replay behavior). |
| scripts/tutorials/sim/trapezoidal_profile.py | Adds a minimal scalar profile plotting example (no simulation). |
| scripts/tutorials/sim/trapezoidal_planner.py | Adds a full tutorial: joint + Cartesian planning, plotting diagnostics, and replay timing. |
| scripts/benchmark/motion_generation/trapezoidal_planner.py | Adds a reproducible Torch/Warp benchmark harness and markdown report output. |
| embodichain/utils/warp/kinematics/opw_solver.py | Adds a Warp kernel to select a temporally continuous OPW IK branch over a pose path. |
| embodichain/lab/sim/solvers/opw_solver.py | Adds OPWSolver.get_ik_path() to solve and continuously select an entire pose path in Warp. |
| embodichain/lab/sim/planners/trapezoidal_warp.py | Implements Warp profile construction and sampling kernels for the trapezoidal planner. |
| embodichain/lab/sim/planners/trapezoidal_planner.py | Adds the core batched trapezoidal/Double‑S planner (Torch reference + Warp dispatch). |
| embodichain/lab/sim/planners/motion_generator.py | Registers the new planner type and adds plan-options resolution for trapezoidal sampling. |
| embodichain/lab/sim/planners/init.py | Exports the new planner from the planners package. |
| embodichain/lab/sim/objects/robot.py | Adds Robot.compute_ik_path() for continuous pose-path IK through solver interfaces. |
| docs/source/overview/sim/solvers/opw_solver.md | Documents OPW whole-path IK behavior and kernels. |
| docs/source/api_reference/public_api.rst | Adds trapezoidal planner module to the public API autosummary list. |
| docs/source/api_reference/embodichain/embodichain.lab.sim.planners.rst | Adds API docs section and usage example for TrapezoidalPlanner. |
| agent_context/topics/motion-planning/motion-planning.md | Updates project context docs with the new planner and tutorial details. |
| agent_context/topics/ik-solvers/ik-solvers.md | Documents OPW whole-path IK behavior in the IK solvers overview. |
| agent_context/MAP.yaml | Extends motion-planning topic source-of-truth list with new modules/scripts. |
💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 19 out of 19 changed files in this pull request and generated no new comments.
Suppressed comments (1)
Previously missed (1) — in code that hasn't changed since the last review.
embodichain/lab/sim/objects/robot.py:1117
compute_ik_path()validates only the batch dimension/ndim forpose_tensor, but not the trailing matrix shape(4, 4)it promises in the docstring/error message. This can allow invalid inputs through and then fail later insidetorch.matmul/torch.inversewith a less clear error.
if pose_tensor.ndim != 4 or pose_tensor.shape[0] != batch_size:
raise ValueError("pose must have shape (B, N, 4, 4).")
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 19 out of 19 changed files in this pull request and generated no new comments.
Suppressed comments (3)
Previously missed (2) — in code that hasn't changed since the last review.
embodichain/utils/warp/kinematics/opw_solver.py:533
- The kernel loops are hard-coded to 8 candidates and 6 joints (see
for candidate in range(8)/for joint in range(6)), but these parameter comments suggest a variableN_SOL/DOF. Please update the comments to reflect the fixed OPW sizes to avoid misleading future callers.
This issue also appears on line 537 of the same file.
full_ik_result: wp.array(dtype=float, ndim=4), # [B, N, N_SOL, DOF]
full_ik_valid: wp.array(dtype=int, ndim=3), # [B, N, N_SOL]
initial_seed: wp.array(dtype=float, ndim=2), # [B, DOF]
embodichain/lab/sim/planners/trapezoidal_warp.py:347
compose_profile_samples_warp()validates dtypes but not that all input tensors share the same device. If a caller accidentally mixes CPU/CUDA tensors, Warp will fail later with a less actionable error; adding an explicit device check here makes the public helper more robust (and matches the checks inbuild_profile_warp()).
)
if any(tensor.dtype != torch.float32 for tensor in tensors):
raise ValueError("The Warp trajectory backend requires float32 tensors.")
batch_size, sample_count = times.shape
embodichain/utils/warp/kinematics/opw_solver.py:538
path_resultis always 6-DOF (the kernel useswp_vec6flimits/weights and loopsfor joint in range(6)), but the comment saysDOF. Align this with the fixed OPW DOF to keep the signature self-documenting.
path_result: wp.array(dtype=float, ndim=3), # [B, N, DOF]
path_valid: wp.array(dtype=int, ndim=2), # [B, N]
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 19 out of 19 changed files in this pull request and generated no new comments.
Suppressed comments (1)
Previously missed (1) — in code that hasn't changed since the last review.
embodichain/lab/sim/objects/robot.py:1117
compute_ik_path()validates only the batch dimension and rank forpose, but not the trailing matrix shape. If a caller passes(B, N, 7)or(B, N, 3, 4)the error message claims(B, N, 4, 4)yet the check passes and the subsequentmatmul/inversewill fail with a less clear runtime error. Tighten the shape check to includepose_tensor.shape[-2:] == (4, 4).
if pose_tensor.ndim != 4 or pose_tensor.shape[0] != batch_size:
raise ValueError("pose must have shape (B, N, 4, 4).")
Review against the current
|
# Conflicts: # tests/sim/objects/test_robot.py
yuecideng
left a comment
yuecideng
left a comment
There was a problem hiding this comment.
Focused follow-up on the latest head. The three actionable findings and one architecture suggestion are attached inline.
Bound blended velocity, acceleration and jerk before output sampling, and share typed scalar time laws between joint and SE(3) planning. Preserve constraint reports through MotionGenerator and invalidate changed results. Validate SE(3) timing options and stabilize small-angle calculations. Declare continuous batch IK support in BaseSolver and check it before generating OPW candidates. Reuse Bezier arc-length parameters for positions and derivatives while eliminating redundant dense sampling. Add regression coverage and update API docs and project context. Validation: 196 focused tests passed; Black checked all 848 Python files; API coverage is complete, with no new Sphinx diagnostics.
# Conflicts: # embodichain/lab/sim/motion/motion_generator.py
Description
This PR adds a batched trajectory planner supporting trapezoidal velocity and jerk-limited seven-phase Double-S motion profiles.
The planner supports both Torch and NVIDIA Warp backends and integrates with the existing MotionGenerator interface. The Warp backend parallelizes scalar profile construction, phase evaluation, and batched joint trajectory
composition.
It also adds Cartesian straight-line trajectory planning for the simulation tutorial. Cartesian distance is time-parameterized before IK, ensuring the requested end-effector path follows the configured velocity, acceleration, and
jerk constraints.
For OPW-based robots, the complete Cartesian pose path is now solved through a batched Warp path-IK interface:
This avoids launching OPW IK separately for every trajectory sample while preserving joint-path continuity.
Additional changes include:
Add TrapezoidalPlanner, TrapezoidalPlannerCfg, and TrapezoidalPlanOptions.
Support scalar and per-joint velocity, acceleration, and jerk limits.
Support synchronized multi-joint motion.
Support triangular fallback for short trapezoidal moves.
Support seven-phase Double-S profiles without display filtering.
Support fixed-count and fixed-time trajectory sampling.
Support optional minimum-duration scaling.
Support redundant collinear waypoint compression.
Add explicit torch, warp, and auto backend selection.
Add continuous Cartesian path IK through Robot.compute_ik_path().
Add batched OPW path solving through OPWSolver.get_ik_path().
Compute Cartesian joint velocity and acceleration using the path time law and differential kinematics rather than numerical time differentiation.
Add simulation replay using the planned trajectory timing.
Add diagnostic plots for:
Add standalone profile and simulation tutorials.
Add a reproducible Torch/Warp trajectory-planner benchmark.
Update motion-planning, solver, and API documentation.
Motivation and context:
The existing trajectory-planning examples primarily relied on TOPPRA and did not provide a lightweight, natively batched implementation for trapezoidal or jerk-limited motion profiles.
Cartesian trajectories also require the time law to be applied in Cartesian space before IK. Applying joint-space timing after independently sampled IK can distort the requested end-effector path and produce inconsistent
derivatives.
This change provides a maintainable trajectory-planning implementation suitable for both CPU execution and large batched CUDA workloads, while keeping Torch as the reference and fallback backend.
Dependencies:
Fixes #
Type of change
Screenshots
• ## Examples
The examples below cover single-trajectory planning, batched planning, full CobotMagic integration, numerical validation, and plot generation.
Single Cartesian line trajectory
Run a jerk-limited Double-S Cartesian line trajectory for one CobotMagic environment:
python scripts/tutorials/sim/planner/trapezoidal_planner.py \ --num_envs 1 \ --path cartesian \ --cartesian-path line \ --profile acceleration_trapezoidal \ --samples 300 \ --plot-output outputs/cobotmagic_single_line.pngtrajectory_1-2026-09-04_17.38.49.mp4
Single Cartesian Bézier trajectory
python scripts/tutorials/sim/planner/trapezoidal_planner.py \ --num_envs 1 \ --path cartesian \ --cartesian-path bezier \ --profile acceleration_trapezoidal \ --samples 300 \ --plot-output outputs/cobotmagic_single_bezier.pngtrajectory_2-2026-09-04_17.44.07.mp4
Single multi-waypoint joint trajectory
Run a joint-space trajectory with jerk-limited timing and quintic corner blending:
python scripts/tutorials/sim/planner/trapezoidal_planner.py \ --num_envs 1 \ --path joint \ --profile acceleration_trapezoidal \ --blend-tolerance 0.05 \ --samples 300 \ --plot-output outputs/cobotmagic_single_joint_blend.pngtrajectory_3-2026-09-04_17.40.50.mp4
Batched Cartesian trajectories
Plan Cartesian line trajectories for four environments:
python scripts/tutorials/sim/planner/trapezoidal_planner.py \ --num_envs 4 \ --path cartesian \ --cartesian-path line \ --profile acceleration_trapezoidal \ --samples 300 \ --plot-env 0 \ --plot-output outputs/cobotmagic_batch_line.pngtrajectory_line-2026-09-04_17.46.59.mp4
Batched multi-waypoint joint trajectories
python scripts/tutorials/sim/planner/trapezoidal_planner.py \ --num_envs 4 \ --path joint \ --profile acceleration_trapezoidal \ --blend-tolerance 0.05 \ --samples 300 \ --plot-env 0 \ --plot-output outputs/cobotmagic_batch_joint_blend.pngFull CobotMagic integration
Run both joint-space and Cartesian trajectories with both supported time profiles:
python scripts/tutorials/sim/planner/trapezoidal_planner.py \ --num_envs 4 \ --path both \ --profile both \ --cartesian-path line \ --blend-tolerance 0.05 \ --minimum-duration 3.0 \ --samples 300 \ --plot-env 0 \ --plot-output outputs/cobotmagic_full_line.pngThis exercises:
To exercise the Cartesian Bézier path instead, replace:
--cartesian-path line
with:
--cartesian-path bezier
For headless execution, add:
--headless --no-show-plot
For example:
python scripts/tutorials/sim/planner/trapezoidal_planner.py \ --headless \ --no-show-plot \ --num_envs 8 \ --path both \ --profile both \ --cartesian-path line \ --blend-tolerance 0.05 \ --minimum-duration 3.0 \ --samples 300 \ --plot-output outputs/cobotmagic_headless.pngSimulation-free numerical validation
Run all numerical scenarios without starting the simulator:
The scenarios cover:
Each scenario prints [PASS] when all invariants hold and exits with a non-zero status on failure.
Plot generation
Generate all simulation-free diagnostic plots:
Plots are written to:
Generate and display one plot interactively:
python scripts/tutorials/sim/planner/trajectory_pr_plots.py blend \ --output outputs/trajectory_plots/blend.png \ --showFocused tests
pytest -q \ tests/sim/planners/test_bezier.py \ tests/sim/planners/test_trapezoidal_planner.py \ tests/sim/planners/test_se3.py \ tests/sim/planners/test_motion_generator.py \ tests/sim/planners/test_motion_generator_batched.pyDefault-planner direction
The reusable implementation is integrated through TrapezoidalPlanner and TrapezoidalPlannerCfg; the tutorial scripts are not production planner entry points.
For training and demonstration collection, the intended production defaults are:
This provides bounded jerk and deterministic batching while preserving exact waypoints. Quintic geometric blending remains opt-in because a non-zero blend tolerance intentionally permits waypoint deviation.
Validation
The following focused validation was completed: