[Proposal] Compose Cartesian trajectory generation and diagnostics through MotionGenerator

[yuecideng]

Proposal

Provide reusable Cartesian trajectory generation through MotionGenerator without changing TrapezoidalPlanner into a Cartesian planner.

The main architectural decisions are:

• Keep TrapezoidalPlanner as a joint-space planner with supported_move_types = {MoveType.JOINT_MOVE}.
• Keep MotionGenerator as the public facade that accepts motion requests and dispatches them to the appropriate planning component.
• Extract the trapezoidal and Double-S scalar time law into one space-independent implementation shared by joint and Cartesian trajectory generation.
• Introduce a dedicated Cartesian trajectory component that composes SE(3) path geometry, scalar time parameterization, continuous batch IK, and differential kinematics.
• Reuse Robot.compute_batch_ik(..., continuous=True) for complete Cartesian pose paths.
• Do not add Robot.compute_ik_path() or OPWSolver.get_ik_path(); continuous branch selection remains internal to the existing batch IK boundary.
• Preserve desired Cartesian samples, explicit timing, derivative data, IK status, path metadata, and typed constraint diagnostics through the MotionGenerator normalization boundary.
• Keep numerical diagnostics and optional Matplotlib rendering separate from planning.

Related work: #566.

Current behavior in PR #566

The current implementation does not make TrapezoidalPlanner a Cartesian planner.

TrapezoidalPlanner:

• Declares only MoveType.JOINT_MOVE.
• Requires PlanState.qpos waypoints.
• Produces joint positions, velocities, accelerations, timing, and joint-limit diagnostics.
• Does not consume xpos or invoke IK.

After registration in MotionGenerator, Cartesian targets are accepted only through an indirect compatibility path:

EEF_MOVE
  -> MotionGenerator
  -> Cartesian pose interpolation
  -> Robot.compute_batch_ik(...)
  -> JOINT_MOVE waypoints
  -> TrapezoidalPlanner
  -> joint-space trajectory

This path is selected when a joint-only planner is configured and MotionGenOptions.is_interpolate=True. Without that option, an EEF_MOVE request is rejected by the trapezoidal backend.

The strategy="ik_interp" and preserve_cartesian_samples=True paths bypass TrapezoidalPlanner completely.

This is indirect Cartesian-input compatibility, not native Cartesian planning:

• Cartesian geometry is enforced only at sampled IK poses.
• Motion between solved samples is still generated in joint space, so the realized TCP path may deviate between samples.
• Velocity, acceleration, and jerk limits remain joint-space limits.
• Dense Cartesian samples combined with stop_at_waypoints=True can create a stop-and-go trajectory.
• The returned result does not currently express a strict Cartesian-path guarantee.

Motivation

PR #566 adds a reusable batched TrapezoidalPlanner, but its Cartesian line and Bézier examples use tutorial-local functions that call the private _plan_linear_profiles() helper and invoke batch IK directly. Joint and Cartesian trajectories therefore do not share a stable production API.

Making TrapezoidalPlanner directly support EEF_MOVE would mix several independent responsibilities:

1. Joint-path construction.
2. SE(3) path geometry.
3. Scalar time parameterization.
4. IK branch continuity.
5. Differential kinematics.
6. Cartesian and joint constraint validation.

It would also create unnecessary combinations between path types and time laws. Line, Bézier, trapezoidal, Double-S, TOPPRA, and future algorithms should remain composable rather than being represented as increasingly broad concrete planner classes.

The reusable part of TrapezoidalPlanner is its scalar time-profile implementation, not the entire joint-space planner.

The diagnostic dashboard in PR #566 is also tutorial-local. Meanwhile, the legacy MotionGenerator.plot_trajectory() accepts raw tensors, assumes a fixed 0.01 second interval, cannot compare desired and realized Cartesian poses, and couples the planning module to Matplotlib.

Proposed architecture

                         MotionGenerator
                        public motion facade
                                  |
                 +----------------+----------------+
                 |                                 |
          JOINT_MOVE request                Cartesian request
                 |                                 |
                 v                                 v
       TrapezoidalPlanner             CartesianTrajectoryGenerator
       joint path and limits             |       |        |
                 |                       |       |        +-- differential kinematics
                 |                       |       +----------- Robot.compute_batch_ik
                 |                       +------------------- CartesianPath
                 |                                            line / Bézier
                 |                                 |
                 +---------------+-----------------+
                                 |
                                 v
                    ScalarTimeParameterizer
                    trapezoidal / Double-S
                                 |
                                 v
                            PlanResult
                                 |
                                 v
                    TrajectoryDiagnostics
                                 |
                                 v
                    optional plot renderer

1. Keep TrapezoidalPlanner joint-space only

TrapezoidalPlanner.supported_move_types should remain:

frozenset({MoveType.JOINT_MOVE})

It should continue to own:

• Joint waypoint validation and optional compression.
• Piecewise-linear or blended joint-path construction.
• Projection of joint velocity, acceleration, and jerk limits.
• Joint trajectory sampling.
• Joint-space constraint diagnostics.

