[Bug]: Excessive zero-padding in 'plan_grasp' interpolated trajectory causes visualization loop freeze

[LucioYipengLi]

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

When running planner.plan_grasp() in motion_planning.py (eg: with --visualize), the playback loop in execute_trajectory() appears to freeze or hang for a long duration (~100 seconds) after the GPU solves the motion plan.

Upon debugging, this is caused by excessive trailing zero/static padding in the returned trajectories:

• planner.plan_grasp returns interpolated trajectories (approach_interpolated_trajectory, grasp_interpolated_trajectory, etc.) padded to a fixed tensor size of 5000 steps (shape[-2] = 5000).
• Out of these 5000 steps, only a small fraction (e.g., ~40 steps) represents actual robot movement. The remaining ~4960 steps are trailing padded waypoints where joint positions remain identical to the final state.
• Passing these untrimmed trajectories directly into the execution loop (execute_trajectory()) causes the program to execute time.sleep(0.02) 5000 times sequentially. This results in the robot standing still for ~1.6 minutes before progressing to the next stage, creating the illusion of a deadlock or unresponsiveness.

Steps to reproduce

import time
import torch
from curobo.motion_planner import MotionPlanner, MotionPlannerCfg
from curobo.types import GoalToolPose, JointState

# Initialize MotionPlanner using official grasp example config
config = MotionPlannerCfg.create(
    robot="franka.yml",
    scene_model="collision_test.yml",
    max_goalset=10,
)
planner = MotionPlanner(config)
planner.warmup(enable_graph=True, num_warmup_iterations=5)

# Setup initial joint state
q_start = JointState.from_position(
    planner.default_joint_state.position.unsqueeze(0),
    joint_names=planner.joint_names,
)

# Setup candidate grasp poses
n_grasps = 3
positions = torch.zeros(1, 1, 1, n_grasps, 3, device="cuda", dtype=torch.float32)
positions[..., 0] = 0.5
positions[0, 0, 0, :, 1] = torch.linspace(-0.15, 0.15, n_grasps)
positions[..., 2] = 0.3

quaternions = torch.zeros(1, 1, 1, n_grasps, 4, device="cuda", dtype=torch.float32)
quaternions[..., 0] = 1.0  # wxyz quaternion

grasp_poses = GoalToolPose(
    tool_frames=planner.tool_frames,
    position=positions,
    quaternion=quaternions,
)

# Run grasp planning
result = planner.plan_grasp(
    current_state=q_start,
    grasp_poses=grasp_poses,
    grasp_approach_offset=0.1,
    grasp_lift_offset=0.1,
    plan_approach_to_grasp=True,
    plan_grasp_to_lift=True,
    grasp_lift_in_tool_frame=True,
)

# Inspect approach trajectory step count
approach_traj = result.approach_interpolated_trajectory
total_steps = approach_traj.position.shape[-2]
print(f"Trajectory tensor steps: {total_steps}")  # Outputs 5000 due to static memory padding

# Simulate the playback loop from execute_trajectory() in motion_planning.py
print("Simulating execution loop without trimming...")

# start_time = time.time()
# for i in range(total_steps):
#     time.sleep(0.02)  # 5000 steps * 0.02s = 100s blocking loop
# print(f"Visualization loop completed in: {time.time() - start_time:.2f} seconds")

print(f"Visualization loop completed in: {0.02*total_steps} seconds")

Expected behavior

In the result returned by executing planner.plan_grasp(), the shape length of a trajectory (e.g., approach_interpolated_trajectory) should be consistent with the planned content, rather than being padded to a uniform shape (which is 5000 in this case).

Actual behavior / error output

$ python curobo/examples/getting_started/motion_planning.py
=== Pose-to-Pose Motion Planning ===
✓ Planning succeeded!
Trajectory has 61 waypoints
Duration: 1.53s
Trajectory plot saved to: /home/******/.cache/curobo/examples/motion_planning/motion_plan.pdf

=== Grasp Planning ===
✓ Grasp planning succeeded!
  Approach: 5000 waypoints
  Grasp:    5000 waypoints
  Lift:     5000 waypoints
Trajectory plot saved to: /home/******/.cache/curobo/examples/motion_planning/grasp_plan.pdf

cuRobo version + commit SHA

0.8.0.post1.dev36+dirty @ a35a708

Installation method

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

Kernel backend

cuda_core (default, runtime compilation)

Python version

3.11.15

PyTorch version (if installed)

2.12.1+cu130 13.0

GPU / driver / CUDA toolkit

RTX 5060Ti, Driver Version: 591.86, Cuda_13.0

Operating system

Ubuntu 22.04.5 LTS, Linux 6.6.87.2-microsoft-standard-WSL2

Isaac Sim version (if applicable)

No response

Additional context

Running '$ python curobo/examples/getting_started/motion_planning.py' directly will result in the same issue.

Temporary solution

Place the following code inside the def execute_trajectory(trajectory) function, specifically below traj = trajectory.squeeze(0) and above for i in range(valid_len):. (The number of iterations in the for loop must be changed to the length of the obtained normal trajectory.)

positions_cpu = traj.position.cpu()
last_step = positions_cpu[0, -1, :]
moving_mask = torch.any(torch.abs(positions_cpu[0, :, :] - last_step) > 1e-4, dim=-1)
if moving_mask.any():
    valid_len = int(torch.where(moving_mask)[0][-1]) + 2
    valid_len = min(valid_len, positions_cpu.shape[-2])
else:
    valid_len = 1

The temporary solution is actually to filter out stagnant trajectories and obtain the truly moving ones, but this method has its limitations.

[LucioYipengLi]

I have currently identified the issues present in the source code. I'd like to work on a fix for this and submit a PR! 😀