Fix Atomic Skill trajectory timing and contact behavior

[Yuan-Xinyi]

Description

Atomic Skill trajectories could reach the requested endpoint while still producing visibly or physically incorrect motion: uneven speed, compressed duration, unconstrained contact motion, discontinuous velocity/hand commands, and missing stabilization phases. This PR fixes those behaviors without changing the typed action contracts.

Stack

• Layer: 2/2
• Base: codex/composed-spatial-generalization
• Depends on: Add composed spatial-generalization benchmarks #779
• Followed by: none

This PR should merge after #779.

Problems and how to reproduce them

The regressions below describe the behavior on the parent commit. The named tests in this PR are also focused reproduction cases: run them against the parent production implementation to observe the failure, and against this branch to verify the fix.

1. IK interpolation did not enforce velocity or acceleration limits

   • Reproduce with an IK path containing unequal joint-space segment lengths, a small velocity_limit/acceleration_limit, and a low sample_count.
   • The previous fixed-count linear interpolation returned the requested number of samples even when finite-difference velocity or acceleration exceeded the configured limits. It also entered and left the motion with a velocity step rather than a rest-to-rest profile.
   • Regression entry points:
     • tests/compute/test_trajectory.py -k minimum_jerk
     • tests/sim/motion/test_motion_generator_batched.py -k "limited_cartesian_motion or ik_interp_consumes"

2. sample_count could compress a slower planner trajectory

   • Reproduce with a planner path whose native dt.sum() is longer than (sample_count - 1) * control_dt, then compose it into Push, Slide, OpenDoor, Twist, AxisAlign, PickUp, Place, HandOver, or CoordinatedPlacement.
   • The previous helper resampled every native path to exactly sample_count positions and discarded its timing. A physically slow path was therefore executed faster, with visibly nonuniform phase speed.
   • Regression entry point: tests/sim/atomic_actions/test_primitives_helpers.py -k resample_planned_trajectory.

3. Contact skills accepted endpoint-only paths that did not preserve contact geometry

   • Reproduce Press, PushObject, Slide, OpenDoor, or Twist with MotionPolicy(strategy="motion_gen").
   • The previous implementation sent only phase endpoints to the free-space planner. Intermediate FK samples could leave a straight push/press line or a door/twist arc while the action still reported success.
   • Regression entry points:
     • tests/sim/atomic_actions/test_actions.py -k motion_gen_rejects_uncertified_contact_path
     • The same condition is covered for Slide and Twist by their strategy validation.

4. Published velocity was discontinuous at starts, stops, and holds

   • Reproduce any composed action containing a repeated position at a phase boundary and inspect trajectory.velocities.
   • The first waypoint could be nonzero, and only one side of a stationary interval was zeroed. Consumers therefore received a nonzero velocity at rest or an immediate jump after a hold.
   • Regression entry point: tests/sim/atomic_actions/test_actions.py -k composed_actions_materialize_velocity_and_end_in_zero_hold.

5. Runtime cadence silently rounded or merged incompatible durations

   • Reproduce a batch with two active rows having different positive frame durations; the runner previously used the longest duration as a shared barrier.
   • Reproduce with control_dt or a requested sleep duration that is not an integer multiple of physics_dt; the simulation adapter previously rounded upward with ceil.
   • Both cases change physical playback time without changing the trajectory and can create uneven motion across environments.
   • Regression entry points:
     • tests/sim/atomic_actions/test_runner.py -k mismatched_active_batch_intervals
     • tests/sim/atomic_actions/test_sim_adapter.py -k off_grid

6. Coordinated dual-arm motion only enforced the closed chain sparsely

   • Reproduce CoordinatedPickment/CoordinatedPlacement with a moving shared object, then compute both end-effector transforms at interpolated waypoints.
   • The previous implementation solved sparse object keyframes and interpolated joint angles between them. Intermediate samples could violate either grasp transform even when all sparse IK calls succeeded.
   • Regression entry point: tests/sim/atomic_actions/test_actions.py -k coordinated_pick_selects_reachable_candidate.