It must not consume EEF_MOVE, own SE(3) geometry, or call IK.

2. Extract a shared scalar time parameterizer

Refactor the existing trapezoidal and Double-S profile implementation into a reusable, robot-independent component, conceptually:

@dataclass(frozen=True, slots=True)
class ScalarTrajectory:
    position: torch.Tensor
    velocity: torch.Tensor
    acceleration: torch.Tensor
    jerk: torch.Tensor | None
    dt: torch.Tensor


def parameterize_scalar_path(
    path_length: torch.Tensor,
    *,
    profile: Literal["trapezoidal", "double_s"],
    velocity_limit: torch.Tensor,
    acceleration_limit: torch.Tensor,
    jerk_limit: torch.Tensor | None,
    sample_method: TrajectorySampleMethod,
    sample_interval: int | float,
    minimum_duration: float | None,
    backend: Literal["auto", "torch", "warp"],
) -> ScalarTrajectory:
    ...

The component should know nothing about joints, robots, IK, or Cartesian poses. Joint and Cartesian components project their own constraints onto the same scalar path coordinate.

This must be a refactor of the implementation added by PR #566, not a second trapezoidal or Double-S implementation.

3. Add a dedicated Cartesian trajectory component

Introduce a sibling component such as CartesianTrajectoryGenerator. It should initially be composed by MotionGenerator, rather than advertising Cartesian support on a joint-only BasePlanner.

Its responsibilities are:

1. Construct an explicit Cartesian path, such as an SE(3) line or Bézier path.
2. Compute path length and geometric derivatives.
3. Project linear and angular limits onto a scalar path coordinate.
4. Apply the shared trapezoidal or Double-S time law.
5. Evaluate all desired poses at the resulting samples.
6. Invoke continuous batch IK once for the complete pose sequence.
7. Compute or validate joint derivatives and joint constraints.
8. Return a complete PlanResult.

A separate typed option should own Cartesian configuration, for example:

@configclass
class CartesianTrajectoryOptions:
    path_type: Literal["line", "bezier"] = "line"
    profile: Literal["trapezoidal", "double_s"] = "trapezoidal"
    linear_limits: DerivativeLimits = DerivativeLimits()
    angular_limits: DerivativeLimits = DerivativeLimits(
        velocity=1.0,
        acceleration=2.0,
        jerk=5.0,
    )
    orientation_mode: Literal["fixed", "geodesic"] = "fixed"
    control_points: torch.Tensor | None = None

Bézier control points must be explicit caller input. Reusable code must not contain the tutorial-specific lateral offset.

4. Keep MotionGenerator as the public facade

MotionGenerator.generate() should own:

• Backend and component dispatch.
• Backend-neutral options.
• Runtime context such as start_qpos and control_part.
• Batch and failure normalization.
• Result resampling and diagnostic validity rules.

The intended dispatch is:

• JOINT_MOVE + TrapezoidalPlanner: call TrapezoidalPlanner directly.
• Cartesian request: invoke CartesianTrajectoryGenerator.
• Collision-aware Cartesian request: continue to use a backend that natively owns that capability, such as cuRobo.
• strategy="ik_interp": retain only as an explicitly simple interpolation strategy, with behavior distinct from a time-parameterized Cartesian trajectory.

The facade should not claim that TrapezoidalPlanner natively supports EEF_MOVE.

5. Reuse the existing batch IK boundary

After evaluating the complete Cartesian path, call the existing public robot API once:

sample_success, positions = robot.compute_batch_ik(
    pose=desired_poses,
    joint_seed=start_qpos,
    name=control_part,
    continuous=True,
)

Do not introduce Robot.compute_ik_path() or OPWSolver.get_ik_path(). OPW candidate generation and continuous equivalent selection should remain solver-internal behavior reached through compute_batch_ik.

The per-sample IK mask must be retained for diagnostics. Failed environments must produce a safe hold trajectory and zero derivatives rather than partially valid commands.

6. Strengthen the PlanResult diagnostic contract

Replace or wrap the untyped dict[str, torch.Tensor] report with a typed result such as:

@dataclass(frozen=True, slots=True)
class TrajectoryConstraintReport:
    space: Literal["joint", "cartesian"]
    peak_velocity: torch.Tensor
    peak_acceleration: torch.Tensor
    peak_jerk: torch.Tensor | None
    velocity_utilization: torch.Tensor
    acceleration_utilization: torch.Tensor
    jerk_utilization: torch.Tensor | None
    within_limits: torch.Tensor
    jerk_defined: bool

PlanResult should be able to preserve:

• Desired Cartesian samples.
• Joint positions, velocities, accelerations, and optional analytic jerk.
• Explicit per-sample dt.
• Per-sample IK success.
• Per-environment valid sample counts or a valid-sample mask.
• Path kind such as joint, line, or Bézier.
• Typed joint and Cartesian constraint diagnostics.

MotionGenerator._normalize_plan_result() must preserve these fields when samples are unchanged. If it resamples a trajectory, it must recompute or explicitly invalidate affected derivatives and reports.

