Note: Some of the files in here haven't been released yet and may not work. Sorry!. This tutorial assumes that you have completed the previous tutorials: Adding Virtual Objects.
(!) 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 Virtual Objects to the Robot

Description: In this tutorial we describe attaching virtual objects to the robot's body - these are objects that are assumed to move with the robot instead of being static in the environment.

Keywords: attached_objects planning_scene electric

Tutorial Level: INTERMEDIATE

Setting up the tutorial

In one terminal run the following:

roslaunch pr2_arm_navigation_tutorials pr2_floorobj_world.launch

In another terminal run:

roslaunch pr2_arm_navigation_tutorials planning_scene_architecture.launch

This will bring up an Rviz window.

Running the tutorial

This tutorial uses the executable in pr2_arm_navigation_tutorials called get_state_validity_virtual_attached_object. In another terminal, run the following command:

rosrun pr2_arm_navigation_tutorials get_state_validity_virtual_attached_object

You should see something like the following (note that the object may flicker between green and brown in Rviz):

current state

In this example we've attached a pole to one of the robot's right fingers. The link to which the object is attached is very important. When we attach the object a fixed transform is computed between the link for attachment and the object - note that it's not required that the object actually touch the link for attachment, though we assume it's ok if they do touch. Then when we do forward kinematics in the future and update the link of attachment we will also update the position of the attached object given the position of the attached object. Thus if we chose to attach the object to the base link of the robot and moved the robot's arm then the object would not move, as we would not have updated the position of the base link. Generally the appropriate link to attach to is one that is actually in contact with the object, like the fingertip pads.

The second important part of the attached object specification are the touch links - the set of links that the object is allowed to touch. To illustrate, try the following command:

rosrun pr2_arm_navigation_tutorials get_state_validity_virtual_attached_object 0

You should get the following:

no touch links

In this case we're in the same position, but with a 0 as the first argument of the call we haven't added any touch links. This means that the attached object is now in collision with other parts of the gripper. We need to tell the collision checking algorithm that it's ok if the object contacts the gripper, though it's not ok if touches other parts on the right arm - we don't want to swing the poll into the upper arm, for instance. By default the only link that is added to the touch links is the link for attachment. Now quit and try the following:

rosrun pr2_arm_navigation_tutorials get_state_validity_virtual_attached_object 1 .8

You should get something that looks like this:

ok pose

Now we're adding the entire right gripper to the touch links and also specifying a second argument, which maps to a position for the r_wrist_roll_joint - as we give different commands it spins the wrist around. In this case we've attached the object to the robot's gripper, and when we update the position for the r_wrist_roll_joint, which is further up in the kinematic tree, it will also compute a new position for the fingers, and thus a new position for the object. The above pose for the wrist results in a valid state for the pole, but this command won't:

rosrun pr2_arm_navigation_tutorials get_state_validity_virtual_attached_object 1 .9

You should get something that looks like this:

collision environment

Rotating the wrist a little further brings the object in contact with the pole, which isn't allowed. Finally, quit and try this:

rosrun pr2_arm_navigation_tutorials get_state_validity_virtual_attached_object 1 -1.55

You should get something that looks like this:

self-collision

Spinning the other way brings us in contact with the other gripper. Though we've specified that it's ok for the attached object to touch the right gripper, it's not ok if it touches the left.

