attach collision objects when planning motion

[wuisky]

Hello, this PR is a workaround for #3.

Since the geometry information in attached_collision_objects is not in world frame but is with respect to local frame which the object is attached to,
I passed the argument "world_objects_pose_offset=ee_pose".

I'm not sure this is a good workaround but this patch works for me.
Hope this help.

[bmdyrdal]

Tested and works for me!

Should maybe detach the objects after planning?
self.motion_gen.detach_spheres_from_robot(link_name)

[Alice-ly]

Thankyou for providing solution, it also works for me;
And I plot all my robot sphere, and feel the argument "world_objects_pose_offset=ee_pose" is used for coordinate conversion. Because when the argument is not added, my robot sphere of attached object is far away from the manipulator instead of being in the manipulater.

[Daniel-LoCY]

Hi, I used your method to add an extra object to the gripper for collision avoidance, but I encountered the following issue. How can I solve it?

[cumotion_planner_node-6] [WARNING] [curobo] MG: No spheres found, max_spheres: 0 n_objects: 1

Thank you.

[wuisky]

if you start cumotion_planner_node like:

ros2 run isaac_ros_cumotion cumotion_planner_node --ros-args -p yml_file_path:=your_robot.yml   -p urdf_path:=your_robot.urdf 

make sure you have

 extra_collision_spheres: {"attached_linkname": 100}  // you can add as many spheres  as you want

in your_robot.yml. or, if you start cumotion_planner_node like:

ros2 run isaac_ros_cumotion cumotion_planner_node --ros-args -p robot:=your_robot.xrdf   -p urdf_path:=your_robot.urdf

make sure you already add

  attached_linkname:
        - center: [0.001, 0.001, 0.05]
          radius: -0.01
        - center: [0.001, 0.001, 0.05]
          radius: -0.01
        - center: [0.001, 0.001, 0.05]
          radius: -0.01
        .
        . 
        .  

in your_robot.xrdf. You can add as many spheres as you want here, too

[Daniel-LoCY]

Thanks, I'll give it another try.