Double-S may expose analytic jerk. Trapezoidal acceleration has discontinuities, so jerk at those boundaries must be marked undefined rather than reported as zero.

7. Separate numerical analysis from rendering

Add a pure tensor diagnostic module, for example:

embodichain/lab/sim/planners/diagnostics.py

It should compute:

• Desired versus FK-realized Cartesian poses.
• Cartesian position tracking error.
• Geodesic SO(3) orientation tracking error.
• Line straightness deviation.
• Joint and Cartesian constraint utilization.
• Per-environment and worst-environment summaries.

Bézier curvature must not be labeled as tracking error, and straightness should only be reported for line paths.

MotionGenerator may expose a thin convenience method that binds the robot, but it should return numerical diagnostics and must not import Matplotlib.

The optional renderer should consume the diagnostic result, use lazy plotting imports, and support explicit output and display options. No figure or extra FK work should be performed when neither output nor display is requested.

The legacy raw-tensor MotionGenerator.plot_trajectory() should be deprecated rather than extended.

Suggested implementation sites

• embodichain/lab/sim/planners/trapezoidal_planner.py
  • Keep joint-space planning.
  • Extract or consume the shared scalar time parameterizer.
• embodichain/lab/sim/planners/scalar_time_parameterizer.py
  • Shared trapezoidal and Double-S time profiles.
• embodichain/lab/sim/planners/cartesian_trajectory.py
  • Compose path geometry, time parameterization, batch IK, and differential kinematics.
• embodichain/lab/sim/planners/se3.py
  • SE(3) line geometry and derivatives.
• embodichain/lab/sim/planners/bezier.py
  • Explicit Bézier geometry and derivatives.
• embodichain/lab/sim/planners/motion_generator.py
  • Public dispatch and complete result preservation.
  • Thin numerical-analysis convenience method.
  • Legacy plot-method deprecation.
• embodichain/lab/sim/planners/utils.py
  • Typed result and report contracts.
• embodichain/lab/sim/planners/diagnostics.py
  • Pure tensor diagnostic computation.
• scripts/tutorials/sim/planner/
  • Examples and optional plotting only.
• agent_context/topics/motion-planning/motion-planning.md
  • Updated resolution path and contracts.

Acceptance criteria

• TrapezoidalPlanner remains a joint-space planner and declares only JOINT_MOVE.
• Joint trapezoidal and Double-S requests run through MotionGenerator.generate() and TrapezoidalPlanner.
• Cartesian requests run through MotionGenerator.generate() and a dedicated Cartesian trajectory component, not direct EEF_MOVE support in TrapezoidalPlanner.
• Joint and Cartesian flows reuse one scalar time-parameterization implementation.
• A complete Cartesian path invokes Robot.compute_batch_ik(..., continuous=True) once.
• No compute_ik_path() or OPWSolver.get_ik_path() API is introduced.
• The API clearly distinguishes indirect sampled Cartesian compatibility from strict Cartesian-path guarantees.
• Dense Cartesian samples do not implicitly become rest points unless the caller explicitly requests stop-at-waypoint behavior.
• Bézier control points are explicit caller input.
• PlanResult preserves desired poses, timing, derivatives, IK status, path metadata, and typed diagnostics through MotionGenerator.
• Joint and Cartesian limits are reported separately and validated in their correct spaces.
• Double-S diagnostics use analytic jerk.
• Trapezoidal jerk discontinuities are marked undefined.
• Cartesian position and orientation tracking errors are computed against FK-realized poses.
• Straightness is reported only for line paths.
• Quaternion handling consistently uses the project xyzw convention.
• Importing planners or MotionGenerator does not require Matplotlib.
• Plotting is skipped when neither display nor output is requested.
• Motion-planning agent context, tests, and public API documentation are updated.

Validation

Focused tests should cover:

• TrapezoidalPlanner rejecting EEF_MOVE and accepting JOINT_MOVE.
• MotionGenerator dispatching Cartesian requests to the Cartesian component rather than the joint planner.
• One-call continuous batch IK behavior.
• Line and explicit Bézier path geometry.
• Shared time-law behavior for joint and Cartesian paths.
• Per-sample and partial-batch IK failures.
• Joint-limit validation after Cartesian IK.
• Avoidance of unintended stops at dense Cartesian samples.
• Diagnostic preservation and invalidation across normalization and resampling.
• Position and SO(3) orientation error calculations.
• Distinction between tracking error and line straightness.
• Analytic Double-S jerk and undefined trapezoidal boundary jerk.
• Valid-sample masking for padded batches.
• xyzw quaternion fixtures.
• Matplotlib-free planner imports.
• Headless PNG generation in the optional renderer.

Additional context

This proposal intentionally separates five concerns:

• TrapezoidalPlanner owns joint-space paths.
• CartesianTrajectoryGenerator owns Cartesian path composition.
• ScalarTimeParameterizer owns trapezoidal and Double-S timing.
• MotionGenerator owns public dispatch and normalization.
• Diagnostic and rendering components consume results without becoming part of planning.

Implementation should be submitted as a separate focused pull request after the design is agreed.

Checklist

• I have checked that there is no similar issue in the repo (required)