[Feature]: Per-call `lock_joints` override on a shared multi-arm `MotionPlanner`

[bheijden]

Problem / motivation

We build one MotionPlanner for a multi-arm robot and dispatch per-arm goals at call time via goal_tool_poses. Arms without a goal are held by the planner's optimizer, not hard-locked, so trajopt frequently routes them through significant joint-space detours.

We want to hard-lock an arbitrary subset of joints at their current values per plan call, then unlock them on the next call where we plan all arms together. Today this means changing the lock_joints set, which is baked into the kinematics tensor shapes at MotionPlanner construction. Kinematics.update_kinematics_config is shape-preserving and refuses any change to the set. Rebuilding the planner per call costs many seconds (kinematics rebuild plus CUDA graph re-capture plus warmup), so it is not practical for interactive use.

Proposed solution

Per-call `lock_joints` override on `MotionPlanner.plan_pose` and `MotionPlanner.plan_cspace` that hard-locks a chosen subset of joints at supplied values for that one call, with no rebuild and no CUDA graph re-capture:


mp = MotionPlanner(MotionPlannerCfg.create(...))

# Plan only arm A. Hard-lock arm B's joints at their current values.
mp.plan_pose(
    goal_tool_poses={"arm_a_tool": target_a},
    current_state=full_js,
    lock_joints_override={"arm_b_j1": q1, "arm_b_j2": q2, ...},
)

# Same planner. Plan both arms. Override is per-call only.
mp.plan_pose(
    goal_tool_poses={"arm_a_tool": target_a, "arm_b_tool": target_b},
    current_state=full_js,
)


The override should accept an arbitrary subset of joints by name and should not assume the arms share structure or DOF count.

Alternatives considered

• Custom cspace cost penalizing motion of the joints we want held: weight tuning is brittle and a soft cost is not equivalent to a hard lock. Requires tuning.
• Pre-built planner pool, one planner per lock-set permutation: workable but each planner pays its own compilation cost at startup and again whenever the scene changes enough to force a recompile, GPU memory scales with the pool size, and the number of subsets grows quickly with the number of arms.
• Rebuild the MotionPlanner on every switch: latency is too high for an interactive UI.
• Goal-hold the idle arm by passing its current FK as a target: still a soft objective; we have observed idle-joint drifts from a few degrees up to a full revolution.
• Post-check that rejects plans whose idle joints drifted: catches the symptom, does not prevent it, and wastes the plan compute.

Additional context

cuRobo version: 0.8.0.

[balakumar-s]

Locking would require regenerating the kinematics tensors, which breaks any existing cuda graph. The way we do it today is having multiple motion planning instances for each locked joint set. You can update the locked joint values without breaking cuda graphs. You can also share the collision representation between planner instances.

How many lock-set permutations are you thinking? For a dual arm setup, we have at most 4 (1 per arm x 2, 1 for mobile base, 1 for bi-manual).

[balakumar-s]

Also, could you clarify why when scene changes, a recompile happens?

Pre-built planner pool, one planner per lock-set permutation: workable but each planner pays its own compilation cost at startup and again whenever the scene changes enough to force a recompile, GPU memory scales with the pool size, and the number of subsets grows quickly with the number of arms.

[bheijden]

Thanks @balakumar-s — the "scene change forces a recompile" was a false claim on my part, confirmed empirically. Pose-only obstacle moves and cuboid additions stay at steady-state plan latency (~100 ms). Adding the first mesh in a session took ~4 s; subsequent mesh adds dropped to ~500 ms. The one genuinely expensive mutation I found is exceeding the pre-allocated collision_cache capacity (12 meshes when sized for 10) — ~50 s — so sizing the cache generously up front avoids it. Three lock-set permutations cover our dual-arm case (left only, right only, both).

On sharing the collision representation: I looked through the source and don't see a public injection point. MotionPlannerCfg is a plain dataclass so it can be built manually rather than via create(...), but MotionPlanner._initialize_components() always constructs scene_collision_checker from scene_collision_cfg, and each sub-solver (IKSolver, TrajOptSolver, PRMGraphPlanner) captures that reference at construction time. Subclassing MotionPlanner and overriding _initialize_components is the closest hack I can see — is that the recommended path, or is there an API I'm missing?

Two follow-ups:

1. If the collision representation is shared, a mesh would only need to be added once to the shared instance, right? Per-planner cost would then be just the IK + trajopt kernel JIT for that planner's lock-set. Building back-to-back in one process I see ~19 s for the first planner and ~10 s for the second (JIT cache warms), so a pool of 3 ≈ 39 s startup. Does sharing the collision representation cut that further?

2. Why can't the "zero-locked-joints" case be the construction-time superset, with a per-call mask zeroing out gradients on whichever subset of joints we want fixed for that call? Tensor shapes stay constant, CUDA graphs stay valid, and the masked joints just sit at their current values for the duration of the call.