Attached Collision Object Being Ignored
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:
Immediately after attaching:
Planning so the attached tool collides with the table
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 ...
Hi there, can you confirm that the
link_name
for themoveit_msgs::AttachedCollisionObject
is set correct in the C++ part? I see it set in Python, but don't see asegment_collision_objects[0].link_name
. This could resolve in an emptylink_name
.Sure thing. Adding the line
ROS_INFO_STREAM(attach_collision_object)
right afterattach_collision_object
is set yields an output:From my understanding
remove_object
has the sameobject.id
as theattach_collision_object
. If this is the case the object with this ID is removed from theplanning_scene
. This means there is no object with the ID Shovel to add to the robot. Try adding it to the scene againattach_collision_object.object.operation = attach_collision_object.object.ADD;
, or something similar. Why do you remove the object from the world tho?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.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?that would not seem to be necessary, as @APettinger has sufficient karma to attach screenshots to this question directly.
I have edited the question, attached pictures and MoveIt! output
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.