[Bug]: `plan_grasp` fails because the goalset `plan_pose` at L446 needs to run twice for subsequent steps to succeed

[Instinct323]

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

MotionPlanner.plan_grasp() consistently returns success all False on the first invocation after warmup().

plan_grasp() internally calls plan_pose() in four steps:

1. L446 — goalset planning (_plan_pose_goalset, num_goalset > 1)
2. L487 — approach planning (_plan_pose_single)
3. L521 — grasp planning (_plan_pose_single)
4. L569 — lift planning (_plan_pose_single)

The plan_pose() call at L446 always succeeds individually. However, when invoked only once, the result it produces does not enable the subsequent steps (approach/grasp/lift) to succeed, causing plan_grasp to return success = False. Running L446's plan_pose() twice (via a loop, or a prior dummy plan_pose(goalset) call) makes the second result sufficient for Steps 2–4, and plan_grasp succeeds.

Scenario	Result
plan_pose() alone (first call after warmup)	✅ Succeeds
plan_grasp() alone — L446 plan_pose succeeds, but overall result False	❌ success all False
plan_pose(goalset) → plan_grasp() — L446 is 2nd goalset call	✅ Succeeds
plan_grasp() with L446 plan_pose called twice in a loop	✅ Succeeds

Steps to reproduce

from __future__ import annotations

import os
from dataclasses import dataclass, field
from functools import cached_property

import curobo.scene as cusc
import matplotlib.pyplot as plt
import numpy as np
import torch
import trimesh
from curobo._src.state.state_joint_ops import stack_joint_states
from curobo.config_io import resolve_config, join_path, load_yaml
from curobo.content import get_scene_configs_path
from curobo.motion_planner import MotionPlanner, MotionPlannerCfg
from curobo.types import DeviceCfg, Pose, JointState, GoalToolPose

os.environ["CUROBO_TORCH_COMPILE_DISABLE"] = "1"

# cuda settings
torch.backends.cudnn.benchmark = True
torch.backends.cudnn.allow_tf32 = True
torch.backends.cuda.matmul.allow_tf32 = True


def process_result(result, *args, **kwargs):
    success = result.success
    if not success.any(): return

    plans = [stack_joint_states(result.approach_interpolated_trajectory, result.grasp_interpolated_trajectory)[0, 0]]
    if result.lift_trajectory:
        plans.append(result.lift_interpolated_trajectory[0, 0])

    planner, vis_cfg = args
    if vis_cfg:
        traj = stack_joint_states(result.approach_trajectory, result.grasp_trajectory)
        if result.lift_trajectory:
            traj = stack_joint_states(traj, result.lift_trajectory)
        vis_cfg.visualize(traj[0, 0].position, planner).show()
    return plans


@dataclass
class PlanVisConfig:
    n_vis: int = 15
    cmap: str = "autumn_r"
    alpha_start: float = .5
    alpha_traj: float = .1
    alpha_end: float = 1.

    def __post_init__(self):
        assert self.n_vis >= 2

    def sample_indices(self, n: int) -> tuple[torch.Tensor, np.ndarray]:
        """ Sample indices for visualization. """
        i = torch.linspace(0, n - 1, self.n_vis).long()
        colors = plt.get_cmap(self.cmap)(i / i[-1])
        colors[0, -1], colors[1:-1, -1], colors[-1, -1] = self.alpha_start, self.alpha_traj, self.alpha_end
        return i, colors

    def visualize(self,
                  positions: torch.Tensor,
                  planner: "CuRoboPlanner") -> trimesh.Scene:
        i, colors = self.sample_indices(len(positions))

        # generate scene
        scene: list[cusc.Scene] = [planner.scene_robot(q[None], as_trimesh=False) for q in positions[i]]
        for s, c in zip(scene, colors): s.add_color(c)
        scene.insert(0, scene.pop())
        scene.append(planner.scene_world(as_trimesh=False))

        # to trimesh
        return trimesh.util.concatenate([cusc.Scene.get_scene_graph(s) for s in scene])


