Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Attaching objects to the robot's body

Many robots have some capability to pick up objects found in the environment. The robots may use a gripper to grasp an object, magnetized or velcro patches to pick up metal or cloth, or a basket or dust pan to lift an object. Once the robot has picked up an object it may wish to move in the environment without hitting the object against other objects, as doing so may represent a safety hazard to cause the robot to drop an object. Furthermore, a designer may occasionally want to attach something to the robot - an extra sensor not indicated in the robot's configuration or even a cosmetic decoration. In each case some object has been attached to the robot and will move with the robot; this object needs to be accounted for when checking states for collisions as well as when filtering out points from the collision map.

This tutorial covers the mechanism in the motion planning environment that exists for attaching known objects to the robot's body. These objects are assumed to be rigidly attached to some particular link of the robot's body. The attached object's position will then be updated with that link as a fixed transform. This tutorial will introduce how to attach objects to the robot's body as well as how to convert objects back and forth between being attached and being non-attached.

Setup

Follow the Setup instructions from the previous tutorial

Adding an attached object to the robot's gripper

In the arm_navigation_tutorials package, which you should have created in the previous tutorials, edit a file called attachCylinder.cpp and place in it the following:

   1 #include <ros/ros.h>
   2 #include <mapping_msgs/AttachedCollisionObject.h>
   3 #include <geometric_shapes_msgs/Shape.h>
   4 
   5 int main(int argc, char** argv) {
   6 
   7   ros::init(argc, argv, "attachCylinder");
   8 
   9   ros::NodeHandle nh;
  10 
  11   ros::Publisher att_object_in_map_pub_;
  12   att_object_in_map_pub_  = nh.advertise<mapping_msgs::AttachedCollisionObject>("attached_collision_object", 10);
  13   sleep(2);
  14 
  15   //add a cylinder into the collision space attached to the r_gripper_r_finger_tip_link
  16   mapping_msgs::AttachedCollisionObject att_object;
  17   att_object.link_name = "r_gripper_r_finger_tip_link";
  18   att_object.touch_links.push_back("r_gripper_palm_link");
  19   att_object.touch_links.push_back("r_gripper_r_finger_link");
  20   att_object.touch_links.push_back("r_gripper_l_finger_link");
  21   att_object.touch_links.push_back("r_gripper_l_finger_tip_link");
  22 
  23   att_object.object.id = "attached_pole";
  24   att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
  25   att_object.object.header.frame_id = "r_gripper_r_finger_tip_link";
  26   att_object.object.header.stamp = ros::Time::now();
  27   geometric_shapes_msgs::Shape object;
  28   object.type = geometric_shapes_msgs::Shape::CYLINDER;
  29   object.dimensions.resize(2);
  30   object.dimensions[0] = .02;
  31   object.dimensions[1] = 1.2;
  32   geometry_msgs::Pose pose;
  33   pose.position.x = 0.0;
  34   pose.position.y = 0.0;
  35   pose.position.z = 0.0;
  36   pose.orientation.x = 0;
  37   pose.orientation.y = 0;
  38   pose.orientation.z = 0;
  39   pose.orientation.w = 1;
  40   att_object.object.shapes.push_back(object);
  41   att_object.object.poses.push_back(pose);
  42   
  43   att_object_in_map_pub_.publish(att_object);
  44 
  45   ROS_INFO("Should have published");
  46 
  47   ros::Duration(2.0).sleep();
  48 
  49   ros::shutdown();

  11   ros::Publisher att_object_in_map_pub_;

Here we are creating the publisher for the Attached Collision Objects. Notice that the type and topic are different than for non-attached objects.

  15   //add a cylinder into the collision space attached to the r_gripper_r_finger_tip_link
  16   mapping_msgs::AttachedCollisionObject att_object;
  17   att_object.link_name = "r_gripper_r_finger_tip_link";
  18   att_object.touch_links.push_back("r_gripper_palm_link");
  19   att_object.touch_links.push_back("r_gripper_r_finger_link");
  20   att_object.touch_links.push_back("r_gripper_l_finger_link");

In these lines we declare the AttachedObject and specify two essential fields: link_name and touch_links. The link_name field declares the link to which we want to attach the body - the attached body must be attached to a single link. Which link you specify is very important, as the attached body is assumed to move with that link and no other. The touch_links field governs the self-collision behavior of the attached object. The attached object is allowed to collide with the link to which it is attached, but it may be the case that there are other links with which it should be allowed to collide. Say, for instance, that the PR2 has picked up an object in its right gripper and it has been attached to the r_gripper_r_finger_tip_link. The other finger tip is almost certain to be touching the object, as well as the palm link and finger links. If these links are not specified in the touch_links then all robot states and trajectories will have collisions between these links and the attached object.

  23   att_object.object.id = "attached_pole";
  24   att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
  25   att_object.object.header.frame_id = "r_gripper_r_finger_tip_link";
  26   att_object.object.header.stamp = ros::Time::now();
  27   geometric_shapes_msgs::Shape object;
  28   object.type = geometric_shapes_msgs::Shape::CYLINDER;
  29   object.dimensions.resize(2);
  30   object.dimensions[0] = .02;
  31   object.dimensions[1] = 1.2;
  32   geometry_msgs::Pose pose;
  33   pose.position.x = 0.0;
  34   pose.position.y = 0.0;
  35   pose.position.z = 0.0;
  36   pose.orientation.x = 0;
  37   pose.orientation.y = 0;
  38   pose.orientation.z = 0;
  39   pose.orientation.w = 1;
  40   att_object.object.shapes.push_back(object);

The attached object actually contains a known object, so this code should look very similar to the tutorial on known objects. One thing to note is that we are specifying the object's position in the frame associated with the link to which we are attaching the object - it will be centered on the frame's origin. Note that you can specify the attached object's position in any frame, and there is no requirement that it actually be touching the robot.

Running the code

Add the following line to arm_navigation_tutorials/CMakeLists.txt, compile the package, and run the attach_cylinder executable.

rosbuild_add_executable(attach_cylinder src/attachCylinder.cpp)

You should see something like this in rviz:

The pole is centered on the right gripper's right finger tip link as we specified. The brown color indicates that the object is attached; green objects are static known objects.

Checking states for validity given the attached body

In this tutorial we are not actually going to make the robot's arm move - we will cover that in a future tutorial. If the arm did move the attached object would track it at slight delay, as visualization markers are only update every second. But we will cover checking states for validity given the attached object. Edit get_state_validity from the previous tutorial or find it in pr2_arm_navigation_tutorials/src and replace the lines under the comment //these set whatever non-zero joint angle values are desired with the following, making sure that there are no other lines setting joint_states to non-zero positions:

    req.robot_state.joint_state.position[3] = -1.2;
    req.robot_state.joint_state.position[5] = -0.15;

Recompile get_state_validity and enable the 10th entry -- marked State Validity -- in rviz. You should see a ghosted robot in rviz. Now run get_state_validity. It should respond that there are no collisions and you should see something like this:

The ghosted robot will show the robot's position at the requested state, though not the position of the attached object at that position. Now go back to attachCylinder and comment out line 21:

     //att_object.touch_links.push_back("r_gripper_l_finger_tip_link");

Recompile and re-run attached_cylinder. Like known objects, publishing an attached object with the same name will replace the old object of the same name with the new one. Finally re-run get_state_validity. It should report collision and rviz should show:

Note that the selected points are reported as occurring with attached_pole and r_finger_r_finger_tip_link. Note that the collision positions occured at the padding position of the finger tip link, which is padded with 1 centimeter of padding as specified in pr2_arm_navigation_actions/config/environment_server_padding.yaml.

The attached object can collide not only with links of the robot but also with points or static objects in the environment. Uncomment the line in attachCylinder.cpp we just commented out, re-built, and re-run. And then add the following line to get_state_validity.cpp after the other two lines setting joint positions:

      req.robot_state.joint_state.position[6] = 1.57;

Recompile and rerun get_state_validity. It should report collision and you should see the following:

Note that with the wrist rotated the attached pole goes directly through the point cloud of the static pole, given a cylindrical pattern of the collision markers between attached_pole and points.

Converting between attached and static objects

It is likely that once a robot has picked up an object it will eventually want to put it down somewhere. One method to do this is to record the position of the attached object, delete the attached object from the environment entirely, and add the object back in as a static collision object. This would require multiple publications and lots of accounting, but to make it easier we implemented a method that just requires a single line edit of attachCylinder. Change line 24 in attach_cylinder.cpp to read:

   att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::DETACH_AND_ADD_AS_OBJECT;

You need not change the rest of the message as it is ignored - the object will be inserted as a static obstacle at its current position given the robot configuration. Recompile and run attach_cylinder. You should see the following:

Note that the object has turned green and that it has shrunk. When objects are attached they are padded, as it is assumed that when moving the robot will now need a buffer to avoid hitting the environment with the attached objects. The padding to be applied to attached objects is also specified in environment_server_padding.yaml. Again run get_state_validity. It should report collisions between the pole and the hand at the current position of the pole - touch links are no longer in effect, and the pole does not move with the hand.

Static objects can also be easily attached to the robot at their current position. Note that you must use an AttachedCollisionObject message for this, as you must specify the link to which to attach and the touch links. Replace line 24 in attach_cylinder.cpp with the following:

    att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::ATTACH_AND_REMOVE_AS_OBJECT;

Recompile and rerun attached_cylinder and the green object should be replaced with a padded brown cylinder again.