[Bug]: plan_cspace fails on a self-collision detour its own graph planner solves

[kjyothiswaroop]

Prerequisites

• I have searched existing issues and discussions and could not find a duplicate.
• I have read the relevant section of the documentation.
• I can reproduce this on the latest main (or the most recent release).

Bug summary

Context: Freespace joint-space planning — MotionPlanner.plan_cspace(goal, start, max_attempts=8) on a 7-DOF arm with enable_graph=True (so enable_graph_attempt=1). Empty collision world (scene_model=None, no world obstacles) — the only obstacle is robot self-collision.

Observed: plan_cspace() returns success=False on a start→goal query whose straight-line (linear-interpolation) seed passes through a self-collision (Link3↔gripper_base). But a valid collision-free path demonstrably exists in cuRobo's own model:

• graph_planner.find_path(start, goal) returns one in ~90 ms (success=True, path_length ≈ 6.6 rad). Its raw interpolated_waypoints — with zero trajectory optimization — are collision-free (0/100 waypoints, ~98 mm min self-clearance) and reach the goal exactly (0.0 rad error).
• A fully-optimized OMPL+TrajOpt (Tesseract) path for the same query is also collision-free in cuRobo's sphere model (0/30, ~12 mm min clearance).

In plan_cspace (_src/motion/motion_planner_batch.py), from attempt >= enable_graph_attempt the graph does seed trajopt:

for attempt in range(max_attempts):
    seed_traj = None
    if self.graph_planner is not None and attempt >= enable_graph_attempt:
        goal_configs = goal_states.position.view(batch_size, 1, dof).repeat(1, num_seeds, 1)
        seed_traj = self._get_graph_seed_trajectories(current_state, goal_configs)
    trajopt_result = self.trajopt_solver.solve_cspace(goal_states, current_state, seed_traj=seed_traj)

…yet trajopt returns success=False. The best trajectory it returns leaves J4 at its start value (goal error ≈ the full J4 delta ≈ 2.6 rad) — it stalls at the self-collision instead of following the graph's detour. Feeding either known-good path (graph or Tesseract) directly to trajopt_solver.solve_cspace(seed_traj=...) also fails.

curobo_repro.zip

Steps to reproduce

Minimal self-contained repro (attached `curobo_repro.zip`: `robot.yml` + `robot.urdf` with `load_meshes: false`, `tesseract_solution.json`, `repro.py`). No scan, no meshes, no ROS. `cd curobo_repro && python repro.py`:


#!/usr/bin/env python3
"""Reproduces cuRobo plan_cspace() returning success=False on a self-collision
detour that the graph planner solves and that OMPL+TrajOpt (Tesseract) solves.

The world is empty, so the only obstacle is robot self-collision. Both the graph
planner's own path and a reference Tesseract path are verified collision-free in
cuRobo's sphere model, yet plan_cspace/trajopt produces neither.

    python repro.py            # graph-succeeds / plan_cspace-fails contrast
    python repro.py --viser    # Viser view (http://localhost:8080) to play each solution
"""
import os
import sys
import time
import json

import numpy as np
import torch
import yaml

from curobo.motion_planner import MotionPlanner, MotionPlannerCfg
from curobo.types import JointState

HERE = os.path.dirname(os.path.abspath(__file__))
YML = os.path.join(HERE, "robot.yml")

JOINTS = ["J1", "J2", "J3", "J4", "J5", "J6", "J7"]
START = [-1.523, -0.001, -3.699, 4.103, 1.082, 1.912, -3.870]
GOAL = [-0.364, 0.797, -3.169, 1.478, 1.794, 1.115, -2.617]

# Matches our production config; the world is empty to isolate self-collision.
cfg = MotionPlannerCfg.create(robot=YML, scene_model=None, num_trajopt_seeds=12,
                              optimizer_collision_activation_distance=0.02)
planner = MotionPlanner(cfg)
planner.warmup(enable_graph=True, num_warmup_iterations=5)
jn = planner.joint_names

kin = yaml.safe_load(open(YML))["kinematics"]
ignore_map = kin["self_collision_ignore"]
sphere_link = [ln for ln in kin["collision_link_names"] for _ in kin["collision_spheres"][ln]]


def order(q):
    """Reorder a J1..J7 config into the planner's joint order."""
    m = dict(zip(JOINTS, q))
    return [m[j] for j in jn]


def joint_state(q):
    """JointState for a J1..J7 config."""
    pos = torch.tensor([order(q)], dtype=torch.float32, device="cuda")
    return JointState.from_position(pos, joint_names=jn)


def ignored(a, b):
    """True if link pair (a, b) is in the self-collision ignore map."""
    return b in (ignore_map.get(a) or []) or a in (ignore_map.get(b) or [])


