Add batched trapezoidal and Double-S trajectory planning

agent_context/MAP.yaml

@@ -407,6 +407,9 @@ topics:
     source_of_truth:
       - embodichain/lab/sim/planners/base_planner.py
       - embodichain/lab/sim/planners/toppra_planner.py
+      - embodichain/lab/sim/planners/trapezoidal_planner.py
+      - embodichain/lab/sim/planners/trapezoidal_warp.py
+      - scripts/benchmark/motion_generation/trapezoidal_planner.py
       - embodichain/lab/sim/planners/curobo/curobo_planner.py
       - embodichain/lab/sim/planners/curobo/curobo_yaml.py
       - embodichain/lab/sim/planners/motion_generator.py

agent_context/topics/ik-solvers/ik-solvers.md

@@ -205,6 +205,10 @@ and available CUDA backends in seeded and full redundancy-search modes.
 
 - `a1, a2, b, c1–c4, offsets, flip_axes, has_parallelogram`: OPW kinematic parameters.
 - `safe_margin`: joint-limit safety margin in radians.
+- `get_ik(..., return_all_solutions=True)` is the candidate-generation path for
+  continuous batch IK. `Robot.compute_batch_ik(..., continuous=True)` performs
+  sequential branch selection through an internal OPW selector, using the
+  previous sample as the next seed and the configured nearest-solution weights.
 
 ---
 

agent_context/topics/motion-planning/motion-planning.md

@@ -7,6 +7,8 @@
 | Planner registry | `embodichain/lab/sim/planners/__init__.py` |
 | Base planner class & config | `embodichain/lab/sim/planners/base_planner.py` → `BasePlanner`, `BasePlannerCfg`, `CollisionWorldInfo`, `PlanOptions`, `validate_plan_options` |
 | TOPPRA planner | `embodichain/lab/sim/planners/toppra_planner.py` → `ToppraPlanner`, `ToppraPlannerCfg`, `ToppraPlanOptions` |
+| Trapezoidal planner | `embodichain/lab/sim/planners/trapezoidal_planner.py` → `TrapezoidalPlanner`, `TrapezoidalPlannerCfg`, `TrapezoidalPlanOptions` |
+| Trapezoidal Warp kernels | `embodichain/utils/warp/kinematics/trapezoidal_warp.py` → batched profile construction and sampling kernels |
 | Neural planner | `embodichain/lab/sim/planners/neural_planner.py` → `NeuralPlanner`, `NeuralPlannerCfg`, `NeuralPlanOptions` |
 | cuRobo planner | `embodichain/lab/sim/planners/curobo/curobo_planner.py` → `CuroboPlanner`, `CuroboPlannerCfg`, `CuroboWorldCfg`, `CuroboPlanOptions` |
 | Planner assets | `embodichain/data/assets/planner_assets.py` → `download_neural_planner_checkpoint()` |
@@ -30,6 +32,7 @@ The entire stack is **env-batched** (`B = num_envs`). `PlanState` / `PlanResult`
 ```
 BasePlanner (ABC)
   ├─ ToppraPlanner        Time-optimal path parameterization (fork-pool fan-out)
+  ├─ TrapezoidalPlanner   Batched trapezoidal or jerk-limited Double-S timing
   ├─ NeuralPlanner (experimental)   APG waypoint rollout (native batching)
   └─ CuroboPlanner        CUDA collision-aware planning (native batching)
 
@@ -40,12 +43,14 @@ Config hierarchy:
 ```
 BasePlannerCfg            robot_uid (MISSING), planner_type
   ├─ ToppraPlannerCfg     planner_type = "toppra", max_workers, mp_context
+  ├─ TrapezoidalPlannerCfg planner_type = "trapezoidal"
   └─ NeuralPlannerCfg     planner_type = "neural", checkpoint_path (MISSING)
 
 MotionGenCfg              planner_cfg (MISSING — must be a BasePlannerCfg subclass)
 
 PlanOptions               (empty base)
   ├─ ToppraPlanOptions    constraints, sample_method, sample_interval
+  ├─ TrapezoidalPlanOptions profile, constraints, sample_method, sample_interval
   └─ NeuralPlanOptions    control_part, start_qpos, max_steps
 
 MotionGenOptions          strategy, sample_count, velocity/acceleration limits,