@dataclass
class CuRoboPlanner:
    """ reference: https://curobo.org/ """
    robot_cfg: str = "franka.yml"
    scene_cfg: cusc.Scene = .05
    device_cfg: DeviceCfg = field(default_factory=DeviceCfg)

    # Motion planner parameters
    interp_dt: float = 5e-4
    max_goalset: int = 1

    def __post_init__(self):
        if isinstance(self.scene_cfg, str):
            self.scene_cfg = load_yaml(resolve_config(join_path(get_scene_configs_path(), self.scene_cfg)))
        if not isinstance(self.scene_cfg, cusc.Scene):
            self.scene_cfg = cusc.Scene.create(self.scene_cfg)
        self.scene_cfg = cusc.Scene.create_collision_support_world(self.scene_cfg)

        assert len(self.motion_planner.tool_frames) == 1, "Only single tool frame is supported."

    @property
    def default_js(self) -> torch.Tensor:
        return self.motion_planner.default_joint_state.position

    @cached_property
    def motion_planner(self) -> MotionPlanner:
        mp_cfg = MotionPlannerCfg.create(
            robot=self.robot_cfg,
            scene_model=self.scene_cfg,
            device_cfg=self.device_cfg,
            max_goalset=self.max_goalset,
        )
        mp_cfg.trajopt_solver_config.interpolation_dt = self.interp_dt
        mp = MotionPlanner(mp_cfg)
        mp.warmup(enable_graph=True, num_warmup_iterations=5)
        return mp

    @cached_property
    def ik_solver(self):
        return self.motion_planner.ik_solver

    @cached_property
    def kinematics(self):
        return self.motion_planner.kinematics

    def plan_grasp(self,
                   q_start: torch.Tensor,
                   goal: torch.Tensor,
                   d_approach: float = .1,
                   d_retract: float = .1,
                   is_lift: bool = False,
                   vis_cfg: PlanVisConfig = None):
        q_start = JointState.from_position(q_start[None]).to(self.device_cfg)
        goal = self.process_goal(goal)
        # FIXME: black magic
        _x = self.motion_planner.plan_pose(goal, q_start)

        # process
        result = self.motion_planner.plan_grasp(
            goal, q_start,
            grasp_approach_offset=-d_approach,
            grasp_lift_offset=d_retract * (1 if is_lift else -1),
            plan_grasp_to_lift=d_retract > 0,
            grasp_lift_in_tool_frame=not is_lift,
        )
        print(result)
        return process_result(result, self, vis_cfg)

    def process_goal(self,
                     goal: torch.Tensor) -> GoalToolPose:
        dim = goal.shape[-1]
        if dim == 4:
            goal = Pose.from_matrix(goal).to(self.device_cfg)
            return GoalToolPose.from_poses(
                {k: v for k, v in zip(self.tool_frames, [goal])},
                num_goalset=len(goal)
            )
        elif dim == 7:
            return JointState.from_position(goal).to(self.device_cfg)
        else:
            raise ValueError(f"Invalid goal shape: {goal.shape}")

    def sample_states(self,
                      n: int) -> torch.Tensor:
        return self.ik_solver.sample_configs(n)

    def scene_robot(self,
                    q: torch.Tensor,
                    as_trimesh: bool = True) -> cusc.Scene | trimesh.Scene:
        q = self.device_cfg.to_device(q)[..., :self.kinematics.dof]
        scene = cusc.Scene(sphere=self.kinematics.get_robot_as_spheres(q)[0])
        return cusc.Scene.get_scene_graph(scene) if as_trimesh else scene

    def scene_world(self,
                    as_trimesh: bool = True) -> cusc.Scene | trimesh.Scene:
        return cusc.Scene.get_scene_graph(self.scene_cfg) if as_trimesh else self.scene_cfg

    def solve_fk(self, q: torch.Tensor) -> torch.Tensor:
        q = JointState.from_position(q)
        kin_state = self.kinematics.compute_kinematics(q).tool_poses
        return kin_state.get_link_pose(self.tool_frames[0]).get_matrix()

    @property
    def tool_frames(self) -> list[str]:
        return self.motion_planner.tool_frames


if __name__ == '__main__':
    torch.set_printoptions(precision=4, sci_mode=False)

    planner = CuRoboPlanner(scene_cfg="collision_test.yml", interp_dt=5e-4, max_goalset=10)
    vis_cfg = PlanVisConfig()

    for i in range(1):
        q = planner.sample_states(planner.max_goalset)
        goal = planner.solve_fk(q)

        planner.plan_grasp(planner.default_js, torch.as_tensor(goal), vis_cfg=vis_cfg, is_lift=True, d_retract=.1)

Expected behavior

plan_grasp() should succeed on the first call after warmup() has completed. The warmup() phase is expected to initialize all internal solver state so that the first real planning call produces valid results.

Actual behavior / error output

The first `plan_grasp()` call returns `success = False` for all goals. The goalset `plan_pose()` call at L446 succeeds individually (`goalset_result.success.any()` is True), but the result it produces does not enable the subsequent approach/grasp/lift steps to succeed. Running L446's `plan_pose()` twice produces a second result that does enable Steps 2–4, making `plan_grasp` succeed.

cuRobo version + commit SHA

0.8.0.post1.dev28+dirty @ 30843a4

Installation method

Source — CUDA 12, PyTorch pre-installed (uv pip install .[cu12])

Kernel backend

cuda_core (default, runtime compilation)

Python version

3.10.8

PyTorch version (if installed)

2.5.1+cu124

GPU / driver / CUDA toolkit

RTX 3090, Driver 570.133.07, CUDA 12.8

Operating system

Ubuntu 20.04, kernel 5.15.0-139-generica

Isaac Sim version (if applicable)

No response

Additional context

Internal Workaround

Modifying curobo/_src/motion/motion_planner.py around line 446:

# Current code — the goalset plan_pose only runs once (result insufficient for downstream steps):
goalset_result = self.plan_pose(grasp_poses, current_state)

# Workaround — run it twice (2nd result enables Steps 2-4):
for _ in range(2):
    goalset_result = self.plan_pose(grasp_poses, current_state)

Analysis

• The plan_pose() call at L446 always succeeds — even on the first call after warmup. The issue is not a failure.
• When L446 runs only once, the overall plan_grasp result is success = False (one of Steps 2–4 fails).
• When L446 runs twice (via loop or prior dummy call), the second result enables Steps 2–4 to succeed.

The pattern suggests that the first real _plan_pose_goalset() invocation produces a trajectory/IK solution that, while valid in isolation, is inadequate as input for the subsequent single-pose planning steps. The second invocation benefits from internal state changes during the first call, producing a higher-quality result. I don't have a definitive root cause — reporting in hopes that maintainers can pinpoint the mechanism.