TrapezoidalPlanner

TrapezoidalPlanner#

TrapezoidalPlanner is a lightweight, natively batched joint-trajectory planner. It time-parameterizes piecewise-linear joint paths with either a trapezoidal velocity profile or a jerk-limited seven-phase Double-S profile. The Torch implementation is the reference and CPU fallback; the NVIDIA Warp backend parallelizes profile construction, phase evaluation, and batched joint trajectory composition for CUDA workloads.

Use this planner when you need deterministic joint-space timing, explicit velocity and acceleration outputs, and no collision-world planning dependency. For collision-aware Cartesian planning, use cuRobo instead.

Capabilities#

  • Batched input and output with a leading environment dimension.

  • Scalar or per-joint velocity, acceleration, and jerk limits.

  • Synchronized multi-joint motion for every path segment.

  • Triangular fallback when a short move cannot reach the requested trapezoidal cruise velocity.

  • Jerk-limited seven-phase Double-S trajectories without display filtering.

  • Fixed-count and approximately fixed-time sampling.

  • Optional minimum-duration scaling without changing the path.

  • Redundant collinear-waypoint compression and optional quintic corner blends.

  • Explicit torch, warp, and auto backend selection.

  • Native positions, velocities, accelerations, dt, and constraint diagnostics in PlanResult.

The planner accepts JOINT_MOVE waypoints. Cartesian straight-line planning in the accompanying tutorial is a separate pipeline: it applies a scalar time law in Cartesian space first, solves the complete pose path through continuous batched IK, and derives joint velocity and acceleration using differential kinematics. This ordering avoids distorting the requested end-effector path by applying joint-space timing after independently sampled IK.

MotionGenerator integration#

Configure the planner through the common MotionGenerator facade:

import torch

from embodichain.lab.sim.motion.motion_generator import (
    MotionGenCfg,
    MotionGenOptions,
    MotionGenerator,
)
from embodichain.lab.sim.motion.planners import (
    PlanState,
    TrapezoidalPlannerCfg,
    TrapezoidalPlanOptions,
)

generator = MotionGenerator(
    MotionGenCfg(
        planner_cfg=TrapezoidalPlannerCfg(robot_uid=robot.uid),
    )
)

# PlanState tensors are batched: (B, DOF).
start_qpos = robot.get_qpos(name="arm")
goal_qpos = torch.zeros_like(start_qpos)

result = generator.generate(
    [PlanState.from_qpos(goal_qpos)],
    MotionGenOptions(
        start_qpos=start_qpos,
        control_part="arm",
        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",
        ),
    ),
)

TrapezoidalPlanner owns sparse joint-waypoint timing. MotionGenerator prepends start_qpos when the first requested waypoint is not already the start, and does not run generic joint pre-interpolation. It also preserves the planner’s native sample grid and its analytical velocity and acceleration outputs. MotionGenOptions.sample_count therefore does not replace an explicit TrapezoidalPlanOptions.sample_interval. When options are automatically resolved from a backend-neutral request, the requested quantity is treated as a lower bound if Cartesian-to-joint conversion produces more required waypoints; an explicit TrapezoidalPlanOptions.sample_interval remains authoritative.

Results follow the shared PlanResult contract. constraint_report adds derivative peaks, per-joint utilization, limits, and status. Native trajectories start with a zero arrival interval.

Profiles and constraints#

Set profile="trapezoidal" for acceleration-limited trapezoidal timing. The planner automatically uses a triangular profile for moves too short to reach a constant-velocity phase. Acceleration changes discontinuously at phase boundaries, so this profile does not enforce the configured jerk limit.

Set profile="double_s" for a seven-phase profile constrained by velocity, acceleration, and jerk. The constraints mapping accepts a positive scalar or one value per joint for each derivative. All joints within a segment are synchronized to the same segment duration.

minimum_duration stretches trajectories that would otherwise finish sooner. It does not shorten slower trajectories or relax derivative limits.

Waypoint behavior#

By default, stop_at_waypoints=True, so every supplied waypoint is a rest point. Set it to False when intermediate samples describe a continuous path:

  • duplicate and same-direction collinear points are compressed;

  • real corners remain in the path;

  • collinearity_tolerance controls the angular comparison;

  • positive blend_tolerance inserts quintic corner blends and requires stop_at_waypoints=False.

If the caller passes start_qpos and also includes exactly the same first waypoint, MotionGenerator avoids inserting a duplicate zero-length segment.

Sampling and backends#

TrajectorySampleMethod.QUANTITY interprets sample_interval as the requested integer output count. It must provide at least one sample for every retained waypoint. TrajectorySampleMethod.TIME interprets it as a positive time step in seconds. In a batch with different durations, shorter rows hold their exact final position with zero velocity, zero acceleration, and zero arrival intervals after completion.

Backend selection follows these rules:

  • backend="torch" uses the reference implementation.

  • backend="warp" requests the Warp implementation and requires float32 tensors plus an available Warp runtime; it can be exercised on Warp CPU or CUDA devices.

  • backend="auto" selects Warp for CUDA float32 inputs and Torch for CPU or float64 inputs.

No new runtime dependency is needed: Warp is already part of EmbodiChain and is only required when its backend is selected.

Atomic Skill integration#

MoveJoints reaches the same planner through AtomicActionEngine:

from embodichain.lab.sim.atomic_actions import (
    ActionInvocation,
    AtomicActionEngine,
    JointPositionGoal,
    MotionPolicy,
)

engine = AtomicActionEngine(generator)
binding = engine.bind_control_parts(
    "move_joints",
    {"primary": {"motion": "arm"}},
)
invocation = ActionInvocation(
    skill_id="move_joints",
    goal=JointPositionGoal(target=goal_qpos),
    binding=binding,
    motion_policy=MotionPolicy(
        strategy="motion_gen",
        plan_opts=TrapezoidalPlanOptions(
            profile="double_s",
            constraints={
                "velocity": 0.5,
                "acceleration": 1.0,
                "jerk": 3.0,
            },
            sample_interval=100,
            backend="auto",
        ),
    ),
)
plan = engine.plan(invocation, latest_context)

The planner’s timed trajectory is retained at the MotionGenerator boundary. Atomic execution may retime it to the environment control grid and derives the velocity targets for that executed grid; terminal and stationary holds command zero velocity.

Tutorials and benchmark#

Run the full batched simulation tutorial for joint planning, Cartesian path IK, timed replay, and position/velocity/acceleration diagnostics:

python scripts/tutorials/sim/planner/trapezoidal_planner.py \
    --path joint --profile both --backend auto

The minimal scalar-profile example is scripts/tutorials/sim/planner/trapezoidal_profile.py. Reproducible Torch/Warp measurements are provided by scripts/benchmark/motion_generation/trapezoidal_planner.py.

See the MotionGenerator guide for the shared planning contract and the API reference for complete class and option documentation.

Contents