calvin_pick_n_place.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <tf/tf.h>
00004 
00005 #include <moveit/move_group_interface/move_group.h>
00006 
00007 #include <geometry_msgs/PoseStamped.h>
00008 #include <moveit_msgs/AttachedCollisionObject.h>
00009 #include <moveit_msgs/CollisionObject.h>
00010 #include <moveit_msgs/Grasp.h>
00011 #include <visualization_msgs/Marker.h>
00012 #include <visualization_msgs/MarkerArray.h>
00013 
00014 #include <shape_tools/solid_primitive_dims.h>
00015 
00016 ros::Publisher pub_co;
00017 ros::Publisher pub_aco;
00018 ros::Publisher grasps_marker;
00019 
00020 moveit_msgs::CollisionObject co;
00021 moveit_msgs::AttachedCollisionObject aco;
00022 
00023 void add_collision_object() {
00024   ROS_INFO("Adding collision object.");
00025   co.operation = moveit_msgs::CollisionObject::ADD;
00026   pub_co.publish(co);
00027 }
00028 void remove_collision_object() {
00029   ROS_INFO("Removing collision object.");
00030   co.operation = moveit_msgs::CollisionObject::REMOVE;
00031   pub_co.publish(co);
00032 }
00033 void add_attached_collision_object() {
00034   ROS_INFO("Adding attached collision object.");
00035   aco.object.operation = moveit_msgs::CollisionObject::ADD;
00036   pub_aco.publish(aco);
00037 }
00038 void remove_attached_collision_object() {
00039   ROS_INFO("Removing attached collision object.");
00040   aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
00041   pub_aco.publish(aco);
00042 }
00043 
00044 moveit_msgs::Grasp tf_transform_to_grasp(tf::Transform t)
00045 {
00046   static int i = 0;
00047 
00048   moveit_msgs::Grasp grasp;
00049   geometry_msgs::PoseStamped pose;
00050 
00051   tf::Vector3& origin = t.getOrigin();
00052   tf::Quaternion rotation = t.getRotation();
00053 
00054   tf::quaternionTFToMsg(rotation, pose.pose.orientation);
00055 
00056   pose.header.frame_id = "base_footprint";
00057   pose.header.stamp = ros::Time::now();
00058   pose.pose.position.x = origin.m_floats[0];
00059   pose.pose.position.y = origin.m_floats[1];
00060   pose.pose.position.z = origin.m_floats[2];
00061   //pose.pose.orientation.x = 0;
00062   //pose.pose.orientation.y = 0;
00063   //pose.pose.orientation.z = 0;
00064   //pose.pose.orientation.w = 1;
00065   grasp.grasp_pose = pose;
00066 
00067   grasp.id = boost::lexical_cast<std::string>(i);
00068 
00069   grasp.pre_grasp_approach.direction.vector.z = 1.0;
00070   grasp.pre_grasp_approach.direction.header.frame_id = "katana_gripper_tool_frame";
00071   grasp.pre_grasp_approach.min_distance = 0.05;
00072   grasp.pre_grasp_approach.desired_distance = 0.1;
00073   grasp.pre_grasp_approach.direction.header.stamp = ros::Time::now();
00074 
00075   grasp.post_grasp_retreat.direction.vector.z = 1.0;
00076   grasp.post_grasp_retreat.min_distance = 0.05;
00077   grasp.post_grasp_retreat.desired_distance = 0.1;
00078   grasp.post_grasp_retreat.direction.header.frame_id = "base_footprint";
00079   grasp.post_grasp_retreat.direction.header.stamp = ros::Time::now();
00080 
00081   // TODO: fill in grasp.post_place_retreat (copy of post_grasp_retreat?)
00082 
00083   grasp.pre_grasp_posture.joint_names.push_back("katana_l_finger_joint");
00084   grasp.pre_grasp_posture.joint_names.push_back("katana_r_finger_joint");
00085   grasp.pre_grasp_posture.points.resize(1);
00086   grasp.pre_grasp_posture.points[0].positions.push_back(0.3);
00087   grasp.pre_grasp_posture.points[0].positions.push_back(0.3);
00088 
00089   grasp.grasp_posture.joint_names.push_back("katana_l_finger_joint");
00090   grasp.grasp_posture.joint_names.push_back("katana_r_finger_joint");
00091   grasp.grasp_posture.points.resize(1);
00092   grasp.grasp_posture.points[0].positions.push_back(-0.44);
00093   grasp.grasp_posture.points[0].positions.push_back(-0.44);
00094 
00095   grasp.allowed_touch_objects.resize(1);
00096   grasp.allowed_touch_objects[0] = "testbox";
00097 
00098   i++;
00099   return grasp;
00100 }
00101 
00102 void publish_grasps_as_markerarray(std::vector<moveit_msgs::Grasp> grasps)
00103 {
00104   visualization_msgs::MarkerArray markers;
00105   int i = 0;
00106 
00107   for(std::vector<moveit_msgs::Grasp>::iterator it = grasps.begin(); it != grasps.end(); ++it) {
00108     visualization_msgs::Marker marker;
00109     marker.header.stamp = ros::Time::now();
00110     marker.header.frame_id = "base_footprint";
00111     marker.id = i;
00112     marker.type = 1;
00113     marker.ns = "graspmarker";
00114     marker.pose = it->grasp_pose.pose;
00115     marker.scale.x = 0.02;
00116     marker.scale.y = 0.02;
00117     marker.scale.z = 0.2;
00118     marker.color.b = 1.0;
00119     marker.color.a = 1.0;
00120     markers.markers.push_back(marker);
00121     i++;
00122   }
00123 
00124   grasps_marker.publish(markers);
00125 }
00126 
00130 std::vector<moveit_msgs::Grasp> generate_grasps(double x, double y, double z)
00131 {
00132   static const double ANGLE_INC = M_PI / 16;
00133   static const double STRAIGHT_ANGLE_MIN = 0.0 + ANGLE_INC;  // + ANGLE_INC, because 0 is already covered by side grasps
00134   static const double ANGLE_MAX = M_PI / 2;
00135 
00136   // how far from the grasp center should the wrist be?
00137   static const double STANDOFF = -0.01;
00138 
00139   std::vector<moveit_msgs::Grasp> grasps;
00140 
00141   tf::Transform transform;
00142 
00143   tf::Transform standoff_trans;
00144   standoff_trans.setOrigin(tf::Vector3(STANDOFF, 0.0, 0.0));
00145   standoff_trans.setRotation(tf::Quaternion(0.0, sqrt(2)/2, 0.0, sqrt(2)/2));
00146 
00147   // ----- side grasps
00148   //
00149   //  1. side grasp (xy-planes of `katana_motor5_wrist_roll_link` and of `katana_base_link` are parallel):
00150   //     - standard: `rpy = (0, 0, *)` (orientation of `katana_motor5_wrist_roll_link` in `katana_base_link` frame)
00151   //     - overhead: `rpy = (pi, 0, *)`
00152   transform.setOrigin(tf::Vector3(x, y, z));
00153 
00154   for (double roll = 0.0; roll <= M_PI; roll += M_PI)
00155   {
00156     double pitch = 0.0;
00157 
00158     // add yaw = 0 first, then +ANGLE_INC, -ANGLE_INC, 2*ANGLE_INC, ...
00159     // reason: grasps with yaw near 0 mean that the approach is from the
00160     // direction of the arm; it is usually easier to place the object back like
00161     // this
00162     for (double yaw = ANGLE_INC; yaw <= ANGLE_MAX; yaw += ANGLE_INC)
00163     {
00164       // + atan2 to center the grasps around the vector from arm to object
00165       transform.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw + atan2(y, x)));
00166       grasps.push_back(tf_transform_to_grasp(transform * standoff_trans));
00167 
00168       if (yaw != 0.0)
00169       {
00170         transform.setRotation(tf::createQuaternionFromRPY(roll, pitch, -yaw + atan2(y, x)));
00171         grasps.push_back(tf_transform_to_grasp(transform * standoff_trans));
00172       }
00173     }
00174   }
00175 
00176   // ----- straight grasps
00177   //
00178   //  2. straight grasp (xz-plane of `katana_motor5_wrist_roll_link` contains z axis of `katana_base_link`)
00179   //     - standard: `rpy = (0, *, atan2(y_w, x_w))`   (x_w, y_w = position of `katana_motor5_wrist_roll_link` in `katana_base_link` frame)
00180   //     - overhead: `rpy = (pi, *, atan2(y_w, x_w))`
00181   for (double roll = 0.0; roll <= M_PI; roll += M_PI)
00182   {
00183     for (double pitch = STRAIGHT_ANGLE_MIN; pitch <= ANGLE_MAX; pitch += ANGLE_INC)
00184     {
00185       double yaw = atan2(y, x);
00186       transform.setOrigin(tf::Vector3(x, y, z));
00187       transform.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw));
00188 
00189       grasps.push_back(tf_transform_to_grasp(transform * standoff_trans));
00190     }
00191   }
00192 
00193   publish_grasps_as_markerarray(grasps);
00194 
00195   return grasps;
00196 }
00197 
00198 bool place(moveit::planning_interface::MoveGroup &group)
00199 {
00200   static const double ANGLE_INC = M_PI / 16;
00201 
00202   std::vector<moveit_msgs::PlaceLocation> loc;
00203 
00204   for (double yaw = -M_PI; yaw < M_PI; yaw += ANGLE_INC)
00205   {
00206     geometry_msgs::PoseStamped p;
00207     p.header.frame_id = "base_footprint";
00208     p.pose.position.x = 0.21;
00209     p.pose.position.y = 0.115;
00210     p.pose.position.z = 0.78;
00211     tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 0.0, yaw), p.pose.orientation);
00212 
00213     moveit_msgs::PlaceLocation g;
00214     g.place_pose = p;
00215 
00216     g.pre_place_approach.direction.vector.z = -1.0;
00217     g.pre_place_approach.direction.header.frame_id = "base_link";
00218     g.pre_place_approach.min_distance = 0.05;
00219     g.pre_place_approach.desired_distance = 0.1;
00220 
00221     g.post_place_retreat.direction.vector.z = 1.0;
00222     g.post_place_retreat.direction.header.frame_id = "base_link";
00223     g.post_place_retreat.min_distance = 0.05;
00224     g.post_place_retreat.desired_distance = 0.1;
00225 
00226     g.post_place_posture.joint_names.push_back("katana_l_finger_joint");
00227     g.post_place_posture.joint_names.push_back("katana_r_finger_joint");
00228     g.post_place_posture.points.resize(1);
00229     g.post_place_posture.points[0].positions.push_back(0.3);
00230     g.post_place_posture.points[0].positions.push_back(0.3);
00231 
00232     // not sure whether we need this
00233     g.allowed_touch_objects.resize(1);
00234     g.allowed_touch_objects[0] = "testbox";
00235 
00236     loc.push_back(g);
00237   }
00238 
00239   group.setSupportSurfaceName("cup");
00240 
00241   return group.place("testbox", loc);
00242 }
00243 
00244 int main(int argc, char **argv) {
00245   ros::init (argc, argv, "calvin_pickdemo");
00246   ros::NodeHandle nh;
00247   ros::AsyncSpinner spinner(1);
00248   spinner.start();
00249 
00250   pub_co = nh.advertise<moveit_msgs::CollisionObject>("collision_object", 10);
00251   pub_aco = nh.advertise<moveit_msgs::AttachedCollisionObject>("attached_collision_object", 10);
00252   grasps_marker = nh.advertise<visualization_msgs::MarkerArray>("grasps_marker", 10);
00253 
00254   moveit::planning_interface::MoveGroup group("arm");
00255   group.setPlanningTime(45.0);
00256 
00257   double x = 0.5;
00258   double y = 0.0;
00259   double z = 0.77;
00260 
00261   co.header.stamp = ros::Time::now();
00262   co.header.frame_id = "base_footprint";
00263   co.id = "testbox";
00264   co.primitives.resize(1);
00265   co.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
00266   co.primitives[0].dimensions.resize(shape_tools::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
00267   co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.04;
00268   co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.055;
00269   co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.08;
00270   co.primitive_poses.resize(1);
00271   co.primitive_poses[0].position.x = x;
00272   co.primitive_poses[0].position.y = y;
00273   co.primitive_poses[0].position.z = z;
00274   co.primitive_poses[0].orientation.w = 1.0;
00275 
00276   aco.object = co;
00277   aco.link_name = "katana_gripper_tool_frame";
00278 
00279   add_collision_object();
00280 
00281   ros::WallDuration(1.0).sleep();
00282 
00283   ROS_INFO("Trying to pick");
00284 
00285   // TODO: group.setSupportSurfaceName("table");     // needed to specify that attached object is allowed to touch table
00286   bool success = group.pick(co.id, generate_grasps(x, y, z));
00287 
00288   if (success)
00289     ROS_INFO("Pick was successful.");
00290   else
00291   {
00292     ROS_FATAL("Pick failed.");
00293     return EXIT_FAILURE;
00294   }
00295 
00296   ros::WallDuration(1.0).sleep();
00297 
00298   ROS_INFO("Placing object in muffin holder");
00299   success &= place(group);
00300 
00301 //  // attach object to muffin holder link (TODO: update pose to current pose of object)
00302 //  aco.link_name = "cup";
00303 //  add_attached_collision_object();
00304 
00305   ROS_INFO("Moving arm to arm_far_away pose");
00306   group.setNamedTarget("arm_far_away");
00307   success &= group.move();
00308 
00309   if (success)
00310   {
00311     ROS_INFO("Done.");
00312     return EXIT_SUCCESS;
00313   }
00314   else
00315   {
00316     ROS_ERROR("One of the moves failed!");
00317     return EXIT_FAILURE;
00318 
00319   }
00320 }


calvin_pick_n_place
Author(s): Michael Görner
autogenerated on Thu Jun 6 2019 17:38:49