@@ -85,6 +90,94 @@ Worker details:
 - `TIME` sampling can produce per-env waypoint counts; shorter trajectories are tail-padded by repeating the final waypoint and `duration` records the real endpoint per env.
 - Per-env failures set `success[b] = False` and fill the env's trajectory with its start qpos; other envs continue. `BrokenProcessPool` tears the pool down and rebuilds it on the next call.
 
+### TrapezoidalPlanner
+
+The trapezoidal planner is a dependency-free, batched Torch backend for
+piecewise-linear joint paths. Each input waypoint is a rest point. The default
+profile is acceleration-limited trapezoidal timing (with triangular fallback
+for short moves); ``profile="double_s"`` selects the rest-to-rest linear-path
+subset of the seven-phase Double-S time law. It preserves that
+implementation's discrete ``amax *= 0.9`` feasibility search for moves without
+a cruise phase and its 1% ``EnforceJointLimits`` margin. Scalar or per-joint
+velocity, acceleration, and jerk limits are projected onto each linear path
+segment. Golden tests cover durations and sampled position, velocity, and
+acceleration against the reference trajectory implementation. It supports fixed
+quantity and approximate fixed-time sampling and returns explicit ``dt``.
+``minimum_duration`` applies one uniform per-environment time scale, preserving
+the path while reducing velocity, acceleration, and jerk. The minimal
+``scripts/tutorials/sim/planner/trapezoidal_profile.py`` example plots scalar position,
+velocity, acceleration, and jerk without starting simulation. A runnable
+batched robot example lives in ``scripts/tutorials/sim/planner/trapezoidal_planner.py``.
+The tutorial names the two diagnostics ``velocity_trapezoidal`` (the
+``trapezoidal`` backend profile) and ``acceleration_trapezoidal`` (the
+jerk-limited ``double_s`` backend profile). It can save diagnostic plots with
+``--plot-output`` and supports headless execution with ``--no-show-plot``.
+Multi-path/profile runs finish planning before showing all figures together.
+``--path joint`` generates synchronized, limit-clamped motion on every arm
+joint. ``--path cartesian`` is Cartesian-first rather than joint-first: the
+trapezoidal backend time-parameterizes metric line arclength ``s(t)`` under
+``--cartesian-velocity``, ``--cartesian-acceleration``, and
+``--cartesian-jerk``; every resulting sample becomes an exact
+fixed-orientation point on the line before continuous-seed IK. The joint
+trajectory is never resampled afterward, so its desired EEF path retains the
+planned Cartesian geometry and time law. ``--path both`` runs both diagnostics
+and reports the scalar Cartesian derivative peaks plus maximum FK line error.
+``--cartesian-distance`` controls line length and ``--cartesian-step`` sets a
+minimum IK sample density. For a six-axis arm the tutorial uses the same
+non-singular seed as the TOPPRA demo before solving the default 0.10 m downward
+line. Joint derivatives used by the diagnostic and replay come from
+second-order differential kinematics. The solver Jacobian gives ``dq/ds`` for
+the fixed-orientation Cartesian tangent, its path derivative gives
+``d²q/ds²``, and the Double-S outputs are composed by the chain rule as
+``dq/dt = dq/ds * ds/dt`` and
+``d²q/dt² = d²q/ds² * (ds/dt)² + dq/ds * d²s/dt²``. The implementation does
+not numerically differentiate sampled IK positions in time. Cartesian derivative constraints
+apply to ``s(t)`` and do not imply identical joint-space jerk bounds after the
+nonlinear IK mapping. No display filtering is applied. Before derivative
+evaluation,
+each analytic IK sample is replaced only by the joint-limit-valid ``2π``
+equivalent nearest to the previous seed, removing representation wrap jumps
+without changing the physical configuration or Cartesian path.
+For OPW robots the Cartesian tutorial submits the complete pose path through
+``Robot.compute_batch_ik(..., continuous=True)``. This reuses the existing batch
+IK boundary and asks OPW for all candidates in one solver call before its
+internal temporally ordered branch selector runs; other solver types fail
+explicitly rather than being silently treated as continuous path solvers.
+Interactive replay consumes ``PlanResult.dt`` rather than submitting all
+samples as fast as Python can loop: every command advances enough physics steps
+for its scaled interval and windowed runs are wall-clock paced. The
+``--replay-speed`` multiplier controls playback speed; headless runs retain
+unthrottled wall-clock execution while preserving physics-step timing. Before
+each run, both current joint state and drive target are reset to the planned
+start pose.
+By default every input waypoint remains a rest point. Set
+``stop_at_waypoints=False`` to remove duplicate and straight, same-direction
+interior points before timing; genuine direction changes remain explicit rest
+points, and batch rows with fewer retained points are final-pose padded.
+``backend="auto"`` uses Warp profile construction and sampling for CUDA float32
+inputs and the Torch reference path otherwise. ``backend="warp"`` also permits
+explicit CPU Warp execution but requires float32. Path compression and limit
+projection remain shared Torch tensor operations. Warp then constructs each
+trapezoidal or Double-S scalar segment profile in parallel, including the
+Double-S no-cruise acceleration reduction and phase integration. Shared
+post-processing applies minimum-duration scaling and the Double-S duration
+margin consistently across backends.
+Torch uses batched ``searchsorted`` for segment lookup without materializing a
+``(B, N, segments)`` comparison tensor. Warp uses binary segment search once
+per ``(B, N)`` sample, then a separate ``(B, N, DOF)`` composition kernel so
+joint dimensions do not repeat profile lookup work.
+Torch phase lookup also uses ``searchsorted`` and expands only selected phase
+durations. Position, velocity, acceleration, and jerk coefficients are gathered
+directly by ``(batch, segment, phase)``, avoiding four additional
+``(B, N, phases)`` temporary tensors.
+An all-stationary batch takes a dedicated hold fast path: it generates only
+sample times and zero-derivative outputs, skipping constraint projection,
+profile construction, segment lookup, and Warp dispatch.
+The reproducible microbenchmark at
+``scripts/benchmark/motion_generation/trapezoidal_planner.py`` measures Torch
+and Warp time, CPU/GPU memory, endpoint success, and cross-backend error, then
+writes the required three-table Markdown report under ``outputs/benchmarks``.
+
 ### NeuralPlanner (experimental)
 
 Learning-based EEF waypoint planner. Franka Panda only.
