shape_grasp_planner.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2015, Fetch Robotics Inc.
3  * Copyright 2013-2014, Unbounded Robotics Inc.
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * * Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in the
13  * documentation and/or other materials provided with the distribution.
14  * * Neither the name of the Unbounded Robotics, Inc. nor the names of its
15  * contributors may be used to endorse or promote products derived from
16  * this software without specific prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28  * POSSIBILITY OF SUCH DAMAGE.
29  */
30 
31 // Author: Michael Ferguson
32 
33 #include <Eigen/Eigen>
34 #include <ros/ros.h>
36 
37 using shape_msgs::SolidPrimitive;
38 
39 namespace simple_grasping
40 {
41 
42 moveit_msgs::GripperTranslation makeGripperTranslation(
43  std::string frame,
44  double min,
45  double desired,
46  double x_axis = 1.0,
47  double y_axis = 0.0,
48  double z_axis = 0.0)
49 {
50  moveit_msgs::GripperTranslation translation;
51  translation.direction.vector.x = x_axis;
52  translation.direction.vector.y = y_axis;
53  translation.direction.vector.z = z_axis;
54  translation.direction.header.frame_id = frame;
55  translation.min_distance = min;
56  translation.desired_distance = desired;
57  return translation;
58 }
59 
60 Eigen::Quaterniond quaternionFromEuler(float yaw, float pitch, float roll)
61 {
62  float sy = sin(yaw*0.5);
63  float cy = cos(yaw*0.5);
64  float sp = sin(pitch*0.5);
65  float cp = cos(pitch*0.5);
66  float sr = sin(roll*0.5);
67  float cr = cos(roll*0.5);
68  float w = cr*cp*cy + sr*sp*sy;
69  float x = sr*cp*cy - cr*sp*sy;
70  float y = cr*sp*cy + sr*cp*sy;
71  float z = cr*cp*sy - sr*sp*cy;
72  return Eigen::Quaterniond(w,x,y,z);
73 }
74 
76 {
77  /*
78  * Gripper model is based on having two fingers, and assumes
79  * that the robot is using the moveit_simple_controller_manager
80  * gripper interface, with "parallel" parameter set to true.
81  */
82  nh.param<std::string>("gripper/left_joint", left_joint_, "l_gripper_finger_joint");
83  nh.param<std::string>("gripper/right_joint", right_joint_, "r_gripper_finger_joint");
84  nh.param("gripper/max_opening", max_opening_, 0.110);
85  nh.param("gripper/max_effort", max_effort_, 50.0);
86  nh.param("gripper/finger_depth", finger_depth_, 0.02);
87  nh.param("gripper/grasp_duration", grasp_duration_, 2.0);
88  nh.param("gripper/gripper_tolerance", gripper_tolerance_, 0.02);
89 
90  /*
91  * Approach is usually aligned with wrist_roll
92  */
93  nh.param<std::string>("gripper/approach/frame", approach_frame_, "wrist_roll_link");
94  nh.param("gripper/approach/min", approach_min_translation_, 0.1);
95  nh.param("gripper/approach/desired", approach_desired_translation_, 0.15);
96 
97  /*
98  * Retreat is usually aligned with wrist_roll
99  */
100  nh.param<std::string>("gripper/retreat/frame", retreat_frame_, "wrist_roll_link");
101  nh.param("gripper/retreat/min", retreat_min_translation_, 0.1);
102  nh.param("gripper/retreat/desired", retreat_desired_translation_, 0.15);
103 
104  // Distance from tool point to planning frame
105  nh.param("gripper/tool_to_planning_frame", tool_offset_, 0.165);
106 }
107 
108 int ShapeGraspPlanner::createGrasp(const geometry_msgs::PoseStamped& pose,
109  double gripper_opening,
110  double gripper_pitch,
111  double x_offset,
112  double z_offset,
113  double quality)
114 {
115  moveit_msgs::Grasp grasp;
116  grasp.grasp_pose = pose;
117 
118  // defaults
119  grasp.pre_grasp_posture = makeGraspPosture(gripper_opening);
120  grasp.grasp_posture = makeGraspPosture(0.0);
121  grasp.pre_grasp_approach = makeGripperTranslation(approach_frame_,
124  grasp.post_grasp_retreat = makeGripperTranslation(retreat_frame_,
127  -1.0); // retreat is in negative x direction
128 
129  // initial pose
130  Eigen::Affine3d p = Eigen::Translation3d(pose.pose.position.x,
131  pose.pose.position.y,
132  pose.pose.position.z) *
133  Eigen::Quaterniond(pose.pose.orientation.w,
134  pose.pose.orientation.x,
135  pose.pose.orientation.y,
136  pose.pose.orientation.z);
137  // translate by x_offset, 0, z_offset
138  p = p * Eigen::Translation3d(x_offset, 0, z_offset);
139  // rotate by 0, pitch, 0
140  p = p * quaternionFromEuler(0.0, gripper_pitch, 0.0);
141  // apply grasp point -> planning frame offset
142  p = p * Eigen::Translation3d(-tool_offset_, 0, 0);
143 
144  grasp.grasp_pose.pose.position.x = p.translation().x();
145  grasp.grasp_pose.pose.position.y = p.translation().y();
146  grasp.grasp_pose.pose.position.z = p.translation().z();
147  Eigen::Quaterniond q = (Eigen::Quaterniond)p.linear();
148  grasp.grasp_pose.pose.orientation.x = q.x();
149  grasp.grasp_pose.pose.orientation.y = q.y();
150  grasp.grasp_pose.pose.orientation.z = q.z();
151  grasp.grasp_pose.pose.orientation.w = q.w();
152 
153  grasp.grasp_quality = quality;
154 
155  grasps_.push_back(grasp);
156  return 1;
157 }
158 
159 // Create the grasps going in one direction around an object
160 // starts with gripper level, rotates it up
161 // this works for boxes and cylinders
163  const geometry_msgs::PoseStamped& pose,
164  double depth, double width, double height,
165  bool use_vertical)
166 {
167  int count = 0;
168 
169  // Gripper opening is limited
170  if (width >= (max_opening_*0.9))
171  return count;
172 
173  // Depth of grasp calculations
174  double x = depth/2.0;
175  double z = height/2.0;
176  if (x > finger_depth_)
177  x = finger_depth_ - x;
178  if (z > finger_depth_)
179  z = finger_depth_ - z;
180 
181  double open = std::min(width + gripper_tolerance_, max_opening_);
182 
183  // Grasp along top of box
184  for (double step = 0.0; step < depth/2.0; step += 0.1)
185  {
186  if (use_vertical)
187  count += createGrasp(pose, open, 1.57, step, -z, 1.0 - 0.1*step); // vertical
188  count += createGrasp(pose, open, 1.07, step, -z + 0.01, 0.7 - 0.1*step); // slightly angled down
189  if (step > 0.05)
190  {
191  if (use_vertical)
192  count += createGrasp(pose, open, 1.57, -step, -z, 1.0 - 0.1*step);
193  count += createGrasp(pose, open, 1.07, -step, -z + 0.01, 0.7 - 0.1*step);
194  }
195  }
196 
197  // Grasp horizontally along side of box
198  for (double step = 0.0; step < height/2.0; step += 0.1)
199  {
200  count += createGrasp(pose, open, 0.0, x, step, 0.8 - 0.1*step); // horizontal
201  count += createGrasp(pose, open, 0.5, x-0.01, step, 0.6 - 0.1*step); // slightly angled up
202  if (step > 0.05)
203  {
204  count += createGrasp(pose, open, 0.0, x, -step, 0.8 - 0.1*step);
205  count += createGrasp(pose, open, 0.5, x-0.01, -step, 0.6 - 0.1*step);
206  }
207  }
208 
209  // A grasp on the corner of the box
210  count += createGrasp(pose, open, 1.57/2.0, x - 0.005, -z + 0.005, 0.25);
211 
212  return count;
213 }
214 
215 int ShapeGraspPlanner::plan(const grasping_msgs::Object& object,
216  std::vector<moveit_msgs::Grasp>& grasps)
217 {
218  ROS_INFO("shape grasp planning starting...");
219 
220  // Need a shape primitive
221  if (object.primitives.size() == 0)
222  {
223  // Shape grasp planner can only plan for objects
224  // with SolidPrimitive bounding boxes
225  return -1;
226  }
227 
228  if (object.primitive_poses.size() != object.primitives.size())
229  {
230  // Invalid object
231  return -1;
232  }
233 
234  // Clear out internal vector
235  grasps_.clear();
236 
237  // Setup Pose
238  geometry_msgs::PoseStamped grasp_pose;
239  grasp_pose.header = object.header;
240  grasp_pose.pose = object.primitive_poses[0];
241 
242  // Setup object orientation
243  Eigen::Quaterniond q(object.primitive_poses[0].orientation.w,
244  object.primitive_poses[0].orientation.x,
245  object.primitive_poses[0].orientation.y,
246  object.primitive_poses[0].orientation.z);
247 
248  // Setup object dimensions
249  double x, y, z;
250  if (object.primitives[0].type == SolidPrimitive::BOX)
251  {
252  x = object.primitives[0].dimensions[SolidPrimitive::BOX_X];
253  y = object.primitives[0].dimensions[SolidPrimitive::BOX_Y];
254  z = object.primitives[0].dimensions[SolidPrimitive::BOX_Z];
255  }
256  else if (object.primitives[0].type == SolidPrimitive::CYLINDER)
257  {
258  x = y = 2.0 * object.primitives[0].dimensions[SolidPrimitive::CYLINDER_RADIUS];
259  z = object.primitives[0].dimensions[SolidPrimitive::CYLINDER_HEIGHT];
260  }
261 
262  // Generate grasps
263  for (int i = 0; i < 4; ++i)
264  {
265  grasp_pose.pose.orientation.x = q.x();
266  grasp_pose.pose.orientation.y = q.y();
267  grasp_pose.pose.orientation.z = q.z();
268  grasp_pose.pose.orientation.w = q.w();
269 
270  if (i < 2)
271  {
272  createGraspSeries(grasp_pose, x, y, z);
273  }
274  else
275  {
276  // Only two sets of unqiue vertical grasps
277  createGraspSeries(grasp_pose, x, y, z, false);
278  }
279 
280  // Next iteration, rotate 90 degrees about Z axis
281  q = q * quaternionFromEuler(-1.57, 0.0, 0.0);
282  std::swap(x, y);
283  }
284 
285  ROS_INFO("shape grasp planning done.");
286 
287  grasps = grasps_;
288  return grasps.size(); // num of grasps
289 }
290 
291 trajectory_msgs::JointTrajectory
293 {
294  trajectory_msgs::JointTrajectory trajectory;
295  trajectory.joint_names.push_back(left_joint_);
296  trajectory.joint_names.push_back(right_joint_);
297  trajectory_msgs::JointTrajectoryPoint point;
298  point.positions.push_back(pose/2.0);
299  point.positions.push_back(pose/2.0);
300  point.effort.push_back(max_effort_);
301  point.effort.push_back(max_effort_);
302  point.time_from_start = ros::Duration(grasp_duration_);
303  trajectory.points.push_back(point);
304  return trajectory;
305 }
306 
307 } // namespace simple_grasping
int createGraspSeries(const geometry_msgs::PoseStamped &pose, double depth, double width, double height, bool use_vertical=true)
Generate a series of grasps around the edge of a shape.
ShapeGraspPlanner(ros::NodeHandle &nh)
Constructor, loads grasp planner configuration from ROS params.
Eigen::Quaterniond quaternionFromEuler(float yaw, float pitch, float roll)
int createGrasp(const geometry_msgs::PoseStamped &pose, double gripper_opening, double gripper_pitch, double x_offset, double z_offset, double quality)
Generate a grasp, add it to internal grasps_.
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
TFSIMD_FORCE_INLINE const tfScalar & w() const
unsigned int step
virtual int plan(const grasping_msgs::Object &object, std::vector< moveit_msgs::Grasp > &grasps)
std::vector< moveit_msgs::Grasp > grasps_
moveit_msgs::GripperTranslation makeGripperTranslation(std::string frame, double min, double desired, double x_axis=1.0, double y_axis=0.0, double z_axis=0.0)
trajectory_msgs::JointTrajectory makeGraspPosture(double pose)


simple_grasping
Author(s): Michael Ferguson
autogenerated on Thu Jan 14 2021 03:20:55