Add batched trapezoidal and Double-S trajectory planning

[chase6305]

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:

1. One Warp launch computes all analytical OPW candidates for every Cartesian sample.
2. A second Warp launch selects a temporally continuous IK branch for each environment.
3. Each sample uses the previously selected joint state as its seed.
4. Joint-limit-valid 2π equivalent configurations are handled during branch selection.

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:

  • End-effector XYZ path
  • End-effector orientation
  • Joint position
  • Joint velocity
  • Joint acceleration

• 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:

• No new runtime dependencies.
• NVIDIA Warp is already used by EmbodiChain and is required when explicitly selecting backend="warp".
• Matplotlib is used by the existing tutorial visualization environment.

Fixes #

Type of change

• Bug fix (non-breaking change which fixes an issue)
• Enhancement (non-breaking change which improves an existing functionality)
• New feature (non-breaking change which adds functionality)
• Breaking change (existing functionality will not work without user modification)
• Documentation update

Screenshots

python scripts/tutorials/sim/planner/trapezoidal_planner.p--device cpu --num_envs 1 --backend torch --profile velocity_trapezoidal --path joint --samples 500 --plot-output outputs/pr/joint_trapezoidal.png

traj_joint_1-2026-09-02_14.44.00.mp4

python scripts/tutorials/sim/planner/trapezoidal_planner.py     --device cpu     --num_envs 1     --backend torch     --profile acceleration_trapezoidal     --path cartesian     --cartesian-distance 0.10     --cartesian-step 0.002     --cartesian-velocity 0.15     --cartesian-acceleration 0.30     --cartesian-jerk 1.0     --samples 500     --plot-output outputs/pr/cartesian_double_s.png

traj_cartesian_1-2026-09-02_14.46.48.mp4

Validation

The following focused validation was completed:

57 passed

Covered behavior includes:

- Torch and Warp scalar-profile construction consistency.
- Torch and Warp trajectory sampling consistency.
- Trapezoidal and Double-S boundary conditions.
- Stationary and short-distance segments.
- Velocity, acceleration, and jerk constraint enforcement.
- Cartesian straight-line geometry.
- Cartesian time-law propagation.
- Continuous OPW Warp branch selection.
- Batched tutorial planning and plotting behavior.
- Python compilation and formatting of affected files.
- git diff --check.

The OPW candidate and continuous-path selection kernels were compiled and tested with the CPU Warp backend. CUDA uses the same Warp kernels, but full GPU simulation validation should also run in CI or on a machine with an
available CUDA driver.

## Checklist

- [ ] I have run the black . command to format the code base.
- [x] I have formatted all affected Python files with Black.
- [x] I have made corresponding changes to the documentation.
- [ ] Public API changes are reflected in the API docs (python docs/scripts/check_api_docs.py), if applicable.
- [x] I have added tests that prove my fix is effective or that my feature works.
- [x] Dependencies have been reviewed; no dependency updates are required.

[greptile-apps]

Greptile Summary

The PR adds batched trapezoidal and Double-S trajectory planning across Torch and Warp, plus continuous OPW path IK and Cartesian trajectory tutorials. The latest periodic-equivalent fix can still classify a full-turn boundary clamp as continuous.

• Adds batched joint trajectory profile construction, sampling, waypoint compression, and MotionGenerator integration.
• Adds continuous whole-path OPW candidate selection and Cartesian-first trajectory generation.
• Adds planner, solver, tutorial, benchmark, documentation, and focused test coverage.

Confidence Score: 4/5

The PR is not yet safe to merge because continuous OPW selection can return a valid path containing an abrupt full-revolution joint step.

The replacement periodic-equivalent logic clamps an unavailable seed-nearest turn to a valid boundary and marks it continuous, while the robot API returns that path without any later jump guard.

Files Needing Attention: embodichain/utils/warp/kinematics/opw_solver.py, embodichain/lab/sim/objects/robot.py

Important Files Changed

Filename	Overview
embodichain/utils/warp/kinematics/opw_solver.py	Adds periodic-equivalent continuous branch selection, but the boundary clamp can preserve the previously reported full-revolution discontinuity.
embodichain/lab/sim/solvers/opw_solver.py	Adds the OPW continuous-path selection wrapper.
embodichain/lab/sim/objects/robot.py	Adds continuous batched IK dispatch and returns the selected OPW path without subsequent continuity validation.
embodichain/lab/sim/planners/trapezoidal_planner.py	Introduces batched trapezoidal and Double-S profile construction, sampling, constraints, and backend selection.
embodichain/utils/warp/kinematics/trapezoidal_warp.py	Implements Warp kernels for scalar profile construction, evaluation, and batched joint composition.
embodichain/lab/sim/planners/motion_generator.py	Registers the new planner and maps MotionGenerator limits and sampling options into its plan options.