@@ -179,7 +272,7 @@ import or branch on concrete planner option types.
 Unified interface for trajectory planning with optional pre-interpolation.
 
 - Wraps a `BasePlanner` instance (resolved from `planner_cfg.planner_type`).
-- Supported planner types: TOPPRA, NeuralPlanner, and cuRobo.
+- Supported planner types: TOPPRA, TrapezoidalPlanner, NeuralPlanner, and cuRobo.
 - `MotionGenCfg.planner_cfg` is **MISSING** — must be provided.
 - `generate()` and `interpolate_trajectory()` are env-batched (`B, N, DOF`).
 - `generate()` always returns a normalized `PlanResult`; failed rows hold the

docs/source/api_reference/embodichain/embodichain.lab.sim.planners.rst

@@ -10,6 +10,9 @@ embodichain.lab.sim.planners
     BasePlanner
     ToppraPlannerCfg
     ToppraPlanner
+    TrapezoidalPlanOptions
+    TrapezoidalPlannerCfg
+    TrapezoidalPlanner
     MotionGenCfg
     MotionGenerator
     TrajectorySampleMethod
@@ -44,6 +47,74 @@ Toppra Planner
     :inherited-members:
     :show-inheritance:
 
+Trapezoidal Planner
+-------------------
+
+The trapezoidal planner applies either acceleration-limited trapezoidal timing
+or jerk-limited Double-S timing to batched, piecewise-linear joint paths. Use
+``TrapezoidalPlanOptions.minimum_duration`` to slow a trajectory without
+changing its path or violating derivative limits.
+For densely interpolated straight paths, set ``stop_at_waypoints=False`` to
+remove redundant same-direction interior points while preserving real corners.
+``backend="auto"`` selects Warp profile construction and sampling for CUDA
+float32 inputs and retains the Torch reference implementation for CPU or
+float64. The Warp path builds trapezoidal or Double-S phases in parallel per
+batch segment, then evaluates scalar samples and composes all joints.
+Both backends avoid a dense sample-by-segment lookup tensor: Torch uses batched
+``searchsorted``, while Warp performs one binary lookup per batch sample before
+parallel joint composition.
+Torch also gathers profile coefficients directly by selected phase, avoiding
+full sample-by-phase copies of position, velocity, acceleration, and jerk data.
+All-stationary batches use a hold fast path and skip constraint projection,
+profile construction, segment lookup, and backend dispatch.
+
+.. autoclass:: TrapezoidalPlanOptions
+    :members:
+    :exclude-members: __init__, copy, replace, to_dict, validate
+
+.. autoclass:: TrapezoidalPlannerCfg
+    :members:
+    :exclude-members: __init__, copy, replace, to_dict, validate
+
+.. autoclass:: TrapezoidalPlanner
+    :members:
+    :inherited-members:
+    :show-inheritance:
+
+Example
+~~~~~~~
+
+Select the Double-S profile when jerk continuity is required while retaining
+the same ``MotionGenerator`` entry point::
+
+    generator = MotionGenerator(
+        MotionGenCfg(
+            planner_cfg=TrapezoidalPlannerCfg(robot_uid=robot.uid),
+        )
+    )
+    result = generator.generate(
+        [PlanState.from_qpos(start), PlanState.from_qpos(goal)],
+        MotionGenOptions(
+            plan_opts=TrapezoidalPlanOptions(
+                profile="double_s",
+                constraints={
+                    "velocity": 0.5,
+                    "acceleration": 1.0,
+                    "jerk": 3.0,
+                },
+                sample_interval=200,
+                stop_at_waypoints=False,
+                backend="auto",
+            )
+        ),
+    )
+
+The complete batched simulation tutorial is
+``scripts/tutorials/sim/planner/trapezoidal_planner.py``.
+Pass ``--backend torch`` or ``--backend warp`` to compare implementations.
+For repeatable timing and memory measurements, run
+``scripts/benchmark/motion_generation/trapezoidal_planner.py``.
+
 Motion Generator
 ----------------
 