7. Hand commands could jump away from the observed robot state

   • Reproduce with non-default observed hand joint positions, then plan PickUp, Place, MoveHeldObject, Pour, AxisAlign, Slide, OpenDoor, Twist, HandOver, or coordinated manipulation.
   • Several paths initialized hand samples from a configured/zero command rather than observed qpos. In addition, interpolate_hand_qpos(..., n_waypoints=1) returned the start state, so a one-frame close/open command had no effect.
   • Regression entry points:
     • tests/sim/atomic_actions/test_trajectory_ops.py -k interpolate_hand_qpos
     • tests/sim/atomic_actions/test_actions.py -k move_held_object_moves_only_exclusively_held_rows

8. HandOver released the source immediately after receiver closure

   • Reproduce a full handover and inspect the segment order and hand positions around receive_close and handover_release.
   • The previous route had no receiver stabilization hold, so the source could open on the next control frame before the receiving grasp had settled.
   • Regression entry point: tests/sim/atomic_actions/test_actions.py -k handover.

9. Pour reversed immediately at the apex

   • Reproduce a pour and inspect the arm trajectory and segment list around maximum rotation.
   • The previous action had no configurable dwell and immediately began the return motion, which can prevent simulated material transfer despite reaching the requested angle.
   • Regression entry point: tests/sim/atomic_actions/test_actions.py -k pour_rotates_held_object.

How each problem is fixed

1. Bounded, smooth interpolation

   • Add a rest-to-rest minimum-jerk time law.
   • Treat sample_count as a lower bound and expand the trajectory until discrete joint velocity and acceleration satisfy the policy limits.
   • For exact Cartesian paths, interpolate one global path-progress parameter and solve IK at every emitted pose so Cartesian geometry and joint limits are checked together.

2. Native timing preservation

   • Retiming now uses planner dt on the control grid.
   • Native duration is never shortened; requested phase length only pads a terminal hold when it is longer.
   • Phase splitting preserves the planner's original timing instead of reconstructing duration from sample count.

3. Fail-closed contact behavior

   • Contact-constrained skills use sampled exact Cartesian paths with ik_interp.
   • Press, PushObject, Slide, OpenDoor, and Twist reject motion_gen until that backend can certify every contact-path sample, rather than silently accepting endpoint-only approximations.
   • Door and twist sample budgets retain sufficient arc samples.

4. Rest-consistent velocity output

   • Force the first and final velocity samples to zero.
   • Zero both sides of every stationary joint interval so holds and phase boundaries cannot publish residual motion.

5. Exact execution cadence

   • Resolve one common positive active duration before dispatch and reject mismatched batched durations.
   • Require control_dt and sleep durations to lie on the physics grid, allowing only floating-point tolerance rather than rounding to a different duration.

6. Dense closed-chain projection

   • Use sparse coordinated IK only to choose/seed a solution.
   • Project every dense object-trajectory waypoint through batch IK for both arms, preserving both grasp transforms throughout the motion.

7. Continuous hand state

   • Seed hand trajectories from observed robot qpos and hold that state through arm-only phases.
   • Make a one-waypoint hand interpolation emit the commanded endpoint.

8. Stable handover sequencing

   • Insert an explicit receive_hold segment before releasing the source.
   • Treat hold steps as additional duration, so the stabilization phase is not squeezed into the original sample budget.

9. Physical pour dwell

   • Add validated dwell_duration configuration.
   • Split the motion at the pour apex, hold the apex for at least the requested duration, then return while preserving the hand state.

Review follow-ups

All four Greptile inline findings were reproduced and are valid:

1. Mixed HandOver directions shared mutable phase lengths

   • Reproduce with a nearest-arm batch whose rows choose opposite handover arms and whose two directional IK plans expand different phases.
   • Each direction now owns its phase-length map. Corresponding phases are terminal-padded to their shared maximum before row selection, preserving semantic phase boundaries.

