Attached Collision Object Being Ignored

asked 2019-06-04 12:38:15 -0600

APettinger gravatar image

updated 2019-06-14 13:35:52 -0600

I have a manipulator with interchangeable end effectors. I am trying to attach the different end effectors to the end of the manipulator and add them to the collision scene so they do not collide with anything.

The AttachedCollisionObject message, including the mesh and frame information, is generated in a rospy rqt GUI and sent to an action server that does a bunch of planning and executing trajectory moves, including attaching and detaching EEFs. This is working well on the real robot, except for the collision objects. The collision objects look like they are added to the environment and attached to the robot in rViz. They show up (in transparent purple) and move with the manipulator in the correct fashion. However, the move_group does not seem to be counting it when considering collisions. Planning and executing still works as expected, but the attached object is allowed to collide with other objects in the environment (the table and manipulator, defined in the URDF). Interestingly, the rest of the collision scene is working in that no self collisions are allowed besides the attached end effector. Some screenshots:

This is the rViz output before attaching the collision model: image description

Immediately after attaching: image description

Planning so the attached tool collides with the table image description

This is the MoveIt! output during the planning phase while planning into (what should be) a collision

[ INFO] [1560536843.745995889]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ WARN] [1560536843.747570276]: Cannot find planning configuration for group 'manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead.
[ INFO] [1560536843.747920448]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1560536843.748462132]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1560536843.779068325]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1560536843.779108449]: Solution found in 0.031089 seconds
[ INFO] [1560536843.814415606]: SimpleSetup: Path simplification took 0.035256 seconds and changed from 3 to 2 states

The AttachedCollisionObject message creation:

def shovel_ee():
    shovel_pose = Pose()
    shovel_pose.orientation.w = 1

    shovel_collision = moveit_msgs.msg.AttachedCollisionObject()
    shovel_collision.object.header.frame_id = ee_frame
    shovel_collision.object.id = "Shovel"
    shovel_collision.link_name = ee_frame

    shovel_collision.object.meshes.append(create_moveit_mesh("/home/adamp/catkin_ws/src/Herbie/herbie_description/meshes/Tool_Shovel.stl"))
    shovel_collision.object.mesh_poses.append(shovel_pose)

    return shovel_collision

def attach_shovel():
    segment = herbie_interface.msg.AutonomousSegment()
    segment.segment_name = "Attach Shovel"
    segment.segment_type = segment.ATTACH_COLLISION_OBJECTS
    segment.collision_objects.append(shovel_ee())
    segment.collision_objects[0].object.operation = segment.collision_objects[0].object.ADD

    return segment

The segment.collision_objects in the attach_shovel function is sent to the action server to add it to the robot. That code is shown here:

moveit_msgs::AttachedCollisionObject attach_collision_object;
std::vector<moveit_msgs::CollisionObject> collision_objects;
moveit_msgs::PlanningScene planning_scene;
moveit_msgs::CollisionObject remove_object;

attach_collision_object = it->segment_collision_objects[0];
collision_objects.push_back(it->segment_collision_objects[0].object);
planning_scene.world.collision_objects.push_back(it->segment_collision_objects[0].object);
remove_object.id = "Shovel";
remove_object.header.frame_id = it->segment_collision_objects[0].object.id;
remove_object.operation = remove_object.REMOVE;
planning_scene.is_diff = true;
srv.request.scene = planning_scene;
planning_scene_diff_client_.call(srv);

planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene.robot_state.attached_collision_objects.push_back(attach_collision_object);
planning_scene.robot_state.is_diff = true;

srv.request ...
(more)
edit retag flag offensive close merge delete

Comments

Hi there, can you confirm that the link_name for the moveit_msgs::AttachedCollisionObject is set correct in the C++ part? I see it set in Python, but don't see a segment_collision_objects[0].link_name. This could resolve in an empty link_name.

HappyBit gravatar imageHappyBit ( 2019-06-11 06:55:59 -0600 )edit

Sure thing. Adding the line ROS_INFO_STREAM(attach_collision_object) right after attach_collision_object is set yields an output:

[ INFO] [1560435014.825297123]: link_name: temoto_end_effector
object: 
  header: 
    seq: 0
    stamp: 0.000000000
    frame_id: temoto_end_effector
  id: Shovel
  type: 
    key: 
    db: 
  primitives[]
  primitive_poses[]
  meshes[]
    meshes[0]: 
        triangles[]
        triangles[0]: 
                vertex_indices[]
            vertex_indices[0]: 0
            vertex_indices[1]: 1
            vertex_indices[2]: 2
....
....
  mesh_poses[]
    mesh_poses[0]: 
        position: 
        x: 0
        y: 0
        z: 0
      orientation: 
        x: 0
        y: 0
        z: 0
        w: 1
  planes[]
  plane_poses[]
  operation: 0
touch_links[]
detach_posture: 
  header: 
    seq: 0
    st
APettinger gravatar imageAPettinger ( 2019-06-13 09:23:31 -0600 )edit

From my understanding remove_object has the same object.id as the attach_collision_object. If this is the case the object with this ID is removed from the planning_scene. This means there is no object with the ID Shovel to add to the robot. Try adding it to the scene again attach_collision_object.object.operation = attach_collision_object.object.ADD;, or something similar. Why do you remove the object from the world tho?

HappyBit gravatar imageHappyBit ( 2019-06-13 09:52:45 -0600 )edit

The tutorial I linked near the bottom of my original question removes the object from the planning_scene at the same time as attaching it to the robot. I did however comment the line //planning_scene.world.collision_objects.push_back(remove_object); and did not see any change in behavior.

APettinger gravatar imageAPettinger ( 2019-06-14 09:54:38 -0600 )edit

I think your object was connected to the planning_scene in the right way. One way to check this is by using the Moveit Mation Planning Interface in rViz. Can you share your Moveit screenshots using an image hoster or something similar? Maybe I see something. Can you also share the output of your moveit node during this process?

HappyBit gravatar imageHappyBit ( 2019-06-14 10:59:59 -0600 )edit

Can you share your Moveit screenshots using an image hoster or something similar?

that would not seem to be necessary, as @APettinger has sufficient karma to attach screenshots to this question directly.

gvdhoorn gravatar imagegvdhoorn ( 2019-06-14 11:03:43 -0600 )edit

I have edited the question, attached pictures and MoveIt! output

APettinger gravatar imageAPettinger ( 2019-06-14 13:36:19 -0600 )edit

Are these meshes your collision models, or the visual ones (ie: detailed)? If this is all collision model quality, they are far too detailed and I would recommend subsampling/decimating them significantly.

gvdhoorn gravatar imagegvdhoorn ( 2019-06-14 15:10:30 -0600 )edit