Sequence Diagram

sequenceDiagram
  participant Robot
  participant OPW as OPW candidate generator
  participant Selector as Continuous selector
  Robot->>OPW: Generate all IK candidates
  OPW-->>Selector: Limit-valid analytical candidates
  loop Each path sample
    Selector->>Selector: Use previous selected state as seed
    Selector->>Selector: Find seed-nearest periodic turn
    Selector->>Selector: Clamp turn into valid interval
    Selector->>Selector: Select nearest candidate and commit
  end
  Selector-->>Robot: Per-sample validity and joint path

Loading

Prompt To Fix All With AI

### Issue 1
embodichain/utils/warp/kinematics/opw_solver.py:60-67
**Boundary clamp breaks continuity**

When the seed-nearest periodic turn lies outside the safety-adjusted joint range, `nearest_equivalent_in_limit` clamps it to a boundary turn and returns it as valid. The selector therefore marks an abrupt approximately 2π joint-position step as continuous, causing extreme commanded motion and invalid trajectory derivatives.

---

For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.

_{Reviews (10): Last reviewed commit: "fix: accept valid periodic OPW equivalen..." | Re-trigger Greptile}

[Copilot]

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() and OPWSolver.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.

[Copilot]

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 for pose_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 inside torch.matmul/torch.inverse with 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).")

[Copilot]

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 variable N_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 in build_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_result is always 6-DOF (the kernel uses wp_vec6f limits/weights and loops for joint in range(6)), but the comment says DOF. 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]

[Copilot]

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 for pose, 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 subsequent matmul/inverse will fail with a less clear runtime error. Tighten the shape check to include pose_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).")

[yuecideng]

Review against the current origin/main

Thanks for the substantial work in this PR. The new batched joint-space time-parameterization backend is useful, and the BasePlanner/MotionGenerator registration, explicit PlanResult.dt, and Torch/Warp split are directionally consistent with the existing motion-planning stack.

I reviewed PR head 0a2752bd against the current origin/main (aeca62f). The synthetic merge is clean, but I recommend requesting changes before merge because the current implementation has one P0 architecture blocker and three reproducible P1 trajectory-correctness issues.

What this PR adds

• TrapezoidalPlanner, TrapezoidalPlannerCfg, and TrapezoidalPlanOptions.
• Batched triangular/trapezoidal and seven-phase Double-S profiles.
• Scalar or per-joint velocity, acceleration, and jerk constraints.
• Torch reference implementation plus a float32 Warp backend for profile construction and sampling.
• Fixed-count/fixed-time sampling, minimum-duration scaling, and optional waypoint compression.
• A Cartesian straight-line tutorial using a Cartesian scalar time law, batched IK, differential kinematics, plotting, and replay.
• Tutorials, a benchmark, tests, API docs, and agent context updates.

Blocking findings

[P0] Reuse compute_batch_ik; do not add a second Path IK stack

The existing Robot.compute_batch_ik already owns the batch-IK boundary:

• accepts (B, N, 4, 4) or (B, N, 7) pose batches;
• converts arena-frame targets into the solver root frame;
• flattens (B, N) into one solver batch;
• calls solver.get_ik() once; and
• restores (B, N, DOF) results.

The existing OPWSolver.get_ik_warp also already computes all eight OPW candidates for an arbitrary target batch in one Warp launch and supports return_all_solutions=True.

The new Robot.compute_ik_path and OPWSolver.get_ik_path duplicate coordinate conversion, validation, flattening/reshaping, OPW candidate allocation, limit/TCP preparation, and the candidate kernel launch. The only genuinely new behavior is sequential candidate selection using the previous sample as the next seed.

Requested change:

• Remove the new public Robot.compute_ik_path() and OPWSolver.get_ik_path() APIs.
• Keep Robot.compute_batch_ik() as the single batch-IK entry point.
• Extend that existing path (or its existing solver options) with an optional continuous-selection mode.
• Reuse solver.get_ik(..., return_all_solutions=True) for OPW candidate generation.
• Keep the sequential candidate selector as an internal implementation detail; it must not launch a second, duplicated OPW candidate solver.
• Update the tutorial, tests, API docs, and agent context to use the existing batch-IK entry point.

Calling the current compute_batch_ik() unchanged is not quite equivalent because it hardcodes return_all_solutions=False and independently selects each target. A small extension is therefore needed to preserve the PR's claimed branch-continuity behavior, but a parallel public Path IK API is not needed.

