search

NVIDIA-ISAAC-ROS / isaac_ros_cumotion

NVIDIA-accelerated packages for arm motion planning and control
https://developer.nvidia.com/isaac-ros-gems
Other
51 stars 6 forks source link

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

Open wuisky opened 2 months ago

wuisky commented 2 months ago

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 Screenshot from 2024-06-13 20-42-57

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 commented 2 months ago

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?