The code

   1 #include <ros/ros.h>
   2 #include <arm_navigation_msgs/GetPlanningScene.h>
   3 #include <planning_environment/models/collision_models.h>
   4 
   5 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
   6 
   7 int main(int argc, char **argv){
   8   ros::init (argc, argv, "get_state_validity_test");
   9   ros::NodeHandle rh;
  10 
  11   ros::Publisher vis_marker_publisher_;
  12   ros::Publisher vis_marker_array_publisher_;
  13 
  14   vis_marker_publisher_ = rh.advertise<visualization_msgs::Marker>("state_validity_markers", 128);
  15   vis_marker_array_publisher_ = rh.advertise<visualization_msgs::MarkerArray>("state_validity_markers_array", 128);
  16 
  17   ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
  18   ros::ServiceClient get_planning_scene_client = 
  19     rh.serviceClient<arm_navigation_msgs::GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME);
  20 
  21   arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
  22   arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
  23 
  24   bool add_touch = true;
  25   if(argc > 1) {
  26 
  27     std::stringstream s(argv[1]);
  28     s >> add_touch;
  29   }
  30 
  31   //add a cylinder into the collision space attached to the r_gripper_r_finger_tip_link
  32   arm_navigation_msgs::AttachedCollisionObject att_object;
  33   att_object.link_name = "r_gripper_r_finger_tip_link";
  34   
  35   att_object.object.id = "attached_pole";
  36   att_object.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
  37   
  38   att_object.object.header.frame_id = "r_gripper_r_finger_tip_link";
  39   att_object.object.header.stamp = ros::Time::now();
  40   arm_navigation_msgs::Shape object;
  41   object.type = arm_navigation_msgs::Shape::CYLINDER;
  42   object.dimensions.resize(2);
  43   object.dimensions[0] = .02;
  44   object.dimensions[1] = 1.2;
  45   geometry_msgs::Pose pose;
  46   pose.position.x = 0.0;
  47   pose.position.y = 0.0;
  48   pose.position.z = 0.0;
  49   pose.orientation.x = 0;
  50   pose.orientation.y = 0;
  51   pose.orientation.z = 0;
  52   pose.orientation.w = 1;
  53   att_object.object.shapes.push_back(object);
  54   att_object.object.poses.push_back(pose);
  55   if(add_touch) {
  56     att_object.touch_links.push_back("r_end_effector");
  57   }
  58   planning_scene_req.planning_scene_diff.attached_collision_objects.push_back(att_object);
  59   
  60   if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) {
  61     ROS_WARN("Can't get planning scene");
  62     return -1;
  63   }
  64 
  65   planning_environment::CollisionModels collision_models("robot_description");
  66   planning_models::KinematicState* state = 
  67     collision_models.setPlanningScene(planning_scene_res.planning_scene);
  68 
  69   std::vector<std::string> arm_names = 
  70     collision_models.getKinematicModel()->getModelGroup("right_arm")->getUpdatedLinkModelNames();
  71   std::vector<std::string> joint_names = 
  72     collision_models.getKinematicModel()->getModelGroup("right_arm")->getJointModelNames();
  73 
  74   if(argc > 2) {
  75     std::stringstream s(argv[2]);
  76     double val;
  77     s >> val;
  78     std::map<std::string, double> nvalues;
  79     nvalues["r_wrist_roll_joint"] = val;
  80     
  81     state->setKinematicState(nvalues);
  82   }
  83   
  84   std_msgs::ColorRGBA good_color, collision_color, joint_limits_color;
  85   good_color.a = collision_color.a = joint_limits_color.a = .8;
  86 
  87   good_color.g = 1.0;
  88   collision_color.r = 1.0;
  89   joint_limits_color.b = 1.0;
  90   
  91   std_msgs::ColorRGBA point_markers;
  92   point_markers.a = 1.0;
  93   point_markers.r = 1.0;
  94   point_markers.g = .8;
  95 
  96   std_msgs::ColorRGBA color;
  97   visualization_msgs::MarkerArray arr;
  98   if(!state->areJointsWithinBounds(joint_names)) {
  99     color = joint_limits_color;
 100   } else if(collision_models.isKinematicStateInCollision(*state)) {
 101     color = collision_color;
 102     collision_models.getAllCollisionPointMarkers(*state,
 103                                                  arr,
 104                                                  point_markers,
 105                                                  ros::Duration(0.2));
 106   } else {
 107     color = good_color;
 108   }
 109 
 110   collision_models.getRobotMarkersGivenState(*state,
 111                                              arr,
 112                                              color,
 113                                              "right_arm",
 114                                              ros::Duration(0.2),
 115                                              &arm_names);
 116 
 117   collision_models.getAttachedCollisionObjectMarkers(*state,
 118                                                      arr,
 119                                                      "right_arm",
 120                                                      color,
 121                                                      ros::Duration(0.2),
 122                                                      false,
 123                                                      &arm_names);
 124 
 125   while(ros::ok()) {    
 126     vis_marker_array_publisher_.publish(arr);
 127     ros::spinOnce();
 128     ros::Duration(.1).sleep();
 129   }
 130   collision_models.revertPlanningScene(state);
 131   ros::shutdown();
 132 }

Understanding the Code

The code is largely identical to the previous tutorial except for a couple a places. Here's where we setup the attached object:

  31   //add a cylinder into the collision space attached to the r_gripper_r_finger_tip_link
  32   arm_navigation_msgs::AttachedCollisionObject att_object;
  33   att_object.link_name = "r_gripper_r_finger_tip_link";
  34   
  35   att_object.object.id = "attached_pole";
  36   att_object.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
  37   
  38   att_object.object.header.frame_id = "r_gripper_r_finger_tip_link";
  39   att_object.object.header.stamp = ros::Time::now();
  40   arm_navigation_msgs::Shape object;
  41   object.type = arm_navigation_msgs::Shape::CYLINDER;
  42   object.dimensions.resize(2);
  43   object.dimensions[0] = .02;
  44   object.dimensions[1] = 1.2;
  45   geometry_msgs::Pose pose;
  46   pose.position.x = 0.0;
  47   pose.position.y = 0.0;
  48   pose.position.z = 0.0;
  49   pose.orientation.x = 0;
  50   pose.orientation.y = 0;
  51   pose.orientation.z = 0;
  52   pose.orientation.w = 1;
  53   att_object.object.shapes.push_back(object);
  54   att_object.object.poses.push_back(pose);
  55   if(add_touch) {
  56     att_object.touch_links.push_back("r_end_effector");
  57   }

The attached object specification contains an object as well as a few additional fields for link_name and touch_links. In this case we set the link name to be r_gripper_r_finger_tip_link, which is one of the links that will generally be in contact with an object. In this case we also specify the attachment point in the frame of the attached link, though any other frame can be used. The attachment point will be computed given the planning scene robot state. Finally, we set the entire r_end_effector group for the touch links - we could also specify individual links if we wanted to.

NOTE: if porting this tutorial to another robot, be sure that the frame to which you attach has a collision model, otherwise attachment will fail.

 117   collision_models.getAttachedCollisionObjectMarkers(*state,
 118                                                      arr,
 119                                                      "right_arm",
 120                                                      color,
 121                                                      ros::Duration(0.2),
 122                                                      false,
 123                                                      &arm_names);

Finally at the bottom we add the markers for attached objects to our marker array. The brown marker for the attached object is produced by the planning_scene_validity_server, and this program produces the green and red markers.

Wiki: arm_navigation/Tutorials/Planning Scene/Attaching Virtual Objects to the Robot (last edited 2012-01-06 18:35:58 by MichaelFerguson)