Trajectory optimizing doesn't work

[Li-jiansheng]

If it’s not a bug, please use discussions: https://github.com/NVlabs/curobo/discussions

Please provide the below information in addition to your issue:

1. cuRobo installation mode (choose from [python, isaac sim, docker python, docker isaac sim]):
2. python version:
3. Isaac Sim version (if using):

---

Issue Details

I want to optimize a series of joint trajectories (149,6),so that the robotic arm does not self-interfere or interfere with the ground, while trying to keep the trajectories as similar to the original ones as possible.

import torch
from curobo.wrap.reacher.trajopt import TrajOptSolver, WorldCollision
from curobo.types.robot import RobotConfig
from curobo.types.base import TensorDeviceType
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
from curobo.types.robot import JointState
from curobo.rollout.rollout_base import Goal
import numpy as np
from curobo.types.math import Pose
from curobo.util_file import (
get_robot_configs_path,
join_path,
load_yaml,
)

tensor_args = TensorDeviceType()

=== 3. 加载舞蹈轨迹数据 ===

file_path = "/home/li/POPDG/test_joint_angle/BV1oS4y1Y79W_Clip_3_slice1_150.csv"
raw_angles = np.loadtxt(file_path, delimiter=',', skiprows=1)

joint_trajectory = torch.tensor(raw_angles, device=tensor_args.device, dtype=tensor_args.dtype)

=== 扩展为 4 维张量（批量大小为 1）===

seed_traj = joint_trajectory.unsqueeze(0).unsqueeze(1) # 结果形状 [1, 1, 149, 6]

world_config = {
"cuboid": {
"ground": {
"dims": [10, 10, 0.01],
"pose": [0, 0, -0.005, 1, 0, 0, 0],
}
}
}

robot_file = "mycobot.yml"
robot_cfg = RobotConfig.from_dict(
load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
)

配置轨迹优化求解器参数

solver_config = TrajOptSolver.load_from_robot_config(
robot_cfg,
world_config,
rotation_threshold=0.05,
position_threshold=0.005,
cspace_threshold=0.05,
self_collision_check=True,
self_collision_opt=True,
use_cuda_graph=True,
traj_tsteps=153, # 设置与 joint_trajectory 一致

)

初始化求解器

solver = TrajOptSolver(solver_config)

初始化 start_state 和 goal_state

start_state = JointState(position=joint_trajectory[0])
goal_state = JointState(position=joint_trajectory[-1])

goal_c = solver.fk(goal_state.position) # 使用 goal_state 计算目标位置
goal_pose = Pose(goal_c.ee_position, goal_c.ee_quaternion)

封装成 Goal 对象

goal = Goal(
current_state=start_state,
goal_state=goal_state,
goal_pose=goal_pose,
batch=1
)

优化轨迹，避免自碰撞和地面碰撞

optimized_result = solver.solve_single(
goal=goal,
seed_traj=seed_traj,
return_all_solutions=False
)

print("qqqqq")

获取优化后的轨迹

optimized_trajectory = optimized_result.solution

插值优化后的轨迹，生成平滑路径

interpolated_trajectory, last_tstep, dt = solver.get_interpolated_trajectory(
optimized_trajectory
)

打印插值结果

print("Interpolated trajectory:", interpolated_trajectory)
print("Last time step:", last_tstep)
print("Optimal dt:", dt)

