cumotion_planner_node doesn't take account for the attached hand when executing path planning

[wuisky]

Hi, thank you for developing such a so nice project.
I have a issue and need some help.

I am using isaac_ros_cumotion_moveit plugin and cumotion_planner_node doing manipulation path planning.
It works pretty nice.
But after i attach a hand to moveit planning scene, I noticed that cumotion_planner_node doesn't take account for the attached hand when executing path planning and the hand will collide with the environment object(green one). I think it was because the attached hand is not attched to MotionGen in execute_callback

I confirmed that the attached hand data is passed into scene here as scene.robot_state.attached_collision_objects, which is moveit_msgs.msg.AttachedCollisionObject, and I think maybe all i need is to call MotionGen.attach_external_objects_to_robot ?
However, I think it is necessary to transfer moveit_msgs.msg.AttachedCollisionObject to Obstacle class for passing it into argument of MotionGen.attach_external_objects_to_robot.
The contents of moveit_msgs.msg.AttachedCollisionObject is like:
moveit_msgs.msg.AttachedCollisionObject(link_name='tool0', object=moveit_msgs.msg.CollisionObject(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id='tool0'), pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=0.0, y=0.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=-2.887439455210694e-28, y=-6.073612672619587e-30, z=0.0, w=1.0)), id='robotiq_hand', type=object_recognition_msgs.msg.ObjectType(key='', db=''), primitives=[], primitive_poses=[], meshes=[shape_msgs.msg.Mesh(triangles=[shape_msgs.msg.MeshTriangle(vertex_indices=array([0, 1, 2], dtype=uint32)), shape_msgs.msg.MeshTriangle(vertex_indices=array([2, 1, 3], dtype=uint32))........lots of MeshTriangle and geometry_msgs/Point

Is it possible to transfer moveit_msgs.msg.AttachedCollisionObject to Obstacle object?
If not, how to take account for the attached hand when executing path planning?

Thank you in advance.

[wuisky]

update

Ok, I figured out how to convert moveit collision object msg to curobo obstacle.
I inserted follow lines at here

for obj in scene.robot_state.attached_collision_objects:
    cumotion_objects, supported_objects = self.get_cumotion_collision_object(obj.object)
    if supported_objects:
        self.get_logger().info('Attach object to robot')
        self.motion_gen.attach_external_objects_to_robot(start_state,
                                                         cumotion_objects,
                                                         link_name=obj.link_name)

However, still not work. There is a collision between attached hand and the world model in the result planned trajectory.
Any advice?