shape_grasp_planner.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015, Fetch Robotics Inc.
00003  * Copyright 2013-2014, Unbounded Robotics Inc.
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the Unbounded Robotics, Inc. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 // Author: Michael Ferguson
00032 
00033 #include <Eigen/Eigen>
00034 #include <ros/ros.h>
00035 #include <simple_grasping/shape_grasp_planner.h>
00036 
00037 using shape_msgs::SolidPrimitive;
00038 
00039 namespace simple_grasping
00040 {
00041 
00042 moveit_msgs::GripperTranslation makeGripperTranslation(
00043   std::string frame,
00044   double min,
00045   double desired,
00046   double x_axis = 1.0,
00047   double y_axis = 0.0,
00048   double z_axis = 0.0)
00049 {
00050   moveit_msgs::GripperTranslation translation;
00051   translation.direction.vector.x = x_axis;
00052   translation.direction.vector.y = y_axis;
00053   translation.direction.vector.z = z_axis;
00054   translation.direction.header.frame_id = frame;
00055   translation.min_distance = min;
00056   translation.desired_distance = desired;
00057   return translation;
00058 }
00059 
00060 Eigen::Quaterniond quaternionFromEuler(float yaw, float pitch, float roll)
00061 {
00062   float sy = sin(yaw*0.5);
00063   float cy = cos(yaw*0.5);
00064   float sp = sin(pitch*0.5);
00065   float cp = cos(pitch*0.5);
00066   float sr = sin(roll*0.5);
00067   float cr = cos(roll*0.5);
00068   float w = cr*cp*cy + sr*sp*sy;
00069   float x = sr*cp*cy - cr*sp*sy;
00070   float y = cr*sp*cy + sr*cp*sy;
00071   float z = cr*cp*sy - sr*sp*cy;
00072   return Eigen::Quaterniond(w,x,y,z);
00073 }
00074 
00075 ShapeGraspPlanner::ShapeGraspPlanner(ros::NodeHandle& nh)
00076 {
00077   /*
00078    * Gripper model is based on having two fingers, and assumes
00079    * that the robot is using the moveit_simple_controller_manager
00080    * gripper interface, with "parallel" parameter set to true.
00081    */
00082   nh.param<std::string>("gripper/left_joint", left_joint_, "l_gripper_finger_joint");
00083   nh.param<std::string>("gripper/right_joint", right_joint_, "r_gripper_finger_joint");
00084   nh.param("gripper/max_opening", max_opening_, 0.110);
00085   nh.param("gripper/max_effort", max_effort_, 50.0);
00086   nh.param("gripper/finger_depth", finger_depth_, 0.02);
00087   nh.param("gripper/grasp_duration", grasp_duration_, 2.0);
00088   nh.param("gripper/gripper_tolerance", gripper_tolerance_, 0.02);
00089 
00090   /*
00091    * Approach is usually aligned with wrist_roll
00092    */
00093   nh.param<std::string>("gripper/approach/frame", approach_frame_, "wrist_roll_link");
00094   nh.param("gripper/approach/min", approach_min_translation_, 0.1);
00095   nh.param("gripper/approach/desired", approach_desired_translation_, 0.15);
00096 
00097   /*
00098    * Retreat is usually aligned with wrist_roll
00099    */
00100   nh.param<std::string>("gripper/retreat/frame", retreat_frame_, "wrist_roll_link");
00101   nh.param("gripper/retreat/min", retreat_min_translation_, 0.1);
00102   nh.param("gripper/retreat/desired", retreat_desired_translation_, 0.15);
00103 
00104   // Distance from tool point to planning frame
00105   nh.param("gripper/tool_to_planning_frame", tool_offset_, 0.165);
00106 }
00107 
00108 int ShapeGraspPlanner::createGrasp(const geometry_msgs::PoseStamped& pose,
00109                                    double gripper_opening,
00110                                    double gripper_pitch,
00111                                    double x_offset,
00112                                    double z_offset,
00113                                    double quality)
00114 {
00115   moveit_msgs::Grasp grasp;
00116   grasp.grasp_pose = pose;
00117 
00118   // defaults
00119   grasp.pre_grasp_posture = makeGraspPosture(gripper_opening);
00120   grasp.grasp_posture = makeGraspPosture(0.0);
00121   grasp.pre_grasp_approach = makeGripperTranslation(approach_frame_,
00122                                                     approach_min_translation_,
00123                                                     approach_desired_translation_);
00124   grasp.post_grasp_retreat = makeGripperTranslation(retreat_frame_,
00125                                                     retreat_min_translation_,
00126                                                     retreat_desired_translation_,
00127                                                     -1.0);  // retreat is in negative x direction
00128 
00129   // initial pose
00130   Eigen::Affine3d p = Eigen::Translation3d(pose.pose.position.x,
00131                                            pose.pose.position.y,
00132                                            pose.pose.position.z) *
00133                         Eigen::Quaterniond(pose.pose.orientation.w,
00134                                            pose.pose.orientation.x,
00135                                            pose.pose.orientation.y,
00136                                            pose.pose.orientation.z);
00137   // translate by x_offset, 0, z_offset
00138   p = p * Eigen::Translation3d(x_offset, 0, z_offset);
00139   // rotate by 0, pitch, 0
00140   p = p * quaternionFromEuler(0.0, gripper_pitch, 0.0);
00141   // apply grasp point -> planning frame offset
00142   p = p * Eigen::Translation3d(-tool_offset_, 0, 0);
00143 
00144   grasp.grasp_pose.pose.position.x = p.translation().x();
00145   grasp.grasp_pose.pose.position.y = p.translation().y();
00146   grasp.grasp_pose.pose.position.z = p.translation().z();
00147   Eigen::Quaterniond q = (Eigen::Quaterniond)p.linear();
00148   grasp.grasp_pose.pose.orientation.x = q.x();
00149   grasp.grasp_pose.pose.orientation.y = q.y();
00150   grasp.grasp_pose.pose.orientation.z = q.z();
00151   grasp.grasp_pose.pose.orientation.w = q.w();
00152 
00153   grasp.grasp_quality = quality;
00154 
00155   grasps_.push_back(grasp);
00156   return 1;
00157 }
00158 
00159 // Create the grasps going in one direction around an object
00160 // starts with gripper level, rotates it up
00161 // this works for boxes and cylinders
00162 int ShapeGraspPlanner::createGraspSeries(
00163   const geometry_msgs::PoseStamped& pose,
00164   double depth, double width, double height,
00165   bool use_vertical)
00166 {
00167   int count = 0;
00168 
00169   // Gripper opening is limited
00170   if (width >= (max_opening_*0.9))
00171     return count;
00172 
00173   // Depth of grasp calculations
00174   double x = depth/2.0;
00175   double z = height/2.0;
00176   if (x > finger_depth_)
00177     x = finger_depth_ - x;
00178   if (z > finger_depth_)
00179     z = finger_depth_ - z;
00180 
00181   double open = std::min(width + gripper_tolerance_, max_opening_);
00182 
00183   // Grasp along top of box
00184   for (double step = 0.0; step < depth/2.0; step += 0.1)
00185   {
00186     if (use_vertical)
00187       count += createGrasp(pose, open, 1.57, step, -z, 1.0 - 0.1*step);  // vertical
00188     count += createGrasp(pose, open, 1.07, step, -z + 0.01, 0.7 - 0.1*step);  // slightly angled down
00189     if (step > 0.05)
00190     {
00191       if (use_vertical)
00192         count += createGrasp(pose, open, 1.57, -step, -z, 1.0 - 0.1*step);
00193       count += createGrasp(pose, open, 1.07, -step, -z + 0.01, 0.7 - 0.1*step);
00194     }
00195   }
00196 
00197   // Grasp horizontally along side of box
00198   for (double step = 0.0; step < height/2.0; step += 0.1)
00199   {
00200     count += createGrasp(pose, open, 0.0, x, step, 0.8 - 0.1*step);  // horizontal
00201     count += createGrasp(pose, open, 0.5, x-0.01, step, 0.6 - 0.1*step);  // slightly angled up
00202     if (step > 0.05)
00203     {
00204       count += createGrasp(pose, open, 0.0, x, -step, 0.8 - 0.1*step);
00205       count += createGrasp(pose, open, 0.5, x-0.01, -step, 0.6 - 0.1*step);
00206     }
00207   }
00208 
00209   // A grasp on the corner of the box
00210   count += createGrasp(pose, open, 1.57/2.0, x - 0.005, -z + 0.005, 0.25);
00211 
00212   return count;
00213 }
00214 
00215 int ShapeGraspPlanner::plan(const grasping_msgs::Object& object,
00216                             std::vector<moveit_msgs::Grasp>& grasps)
00217 {
00218   ROS_INFO("shape grasp planning starting...");
00219 
00220   // Need a shape primitive
00221   if (object.primitives.size() == 0)
00222   {
00223     // Shape grasp planner can only plan for objects
00224     //  with SolidPrimitive bounding boxes
00225     return -1;
00226   }
00227 
00228   if (object.primitive_poses.size() != object.primitives.size())
00229   {
00230     // Invalid object
00231     return -1;
00232   }
00233 
00234   // Clear out internal vector
00235   grasps_.clear();
00236 
00237   // Setup Pose
00238   geometry_msgs::PoseStamped grasp_pose;
00239   grasp_pose.header = object.header;
00240   grasp_pose.pose = object.primitive_poses[0];
00241 
00242   // Setup object orientation
00243   Eigen::Quaterniond q(object.primitive_poses[0].orientation.w,
00244                        object.primitive_poses[0].orientation.x,
00245                        object.primitive_poses[0].orientation.y,
00246                        object.primitive_poses[0].orientation.z);
00247 
00248   // Setup object dimensions
00249   double x, y, z;
00250   if (object.primitives[0].type == SolidPrimitive::BOX)
00251   {
00252     x = object.primitives[0].dimensions[SolidPrimitive::BOX_X];
00253     y = object.primitives[0].dimensions[SolidPrimitive::BOX_Y];
00254     z = object.primitives[0].dimensions[SolidPrimitive::BOX_Z];
00255   }
00256   else if (object.primitives[0].type == SolidPrimitive::CYLINDER)
00257   {
00258     x = y = 2.0 * object.primitives[0].dimensions[SolidPrimitive::CYLINDER_RADIUS];
00259     z = object.primitives[0].dimensions[SolidPrimitive::CYLINDER_HEIGHT];
00260   }
00261 
00262   // Generate grasps
00263   for (int i = 0; i < 4; ++i)
00264   {
00265     grasp_pose.pose.orientation.x = q.x();
00266     grasp_pose.pose.orientation.y = q.y();
00267     grasp_pose.pose.orientation.z = q.z();
00268     grasp_pose.pose.orientation.w = q.w();
00269 
00270     if (i < 2)
00271     {
00272       createGraspSeries(grasp_pose, x, y, z);
00273     }
00274     else
00275     {
00276       // Only two sets of unqiue vertical grasps
00277       createGraspSeries(grasp_pose, x, y, z, false);
00278     }
00279 
00280     // Next iteration, rotate 90 degrees about Z axis
00281     q = q * quaternionFromEuler(-1.57, 0.0, 0.0);
00282     std::swap(x, y);
00283   }
00284 
00285   ROS_INFO("shape grasp planning done.");
00286 
00287   grasps = grasps_;
00288   return grasps.size();  // num of grasps
00289 }
00290 
00291 trajectory_msgs::JointTrajectory
00292 ShapeGraspPlanner::makeGraspPosture(double pose)
00293 {
00294   trajectory_msgs::JointTrajectory trajectory;
00295   trajectory.joint_names.push_back(left_joint_);
00296   trajectory.joint_names.push_back(right_joint_);
00297   trajectory_msgs::JointTrajectoryPoint point;
00298   point.positions.push_back(pose/2.0);
00299   point.positions.push_back(pose/2.0);
00300   point.effort.push_back(max_effort_);
00301   point.effort.push_back(max_effort_);
00302   point.time_from_start = ros::Duration(grasp_duration_);
00303   trajectory.points.push_back(point);
00304   return trajectory;
00305 }
00306 
00307 }  // namespace simple_grasping


simple_grasping
Author(s): Michael Ferguson
autogenerated on Fri Aug 26 2016 13:17:24