2. Passive effect polling used an off-grid simulation sleep

   • Reproduce with physics_dt=0.01, the default minimum_cycle_time=0.001, and a pending physical-effect verification.
   • Simulation clocks advertise their sleep quantum; the runner rounds only passive polling up to that quantum while retaining strict, exact motion-command durations.

3. Limit-driven Cartesian expansion cut inside articulated arcs

   • Reproduce OpenDoor or Twist with active velocity/acceleration limits and inspect newly inserted Cartesian samples: polyline interpolation changes the hinge radius.
   • Exact paths can now provide a normalized-progress sampler. OpenDoor and Twist regenerate every expanded pose from their physical rotation axis, so the rigid-grasp radius is preserved.

4. Configured Task Programs rejected the new timing fields

   • Reproduce by adding velocity_limit, acceleration_limit, or Pour dwell_duration to configured YAML.
   • The closed decoders now accept and validate those fields; runtime metadata also retains the two motion limits.

5. Exact-path starts used a stricter tolerance than successful IK

   • Reproduce with a successful iterative IK result whose FK retains a sub-millimetre error allowed by its solver configuration.
   • Exact-path start validation now uses the active solver's position and rotation tolerances while retaining an exact terminal target and exact articulated samples.

6. Zero-length HandOver holds entered a non-empty padding helper

   • Reproduce a mixed nearest-arm batch with HandOverOptions(hold_steps=0).
   • Corresponding zero-length phases are now retained as empty and skipped during terminal padding; an asymmetric empty/non-empty phase still fails closed.

7. Dense float32 minimum-jerk sampling could reverse and exceed limits

   • Reproduce a nonzero float32 move on a 1 ms grid; the expanded polynomial lost monotonicity near the endpoint and produced backward intervals and acceleration spikes.
   • Progress is now evaluated symmetrically in float64, cast back to the requested output dtype, and its finite-difference velocity/acceleration are revalidated on the actual control grid before returning.

8. The new limits shifted the public MotionPolicy positional constructor

   • Reproduce with the previously valid MotionPolicy("ik_interp", 50, "zero", DynamicCollisionMode.OFF, None) call.
   • New limit fields now follow all pre-existing fields, preserving the old positional order while remaining available by keyword.

9. One failed OpenDoor/Twist row could abort the whole exact-path batch

   • Reproduce with two environments where approach/reach IK fails in only one row; its held FK start differed from the unmasked arc sampler start.
   • Inactive rows now replace later Cartesian keyframes and exact sampler output with an FK-consistent hold. Successful rows keep the articulated path and failed rows remain held.

10. Slide release expansion could outgrow the hand-opening phase

• Reproduce a release retreat whose limit-aware arm trajectory expands beyond hand_interp_steps; assembly previously raised a tensor shape error.
• Arm release and hand opening are terminal-padded to their shared native maximum, and segment bookkeeping uses the expanded phase length.

11. A later invalid runtime interval left an accepted command armed

• Reproduce a valid first frame followed by a mismatched or below-minimum interval. The session cursor advanced before validation raised, without cancelling the persistent first command.
• Interval failures now enter the runner's fail-safe path, cancel and hold previously armed targets, transition to terminal FAILED, and cannot skip ahead on a later step().

12. ik_interp bypassed the Robot-owned speed caps introduced by Use robot joint limits in motion planning #786

• Reproduce with per-joint caps [0.05, 1.2]: omission must preserve both caps, while an explicit 0.2 request must resolve to [0.05, 0.2].
• MotionPolicy preserves omitted velocity as None through configuration and metadata. Planner-backed motion delegates that omission to Use robot joint limits in motion planning #786's runtime resolver; the current stacked base has a compatibility fallback. ik_interp applies the same ordered, batch-strict, mimic-aware Robot caps directly.