here is the wrong：
(POPDG) li@li-760D5:~/curobo/src/curobo/content/assets/robot/mycobot$ /home/li/anaconda3/envs/POPDG/bin/python /home/li/curobo/src/curobo/content/assets/robot/mycobot/mycobot_trajectory_optimize.py
Warp UserWarning: Python 3.9 or newer is recommended for running Warp, detected sys.version_info(major=3, minor=8, micro=19, releaselevel='final', serial=0)
jukashdasj
torch.Size([2, 149, 6])
torch.Size([2, 149, 6])
Traceback (most recent call last):
File "/home/li/curobo/src/curobo/content/assets/robot/mycobot/mycobot_trajectory_optimize.py", line 73, in
optimized_result = solver.solve_single(
File "/home/li/curobo/src/curobo/wrap/reacher/trajopt.py", line 979, in solve_single
return self._solve_from_solve_state(
File "/home/li/curobo/src/curobo/wrap/reacher/trajopt.py", line 918, in _solve_from_solve_state
result = self.solver.solve(goal_buffer, seed_traj)
File "/home/li/curobo/src/curobo/wrap/wrap_base.py", line 166, in solve
act_seq = self.optimize(seed, shift_steps=0)
File "/home/li/curobo/src/curobo/wrap/wrap_base.py", line 77, in optimize
act_seq = opt.optimize(act_seq, shift_steps)
File "/home/li/curobo/src/curobo/opt/opt_base.py", line 175, in optimize
out = self._optimize(opt_tensor, shift_steps, n_iters) # 调用内部优化方法
File "/home/li/curobo/src/curobo/opt/particle/particle_opt_base.py", line 211, in _optimize
self._initialize_cuda_graph(init_act.clone(), shift_steps=shift_steps) # 初始化 CUDA 图
File "/home/li/curobo/src/curobo/opt/particle/particle_opt_base.py", line 238, in _initialize_cuda_graph
self._cu_act_seq = self._run_opt_iters(self._cu_act_in, shift_steps=shift_steps)
File "/home/li/curobo/src/curobo/opt/particle/particle_opt_base.py", line 265, in _run_opt_iters
trajectory = self.generate_rollouts()
File "/home/li/anaconda3/envs/POPDG/lib/python3.8/site-packages/torch/utils/_contextlib.py", line 115, in decorate_context
return func(*args, **kwargs)
File "/home/li/curobo/src/curobo/opt/particle/parallel_mppi.py", line 613, in generate_rollouts
return super().generate_rollouts(init_act)
File "/home/li/curobo/src/curobo/opt/particle/particle_opt_base.py", line 178, in generate_rollouts
trajectories = self.rollout_fn(act_seq)
File "/home/li/curobo/src/curobo/rollout/rollout_base.py", line 478, in call
return self.rollout_fn(act)
File "/home/li/curobo/src/curobo/rollout/arm_base.py", line 598, in rollout_fn
state = self.dynamics_model.forward(
File "/home/li/curobo/src/curobo/rollout/dynamics_model/kinematic_model.py", line 455, in forward
state_seq = self.tensor_step(start_state_shaped, act_seq, state_seq, start_state_idx)
File "/home/li/curobo/src/curobo/rollout/dynamics_model/kinematic_model.py", line 392, in tensor_step
state_seq = self._rollout_step_fn.forward(state, act, state_seq, state_idx)
File "/home/li/curobo/src/curobo/rollout/dynamics_model/tensor_step.py", line 511, in forward
) = CliqueTensorStepIdxCentralDifferenceKernel.apply(
File "/home/li/anaconda3/envs/POPDG/lib/python3.8/site-packages/torch/autograd/function.py", line 506, in apply
return super().apply(*args, **kwargs) # type: ignore[misc]
File "/home/li/curobo/src/curobo/rollout/dynamics_model/integration_utils.py", line 527, in forward
) = tensor_step_pos_clique_idx_fwd(
File "/home/li/curobo/src/curobo/curobolib/tensor_step.py", line 56, in tensor_step_pos_clique_idx_fwd
r = tensor_step_cu.step_idx_position2(
TypeError: step_idx_position2(): incompatible function arguments. The following argument types are supported:
1. (arg0: torch.Tensor, arg1: torch.Tensor, arg2: torch.Tensor, arg3: torch.Tensor, arg4: torch.Tensor, arg5: torch.Tensor, arg6: torch.Tensor, arg7: torch.Tensor, arg8: torch.Tensor, arg9: torch.Tensor, arg10: int, arg11: int, arg12: int, arg13: int) -> List[torch.Tensor]

Invoked with: tensor([[[0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0.],
...,
[0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    ...,

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]]], device='cuda:0'), tensor([[[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    ...,

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]]], device='cuda:0'), tensor([[[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    ...,

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]]], device='cuda:0'), tensor([[[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    ...,

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]],

    [[0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     ...,
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.],
     [0., 0., 0., 0., 0., 0.]]], device='cuda:0'), tensor([[[ 5.2578e-01, -2.2193e+00,  1.9320e+00, -4.5122e-02,  5.5849e-02,
       4.8971e-02],
     [ 5.3033e-01, -2.2079e+00,  1.8892e+00, -1.3616e-02,  4.0835e-02,
       2.7083e-02],
     [ 5.3681e-01, -2.1919e+00,  1.8322e+00,  2.8761e-02,  1.9871e-02,
      -7.4910e-04],
     ...,
     [ 9.9207e-01, -1.1239e+00,  1.6011e+00, -8.2356e-01, -7.9485e-01,
      -8.1072e-01],
     [ 1.0180e+00, -1.1781e+00,  1.5927e+00, -7.5738e-01, -8.1259e-01,
      -8.2317e-01],
     [ 1.0304e+00, -1.2137e+00,  1.5983e+00, -7.1844e-01, -8.1614e-01,
      -8.1614e-01]],

    [[ 5.2496e-01, -2.2227e+00,  1.9299e+00, -4.2881e-02,  6.2935e-02,
       6.3258e-02],
     [ 5.2868e-01, -2.2148e+00,  1.8851e+00, -9.1343e-03,  5.5002e-02,
       5.5651e-02],
     [ 5.3434e-01, -2.2023e+00,  1.8260e+00,  3.5481e-02,  4.1112e-02,
       4.2092e-02],
     ...,
     [ 9.8071e-01, -1.1338e+00,  1.6035e+00, -8.2175e-01, -7.7339e-01,
      -7.6429e-01],
     [ 1.0105e+00, -1.1847e+00,  1.5943e+00, -7.5618e-01, -7.9828e-01,
      -7.9220e-01],
     [ 1.0304e+00, -1.2137e+00,  1.5983e+00, -7.1844e-01, -8.1614e-01,
      -8.1614e-01]],

    [[ 5.3358e-01, -2.2220e+00,  1.9453e+00, -4.3567e-02,  5.8081e-02,
       5.7350e-02],
     [ 5.4592e-01, -2.2134e+00,  1.9158e+00, -1.0507e-02,  4.5296e-02,
       4.3835e-02],
     [ 5.6018e-01, -2.2003e+00,  1.8721e+00,  3.3424e-02,  2.6561e-02,
       2.4369e-02],
     ...,
     [ 1.0093e+00, -1.1282e+00,  1.6413e+00, -8.1806e-01, -7.8319e-01,
      -7.8616e-01],
     [ 1.0295e+00, -1.1809e+00,  1.6195e+00, -7.5371e-01, -8.0481e-01,
      -8.0679e-01],
     [ 1.0304e+00, -1.2137e+00,  1.5983e+00, -7.1844e-01, -8.1614e-01,
      -8.1614e-01]],

    ...,

    [[ 5.2993e-01, -2.2207e+00,  1.9358e+00, -4.5656e-02,  5.0663e-02,
       5.5231e-02],
     [ 5.3639e-01, -2.2118e+00,  1.9342e+00, -5.3525e-02,  3.7814e-02,
       4.6948e-02],
     [ 5.4284e-01, -2.2029e+00,  1.9327e+00, -6.1386e-02,  2.4972e-02,
       3.8667e-02],
     ...,
     [ 1.0352e+00, -1.2182e+00,  1.6041e+00, -7.1844e-01, -8.2159e-01,
      -8.1030e-01],
     [ 1.0347e+00, -1.2144e+00,  1.6014e+00, -7.1996e-01, -8.2174e-01,
      -8.1422e-01],
     [ 1.0304e+00, -1.2137e+00,  1.5983e+00, -7.1844e-01, -8.1614e-01,
      -8.1614e-01]],

    [[ 5.1602e-01, -2.2193e+00,  1.9331e+00, -4.2662e-02,  6.6310e-02,
       5.6294e-02],
     [ 5.0857e-01, -2.2091e+00,  1.9289e+00, -4.7540e-02,  6.9100e-02,
       4.9072e-02],
     [ 5.0112e-01, -2.1989e+00,  1.9247e+00, -5.2418e-02,  7.1880e-02,
       4.1850e-02],
     ...,
     [ 9.9122e-01, -1.2134e+00,  1.5937e+00, -7.1443e-01, -7.7624e-01,
      -8.0869e-01],
     [ 1.0054e+00, -1.2112e+00,  1.5945e+00, -7.1729e-01, -7.9150e-01,
      -8.1314e-01],
     [ 1.0304e+00, -1.2137e+00,  1.5983e+00, -7.1844e-01, -8.1614e-01,
      -8.1614e-01]],

    [[ 5.2688e-01, -2.2228e+00,  1.9351e+00, -4.2351e-02,  5.7612e-02,
       5.7612e-02],
     [ 5.3028e-01, -2.2159e+00,  1.9328e+00, -4.6919e-02,  5.1708e-02,
       5.1708e-02],
     [ 5.3368e-01, -2.2091e+00,  1.9305e+00, -5.1488e-02,  4.5804e-02,
       4.5804e-02],
     ...,
     [ 1.0236e+00, -1.2273e+00,  1.6028e+00, -7.0930e-01, -8.0433e-01,
      -8.0433e-01],
     [ 1.0270e+00, -1.2205e+00,  1.6006e+00, -7.1387e-01, -8.1024e-01,
      -8.1024e-01],
     [ 1.0304e+00, -1.2137e+00,  1.5983e+00, -7.1844e-01, -8.1614e-01,
      -8.1614e-01]]], device='cuda:0'), tensor([ 0.5269, -2.2228,  1.9351, -0.0424,  0.0576,  0.0576], device='cuda:0'), None, None, tensor([[0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0],
    [0]], device='cuda:0', dtype=torch.int32), tensor([6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667,
    6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667, 6.6667],
   device='cuda:0'), 50, 153, 6, 0

I don't know how to do,could you please tell me what's the wrong or give me an example