We’re using the integration of CuMotion with moveit: isaac_ros_cumotion_moveit — isaac_ros_docs documentation
For some target goals CuMotion works (when it’s “easy” to find a collision free trajectory). For some cases, CuMotion doesn’t work, where it ignores the planning scene published to the topic /planning_scene: The CuMotion pipeline finds a plan, but Rviz rejects it stating that there are collisions found along the trajectory. For further investigations, we tried to “see” what CuMotion sees, so we tried the ROS 2 Bridge in Omniverse Isaac Sim, and we inspected collisions, between the collision spheres and other components. We added collision spheres where collisions were inspected, still there were collisions between the spheres.
Hi @mohammad12
Thank you for your message, but to help you better, can you provide a more specific example to forward to engineering?
Best,
Raffaello
Here is an example of the output of cumotion:
[INFO] [1731235415.204882890] [cumotion_action_server]: Executing goal…
[INFO] [1731235415.205204074] [cumotion_action_server]: Planning with time_dilation_factor: 0.7999999999999999
[INFO] [1731235415.206170879] [cumotion_action_server]: Calculating goal pose from Joint target
[WARNING] [curobo] Interpolation buffer shape is smaller than steps_max, creating new buffer of shape tensor(54, device=‘cuda:0’, dtype=torch.int32)
[INFO] [1731235415.286236352] [cumotion_action_server]: returned planning result (query, success, failure_status): 5 True None
Where in Rviz the corresponding response is:
[rviz2-2] [INFO] [1731235587.323808179] [move_group_interface]: MoveGroup action client/server ready
[move_group-7] [INFO] [1731235587.324542091] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[rviz2-2] [INFO] [1731235587.324723066] [move_group_interface]: Planning request accepted
[move_group-7] [INFO] [1731235587.324727522] [moveit_move_group_default_capabilities.move_action_capability]: executing…
[move_group-7] [INFO] [1731235587.324811675] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-7] [INFO] [1731235587.324822208] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline ‘isaac_ros_cumotion’
[move_group-7] [INFO] [1731235587.325459309] [move_group]: Planning trajectory
[move_group-7] [INFO] [1731235587.325757003] [move_group]: Sending goal
[move_group-7] [INFO] [1731235587.329270400] [move_group]: Goal accepted by server, waiting for result
[move_group-7] [INFO] [1731235587.412020122] [move_group]: Received result
[move_group-7] [INFO] [1731235587.412027394] [move_group]: Checking results
[move_group-7] [INFO] [1731235587.412096290] [move_group]: Success
[move_group-7] [INFO] [1731235587.417166934] [move_group]: Received trajectory result
[move_group-7] [INFO] [1731235587.417191631] [move_group]: Trajectory success!
[move_group-7] [ERROR] [1731235587.420485368] [moveit.ros_planning.planning_pipeline]: Computed path is not valid. Invalid states at index locations: [ 8 9 10 11 12 13 14 15 16 17 18 19 20 21 ] out of 32. Explanations follow in command line. Contacts are published on /display_contacts
[move_group-7] [INFO] [1731235587.420629902] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_left_finger’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.420638198] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.420989671] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_left_finger’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.420995720] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.421298917] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_left_inner_knuckle’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.421305739] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.421541683] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_left_inner_knuckle’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.421548028] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.421910835] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_left_outer_knuckle’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.421917407] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.422376017] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_link4’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.422382632] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.422854201] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_link4’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.422861478] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.423128685] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_link4’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.423134813] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.423742207] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_link4’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.423750439] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.424060850] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_link4’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.424067514] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.424367534] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_link4’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.424373373] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.424710035] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_link4’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.424716593] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.425087785] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_link5’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.425093995] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [INFO] [1731235587.425666074] [moveit_collision_detection_fcl.collision_common]: Found a contact between ‘dr_body’ (type ‘Robot link’) and ‘L_link5’ (type ‘Robot link’), which constitutes a collision. Contact information is not stored.
[move_group-7] [INFO] [1731235587.425673315] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-7] [ERROR] [1731235587.425883783] [moveit.ros_planning.planning_pipeline]: Completed listing of explanations for invalid states.
[move_group-7] [INFO] [1731235587.426030155] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.
[rviz2-2] [INFO] [1731235587.426315277] [move_group_interface]: Planning request aborted
[rviz2-2] [ERROR] [1731235587.427350924] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached
Here is an image for the current pose and the target pose:
In Isaac Sim the robot went through the static object
Hey @mohammad12 ! Could you please provide a minimal setup for us to replicate the issue? Thanks!
for_forum.zip (6.5 MB)
Thank you! Here is a minimal setup with 2 ROS packages to run Moveit (ROS Humble) and an XRDF (you can find the URDF in dr_description package).
When I add spheres to this minimal setup (the wheel and the fork,) it seems to work, but I can’t add collision spheres to every object in the full setup. You can find in ‘Goal State’ two configurations (Start1 and End1) to replicate the scenario.
Another thing is that when you try to publish an object to the scene, CuMotion ignores it. For example, if you publish a box as the floor (2x2x0.01 at position (0,0,-0.01) and try to move the arm from Start2 to End2, the robot will go through the box.
What I concluded is that CuMotion doesn’t account for changes in the planning scene, but when I tried to dive into CuMotion code, I found that in the script cumotion_planner.py in isaac_ros_cumotion package that there is some lines that refers to the changes in the planning scene, for example line 506: ‘scene = goal_handle.request.planning_options.planning_scene_diff’. I hope that you can help with this too.