13. CUDA TOPPRA received device tensors at its NumPy worker boundary

• Reproduce a default MotionGenerator + TOPPRA request on CUDA; Robot-derived velocity limits remain on the target device and direct NumPy conversion raises before planning.
• TOPPRA now materializes tensor-backed velocity and acceleration constraints on CPU in the parent process before inline or process-pool dispatch, keeping workers NumPy-only.
• Regression entry point: tests/sim/motion/planners/test_toppra_batched.py -k device_bound_constraints.

Type of change

• Bug fix (non-breaking change which fixes an issue)

Validation

• black --check --diff --color ./ — pass (black==26.3.1)
• git diff --check — pass
• python -m compileall -q embodichain tests — pass
• python docs/scripts/check_api_docs.py — 2613/2613 exports documented
• python .agents/skills/project-dev-context/scripts/context.py check — pass
• pytest -q tests/test_agent_context_map.py tests/test_agent_context_tools.py — 40 passed
• pytest -q tests/compute/test_trajectory.py — 13 passed, 2 skipped
• Focused Yueci regressions across Atomic Actions, runner, motion generation, configured integration, and runtime metadata — 31 passed
• tests/sim/motion/test_motion_generator_batched.py — 91 passed, 1 skipped
• tests/lab/task_program/test_semantic_executor.py — 54 passed
• Focused TOPPRA worker/config/device-bound regressions — 9 passed
• Real CUDA TOPPRA constraint handoff probe — pass

The local embodichain311 environment contains an older DexSim build without
RTRenderMode; the simulation test commands supplied that enum at import time
only so the repository tests could run. No dependency or production-code shim
is included in this PR.

Screenshots

Not applicable; the observable changes are trajectory timing, derivatives, path geometry, and segment sequencing.

Checklist

• I have run the black . command to format the code base.
• I reviewed affected documentation and agent context and updated the Atomic Action and motion-planning context.
• Public API documentation coverage was checked with python docs/scripts/check_api_docs.py.
• I have added focused regression tests for the reported problems.
• No dependency changes were required.

[greptile-apps]

Confidence Score: 5/5

[High impact] The PR appears safe to merge; no new blocking issue was found.

Summary

This PR fixes Atomic Skill timing and contact paths. The latest changes address the remaining coordinated-arm jump finding.

• Planner paths keep their original timing when actions assemble phases.

The supplied previous threads are addressed by the current code. No new actionable issue was established in the changes since the last review.

Diagram

%%{init: {'theme': 'neutral'}}%%
flowchart TD
    A[Shared object poses] --> B[Generate IK candidates for both arms]
    B --> C[Choose candidates near the previous posture]
    C --> D[Check every joint step against speed caps]
    D -->|Pass| E[Build timed commands]
    D -->|Fail| F[Reject the affected row]

Loading

_{Reviews (9) · Last reviewed commit: "fix(atomic-actions): keep coordinated IK..." · Reviewed by Greptile}

[yuecideng]

Reviewed PR #784 against base fdd28ae and head a758585. Five P2 findings are attached inline; I recommend addressing them before merge.

Validation: the independent compute trajectory tests passed (11 passed, 2 skipped). Focused CPU probes reproduced the behavioral regressions against base/head and the numerical limit violation in the new interpolation. These probes use source-isolated methods and in-memory ports. Standard simulation-package test collection was blocked by the local DexSim ImportError for reconstruct_cable_rings_kernel; no live simulation, GPU, renderer, or hardware validation was performed.

[yuecideng]

Follow-up against the latest main (a8226e1): please preserve the Robot-owned joint-speed defaults introduced by #786 when integrating this PR. Acceleration remains a planner-owned budget. The concrete integration issue and expected regressions are attached inline.

[yuecideng]

TOPPRA integration follow-up: preserve the GPU batching and first-order autograd contracts already implemented on the pending #712 branch through #778.