docs/source/api_reference/public_api.rst

@@ -982,6 +982,31 @@ embodichain.lab.sim.planners.toppra_planner
    ToppraPlannerCfg
    ToppraPlanOptions
 
+embodichain.lab.sim.planners.trapezoidal_planner
+------------------------------------------------
+
+.. currentmodule:: embodichain.lab.sim.planners.trapezoidal_planner
+
+.. autosummary::
+
+   TrapezoidalPlanOptions
+   TrapezoidalPlanner
+   TrapezoidalPlannerCfg
+
+embodichain.utils.warp.kinematics.trapezoidal_warp
+--------------------------------------------------
+
+.. currentmodule:: embodichain.utils.warp.kinematics.trapezoidal_warp
+
+Warp-accelerated helpers construct scalar trapezoidal or Double-S motion
+profiles and compose their sampled path derivatives into batched joint-space
+trajectories.
+
+.. autosummary::
+
+   build_profile_warp
+   compose_profile_samples_warp
+
 embodichain.lab.sim.planners.utils
 ----------------------------------
 
@@ -1244,6 +1269,19 @@ embodichain.lab.sim.solvers.null_space_posture_task
 
    NullSpacePostureTask
 
+embodichain.lab.sim.solvers.opw_solver
+--------------------------------------
+
+.. currentmodule:: embodichain.lab.sim.solvers.opw_solver
+
+Configuration and runtime solver for analytic OPW forward and inverse
+kinematics of compatible six-axis manipulators.
+
+.. autosummary::
+
+   OPWSolver
+   OPWSolverCfg
+
 embodichain.lab.sim.solvers.pink_solver
 ---------------------------------------
 

docs/source/overview/sim/solvers/opw_solver.md

@@ -11,6 +11,7 @@
 * Flexible configuration via `OPWSolverCfg`
 * Strict enforcement of joint limits
 * Forward kinematics (FK) and multiple IK solution branches
+* Continuous whole-path IK through the existing batch-IK boundary
 
 ## Configuration
 
@@ -104,6 +105,12 @@ solver = OPWSolver(cfg, device="cuda")
 
 ```
 
+Continuous selection is requested at the robot boundary with
+`Robot.compute_batch_ik(..., continuous=True)`. The robot performs arena/root
+frame conversion and calls `get_ik(..., return_all_solutions=True)` once for the
+flattened pose batch. OPW then applies its internal sequential selector and
+returns validity `(B, N)` plus continuous joint positions `(B, N, 6)`.
+
 ## References
 
 * [OPW Kinematics Paper](https://doi.org/10.1109/TRO.2017.2776312)