def min_self_clearance(config):
    """Smallest non-ignored sphere-pair gap (m) at a configuration; negative means collision."""
    spheres = planner.kinematics.get_robot_as_spheres(
        torch.tensor([list(config)], dtype=torch.float32, device="cuda"))[0]
    centers = np.array([s.position for s in spheres])
    radii = np.array([float(s.radius) for s in spheres])
    worst = float("inf")
    for i in range(len(spheres)):
        for j in range(i + 1, len(spheres)):
            if sphere_link[i] == sphere_link[j] or ignored(sphere_link[i], sphere_link[j]):
                continue
            worst = min(worst, float(np.linalg.norm(centers[i] - centers[j]) - (radii[i] + radii[j])))
    return worst


start_t = torch.tensor([order(START)], dtype=torch.float32, device="cuda")
goal_t = torch.tensor([order(GOAL)], dtype=torch.float32, device="cuda")

# The graph planner solves the query on its own.
graph = planner.graph_planner.find_path(start_t, goal_t)
print(f"[1] graph_planner.find_path:  success={bool(graph.success.any())}  "
      f"path_length={float(graph.path_length):.2f} rad  time={float(graph.solve_time):.3f}s")

# plan_cspace fails on the same query.
result = planner.plan_cspace(joint_state(GOAL), joint_state(START), max_attempts=8)
print(f"[2] plan_cspace:              success={result is not None and bool(result.success.any())}")

# The raw graph path, with no optimization, is collision-free and reaches the goal.
graph_path = graph.interpolated_waypoints.reshape(-1, len(jn)).detach().cpu().numpy()
graph_gaps = [min_self_clearance(graph_path[i]) for i in range(0, len(graph_path), 5)]
goal_error = float(np.max(np.abs(graph_path[-1] - np.array(order(GOAL)))))
print(f"[3] raw graph path (no trajopt): {len(graph_path)} waypoints, "
      f"{sum(g < 0 for g in graph_gaps)} in self-collision, "
      f"min self-clearance {min(graph_gaps) * 1000:.1f} mm, goal error {goal_error:.4f} rad")

# The reference Tesseract/OMPL path is likewise collision-free in cuRobo's model.
tesseract = json.load(open(os.path.join(HERE, "tesseract_solution.json")))
tess_path = np.array([[dict(zip(tesseract["joint_names"], p))[j] for j in jn]
                      for p in tesseract["points"]])
tess_gaps = [min_self_clearance(tess_path[i]) for i in range(len(tess_path))]
print(f"[3b] Tesseract/OMPL solution ({len(tess_path)} waypoints): "
      f"{sum(g < 0 for g in tess_gaps)} in self-collision, "
      f"min self-clearance {min(tess_gaps) * 1000:.1f} mm")

print("\n>>> Two collision-free, goal-reaching solutions exist (graph + Tesseract); "
      "plan_cspace/trajopt produces neither.")

Expected behavior

Since graph_planner.find_path returns a valid collision-free, goal-reaching path and plan_cspace seeds trajopt from it, plan_cspace should succeed.

Question: Is trajopt expected to fail to follow a valid graph seed through a self-collision detour like this? The graph's own path clears self-collision by ~98 mm and hits the goal exactly with no optimization, yet trajopt (seeded with it) stalls J4 at the start rather than tracking the detour, and reports success=False. Is this a known limitation of the B-spline trajopt for tight/self-collision reconfigurations, or a bug in the graph→trajopt handoff?

Secondary question: what is the recommended configuration for self-collision-only detours where a valid graph path exists but trajopt won't converge to it? The following had no effect: graph budget ×5–×8 (max_path_finding_iterations, new_nodes_per_iteration); optimizer_collision_activation_distance 0.02→0.002; B-spline n_knots 16→64; feeding the known-good graph/OMPL path directly as seed_traj.

Actual behavior / error output

graph planner: ACTIVE
[1] graph_planner.find_path:  success=True  path_length=6.58 rad  time=0.088s
[2] plan_cspace:              success=False
[3] raw graph path (no trajopt): 100 waypoints, 0 in self-collision, min self-clearance 98.1 mm, goal error 0.0000 rad
[3b] Tesseract/OMPL solution (30 waypoints): 0 in self-collision, min self-clearance 12.1 mm

cuRobo version + commit SHA

cuRobo v0.8.0.post1.dev36 (commit a35a708)

Installation method

Source — CUDA 13 + PyTorch (uv pip install .[cu13-torch])

Kernel backend

cuda_core (default, runtime compilation)

Python version

Python 3.11.15 (also reproduced on 3.12.3)

PyTorch version (if installed)

2.13.0+cu130 (torch.version.cuda 13.0)

GPU / driver / CUDA toolkit

NVIDIA RTX 5060 Laptop (sm_120), driver 595.84, nvcc release 12.9

Operating system

Ubuntu 24.04.4 LTS

Isaac Sim version (if applicable)

No response

Additional context

curobo_repro.zip

The zip folder contains all the files needed to reproduce the issue, there is a --viser flag with which you can play the graph solution, the solution I received from my Tesseract stack

Additionally, I have tried to reduce the size of the spheres to 0.4x the current size and even then plan_cspace failed.