I am having problem using isaac_ros_cumotion with a new robot, more in depth problem is explained in this github discussion.
Since the discussion post, I have tried setting collision spheres for the whole robot and the problem persisted. I have also checked if the start state is valid for motion generation using self.motion_gen.check_start_state(start_state=start_state) and it is, it returns (True, None). Increasing MotionGenPlanConfig’s max_attempts and enable_graph_attempt also didn’t help.
Setting cumotion_planner_node’s enable_curobo_debug_mode parameter to True outputs:
[cumotion_planner_node-10] [INFO] [curobo] Creating Voxel cache1
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 25 ]
[cumotion_planner_node-10] [INFO] [curobo] ParallelMPPI: Updating sample set
[cumotion_planner_node-10] [INFO] [curobo] ParallelMPPI: Resetting sample iterations
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 4 ]
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 4 ]
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 25 ]
[cumotion_planner_node-10] [INFO] [curobo] ParallelMPPI: Updating sample set
[cumotion_planner_node-10] [INFO] [curobo] ParallelMPPI: Resetting sample iterations
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 4 ]
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 4 ]
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 25 ]
[cumotion_planner_node-10] [INFO] [curobo] ParallelMPPI: Updating sample set
[cumotion_planner_node-10] [INFO] [curobo] ParallelMPPI: Resetting sample iterations
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 4 ]
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 4 ]
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 2 , num_particles: 25 ]
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 2 , num_particles: 4 ]
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] *******USING EXISTING COLLISION CHECKER***********
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] breaking reference
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 4 ]
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 1 , num_particles: 4 ]
[cumotion_planner_node-10] [INFO] [curobo] Updating problem kernel [n_problems: 2 , num_particles: 4 ]
[cumotion_planner_node-10] [INFO] [curobo] Warmup
[cumotion_planner_node-10] [INFO] [curobo] Updating state_seq buffer reference (created new tensor)
[cumotion_planner_node-10] [INFO] [curobo] no valid configuration found
[cumotion_planner_node-10] [INFO] [curobo] no valid configuration found
[cumotion_planner_node-10] [INFO] [curobo] no valid configuration found
[cumotion_planner_node-10] [INFO] [curobo] Planning for Single Goal: 1
[cumotion_planner_node-10] [INFO] [curobo] Planning for Single Goal: 1
[cumotion_planner_node-10] [INFO] [curobo] Planning for Single Goal: 1
[cumotion_planner_node-10] [INFO] [curobo] Planning for Single Goal: 1
[cumotion_planner_node-10] [INFO] [curobo] Warmup complete
[cumotion_planner_node-10] [INFO] [1735909904.590934356] [cumotion_action_server]: cuMotion is ready for planning queries!
It seems that problem happens in GraphPlanBase class’ _cuda_graph_rollout_constraint(...) method where mask becomes False for every sample, but I cannot figure out why because I am not adding any additional constraints and start and goal state aren’t in collision.
Any help is appreciated.