[P1] Multi-segment sampling drops required internal waypoints

_make_sample_times samples only a global grid over total duration. Segment boundary times are not inserted before composing the output trajectory.

For A=(0,0) -> B=(1,0) -> C=(1,1):

• sample_count=2 returns only [A, C];
• sample_count=4 returns approximately A, (0.68,0), (1,0.32), C; and
• even sample_count=10 never contains B exactly.

The continuous internal profile is not part of PlanResult; consumers only receive the sampled positions. Consequently, stop_at_waypoints=True does not ensure that execution reaches or stops at the waypoint, and a controller can cut across a required corner.

Please build sample times per segment so every retained cumulative segment boundary appears exactly in the output. In quantity mode, reject requests with fewer samples than retained waypoints, then allocate remaining samples across segments. Add a regression asserting exact inclusion of B and zero velocity at B.

[P1] Duplicate waypoints can delete a genuine corner

The current compression rule removes any interior point adjacent to a zero-length edge: duplicate_neighbor = ....

Reproduction:

[A=(0,0), B=(1,0), B=(1,0), C=(1,1)] -> [A, C]

This changes an L-shaped path into a diagonal whenever stop_at_waypoints=False.

Please collapse each consecutive duplicate run first while preserving one representative of the corner, and then test collinearity against the nearest non-duplicate neighbors. Add the exact A, B, B, C regression case.

[P1] Explicit motion limits are silently ignored when sample_count=None

The trapezoidal option construction is incorrectly guarded by sample_count is not None in MotionGenerator.resolve_plan_options.

This call:

sample_count=None, velocity_limit=0.01, acceleration_limit=0.02

returns the backend defaults velocity=0.2 and acceleration=0.5. A caller's explicit limits can therefore be silently relaxed.

Please resolve backend options independently of sampling: always apply non-None generic limits, and let sample_count control only the sampling method/count. Add a MotionGenerator-level regression test rather than testing only TrapezoidalPlanner.__new__().

Additional architecture and implementation improvements

[P2] Cartesian planning is implemented as a tutorial-private pipeline

The core TrapezoidalPlanner supports only JOINT_MOVE, while the tutorial imports the private _plan_linear_profiles helper and independently assembles the Cartesian path, IK, derivatives, and replay.

If Cartesian-first timing is intended to be a reusable product capability, expose it through a typed public planning contract composed by MotionGenerator. If it is intentionally tutorial-only, narrow the PR/docs claims accordingly and avoid describing it as a generally available planner feature.

[P2] Batched replay does not preserve per-environment timing

replay_plan advances every environment using dt[:, sample_index].max().

When batch rows have different durations, only the longest row follows its planned timing. Shorter rows are slowed down, so their executed velocity/acceleration no longer match PlanResult.

Please resample all rows onto one shared simulation-time grid before replay, or explicitly restrict replay to B=1/identical dt rows and validate that precondition.

[P2] Public integration coverage is incomplete

The current tests cover the scalar planner, tutorial helpers/fakes, and the OPW selector kernel, but they do not exercise the real public integration path end to end.

Please add focused coverage for:

• MotionGenerator construction and generation with TrapezoidalPlannerCfg;
• Robot.compute_batch_ik() with a (B,N) OPW pose path and continuous selection;
• OPW FK -> batch IK reconstruction with TCP/base transforms;
• configured ik_nearest_weight during continuous selection;
• the three regressions above; and
• Torch/CUDA-Warp parity on an available GPU CI worker.

The motion-planning agent context should also stay focused on ownership, entry points, contracts, and validation surfaces. Tutorial font, dashboard layout, and plotting details belong in tutorial/Sphinx documentation rather than agent architecture context.

Validation performed

I merged the PR into the current origin/main in a temporary worktree and ran the new planner/tutorial/selector tests together with existing MotionGenerator, OPW solver, and agent-context regressions:

96 passed, 2 skipped

Additional checks passed:

• API docs checker: 1679/1679 public exports documented.
• Black check on the changed Python files.
• git diff --check.

The GitHub lint/build/test checks are green on the PR head, but they predate the latest origin/main changes. Full CUDA Warp and real simulation replay validation are still outstanding.

Recommendation

Please address the P0 reuse issue and all three P1 correctness issues before merge. The P2 items should either be resolved in this PR or explicitly scoped/documented so the public API does not promise behavior that currently exists only in the tutorial.

[yuecideng]

Should we standardlize this script as a reusable tool for motion generator @chase6305 ?