problem setting up a new robot and planning with isaac_sim_cumotion, start or end state in collision

[bornaparo]

Hi @balakumar-s,

I am trying to connect baxter robot with isaac_ros_cumotion. For a start, I want to plan only for it's left arm. I have created an xrdf file following the tutorial.

In _build_kinematics_with_lock_joints() I added:

lock_joints = {k: lock_joints[k] for k in lock_joints.keys() if k in joint_data.keys()}
lock_joint_names = list(lock_joints.keys())

because otherwise I was getting:

[cumotion_planner_node-10] Traceback (most recent call last):
[cumotion_planner_node-10]   File "/home/developer/curobo_ws/install/isaac_ros_cumotion/lib/isaac_ros_cumotion/cumotion_planner_node", line 33, in <module>
[cumotion_planner_node-10]     sys.exit(load_entry_point('isaac-ros-cumotion', 'console_scripts', 'cumotion_planner_node')())
[cumotion_planner_node-10]   File "/home/developer/curobo_ws/build/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py", line 754, in main
[cumotion_planner_node-10]     cumotion_action_server = CumotionActionServer()
[cumotion_planner_node-10]   File "/home/developer/curobo_ws/build/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py", line 162, in __init__
[cumotion_planner_node-10]     self.warmup()
[cumotion_planner_node-10]   File "/home/developer/curobo_ws/build/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py", line 233, in warmup
[cumotion_planner_node-10]     motion_gen_config = MotionGenConfig.load_from_robot_config(
[cumotion_planner_node-10]   File "/home/developer/curobo_ws/build/curobo_core/curobo/src/curobo/wrap/reacher/motion_gen.py", line 595, in load_from_robot_config
[cumotion_planner_node-10]     robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
[cumotion_planner_node-10]   File "/home/developer/curobo_ws/build/curobo_core/curobo/src/curobo/types/robot.py", line 50, in from_dict
[cumotion_planner_node-10]     data_dict["kinematics"] = CudaRobotModelConfig.from_config(
[cumotion_planner_node-10]   File "/home/developer/curobo_ws/build/curobo_core/curobo/src/curobo/cuda_robot_model/cuda_robot_model.py", line 126, in from_config
[cumotion_planner_node-10]     generator = CudaRobotGenerator(config)
[cumotion_planner_node-10]   File "/home/developer/curobo_ws/build/curobo_core/curobo/src/curobo/cuda_robot_model/cuda_robot_generator.py", line 225, in __init__
[cumotion_planner_node-10]     self.initialize_tensors()
[cumotion_planner_node-10]   File "/usr/lib/python3.10/contextlib.py", line 79, in inner
[cumotion_planner_node-10]     return func(*args, **kwds)
[cumotion_planner_node-10]   File "/home/developer/curobo_ws/build/curobo_core/curobo/src/curobo/cuda_robot_model/cuda_robot_generator.py", line 303, in initialize_tensors
[cumotion_planner_node-10]     self._build_kinematics_with_lock_joints(
[cumotion_planner_node-10]   File "/usr/lib/python3.10/contextlib.py", line 79, in inner
[cumotion_planner_node-10]     return func(*args, **kwds)
[cumotion_planner_node-10]   File "/home/developer/curobo_ws/build/curobo_core/curobo/src/curobo/cuda_robot_model/cuda_robot_generator.py", line 545, in _build_kinematics_with_lock_joints
[cumotion_planner_node-10]     if "mimic" in joint_data[k]:
[cumotion_planner_node-10] KeyError: 'right_s0

After adding those two lines, cumotion successfully warms up, but I keep getting:

[move_group-3] [INFO] [1734704652.712324134] [move_group]: Planning trajectory
[move_group-3] [INFO] [1734704652.713747881] [move_group]: Sending goal
[move_group-3] [INFO] [1734704652.724767818] [move_group]: Goal accepted by server, waiting for result
[cumotion_planner_node-10] [INFO] [1734704652.732612313] [cumotion_action_server]: Executing goal...
[cumotion_planner_node-10] [INFO] [1734704652.733515527] [cumotion_action_server]: Planning with time_dilation_factor: 0.8
[cumotion_planner_node-10] [INFO] [1734704653.135091719] [cumotion_action_server]: Calculating goal pose from Joint target
[cumotion_planner_node-10] [WARNING] [curobo] Start or End state in collision
[cumotion_planner_node-10] [ERROR] [1734704655.458937736] [cumotion_action_server]: Invalid planning query: MotionGenStatus.GRAPH_FAIL
[cumotion_planner_node-10] [INFO] [1734704655.459401101] [cumotion_action_server]: returned planning result (query, success, failure_status): 0 False MotionGenStatus.GRAPH_FAIL
[move_group-3] [INFO] [1734704655.460849334] [move_group]: Received result
[move_group-3] [INFO] [1734704655.460899900] [move_group]: Checking results
[move_group-3] [INFO] [1734704655.461029174] [move_group]: Success
[move_group-3] [INFO] [1734704655.466120642] [move_group]: Received trajectory result
[move_group-3] [ERROR] [1734704655.466213187] [move_group]: No trajectory
[move_group-3] [INFO] [1734704655.466705306] [moveit_move_group_default_capabilities.move_action_capability]: No motion plan found. No execution attempted.

Start and end state aren't in collision. I am using self.motion_gen.plan_single_js(...) method.

Before I added collision spheres to the torso link I was getting

[cumotion_planner_node-10] [ERROR] [1734705393.181088284] [cumotion_action_server]: Invalid planning query: MotionGenStatus.INVALID_START_STATE_SELF_COLLISION
[cumotion_planner_node-10] [INFO] [1734705393.182280951] [cumotion_action_server]: returned planning result (query, success, failure_status): 0 False MotionGenStatus.INVALID_START_STATE_SELF_COLLISION

but again, start and end state aren't in collision.

This is my urdf, and xrdf. I have also tried adding in xrdf under self_collision -> ignore that every link ignores every other link, but it didn't help.

This is an image of baxter and collision spheres inside isaac_sim:

I guess the problem is somewhere with xrdf file and setting up isaac_ros_cumotion with new robot, because it works well with other moveit planners, but I cannot figure out where and what I'am doing wrong. I tried various xrdf files; collision spheres on both arms, just left arm, left arm and torso, left arm and pedestal, but none of them resolved the problem.

Any help is appreciated, thanks in advance!!