pick.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
41 #include <ros/console.h>
42 
43 namespace pick_place
44 {
45 PickPlan::PickPlan(const PickPlaceConstPtr& pick_place) : PickPlacePlanBase(pick_place, "pick")
46 {
47 }
48 
49 namespace
50 {
51 struct OrderGraspQuality
52 {
53  OrderGraspQuality(const std::vector<moveit_msgs::Grasp>& grasps) : grasps_(grasps)
54  {
55  }
56 
57  bool operator()(const std::size_t a, const std::size_t b) const
58  {
59  return grasps_[a].grasp_quality > grasps_[b].grasp_quality;
60  }
61 
62  const std::vector<moveit_msgs::Grasp>& grasps_;
63 };
64 }
65 
66 bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::PickupGoal& goal)
67 {
68  double timeout = goal.allowed_planning_time;
69  ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
70 
71  std::string planning_group = goal.group_name;
72  std::string end_effector = goal.end_effector;
73  if (end_effector.empty() && !planning_group.empty())
74  {
75  const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
76  if (!jmg)
77  {
78  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
79  return false;
80  }
81  const std::vector<std::string>& eefs = jmg->getAttachedEndEffectorNames();
82  if (!eefs.empty())
83  {
84  end_effector = eefs.front();
85  if (eefs.size() > 1)
86  ROS_WARN_STREAM_NAMED("manipulation", "Choice of end-effector for group '"
87  << planning_group << "' is ambiguous. Assuming '" << end_effector
88  << "'");
89  }
90  }
91  else if (!end_effector.empty() && planning_group.empty())
92  {
93  const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getEndEffector(end_effector);
94  if (!jmg)
95  {
96  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
97  return false;
98  }
99  planning_group = jmg->getEndEffectorParentGroup().first;
100  if (planning_group.empty())
101  {
102  ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '"
103  << end_effector << "'. Please define a parent group in the SRDF.");
104  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
105  return false;
106  }
107  else
108  ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '"
109  << planning_group << "'");
110  }
111  const robot_model::JointModelGroup* eef =
112  end_effector.empty() ? NULL : planning_scene->getRobotModel()->getEndEffector(end_effector);
113  if (!eef)
114  {
115  ROS_ERROR_NAMED("manipulation", "No end-effector specified for pick action");
116  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
117  return false;
118  }
119  const std::string& ik_link = eef->getEndEffectorParentGroup().second;
120 
121  ros::WallTime start_time = ros::WallTime::now();
122 
123  // construct common data for possible manipulation plans
124  ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
125  ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
126  plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
127  plan_data->end_effector_group_ = eef;
128  plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(ik_link);
129  plan_data->timeout_ = endtime;
130  plan_data->path_constraints_ = goal.path_constraints;
131  plan_data->planner_id_ = goal.planner_id;
132  plan_data->minimize_object_distance_ = goal.minimize_object_distance;
133  plan_data->max_goal_sampling_attempts_ = std::max(2u, plan_data->planning_group_->getDefaultIKAttempts());
134  moveit_msgs::AttachedCollisionObject& attach_object_msg = plan_data->diff_attached_object_;
135 
136  // construct the attached object message that will change the world to what it would become after a pick
137  attach_object_msg.link_name = ik_link;
138  attach_object_msg.object.id = goal.target_name;
139  attach_object_msg.object.operation = moveit_msgs::CollisionObject::ADD;
140  attach_object_msg.touch_links =
141  goal.attached_object_touch_links.empty() ? eef->getLinkModelNames() : goal.attached_object_touch_links;
142  collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(
143  new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
144 
145  // we are allowed to touch the target object
146  approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true);
147  // we are allowed to touch certain other objects with the gripper
148  approach_grasp_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
149  if (!goal.support_surface_name.empty())
150  {
151  // we are allowed to have contact between the target object and the support surface before the grasp
152  approach_grasp_acm->setEntry(goal.support_surface_name, goal.target_name, true);
153 
154  // optionally, it may be allowed to touch the support surface with the gripper
155  if (goal.allow_gripper_support_collision)
156  approach_grasp_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
157  }
158 
159  // configure the manipulation pipeline
160  pipeline_.reset();
161  ManipulationStagePtr stage1(
162  new ReachableAndValidPoseFilter(planning_scene, approach_grasp_acm, pick_place_->getConstraintsSamplerManager()));
163  ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_grasp_acm));
164  ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
165  pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
166 
167  initialize();
168  pipeline_.start();
169 
170  // order the grasps by quality
171  std::vector<std::size_t> grasp_order(goal.possible_grasps.size());
172  for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i)
173  grasp_order[i] = i;
174  OrderGraspQuality oq(goal.possible_grasps);
175  std::sort(grasp_order.begin(), grasp_order.end(), oq);
176 
177  // feed the available grasps to the stages we set up
178  for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i)
179  {
180  ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
181  const moveit_msgs::Grasp& g = goal.possible_grasps[grasp_order[i]];
182  p->approach_ = g.pre_grasp_approach;
183  p->retreat_ = g.post_grasp_retreat;
184  p->goal_pose_ = g.grasp_pose;
185  p->id_ = grasp_order[i];
186  // if no frame of reference was specified, assume the transform to be in the reference frame of the object
187  if (p->goal_pose_.header.frame_id.empty())
188  p->goal_pose_.header.frame_id = goal.target_name;
189  p->approach_posture_ = g.pre_grasp_posture;
190  p->retreat_posture_ = g.grasp_posture;
191  pipeline_.push(p);
192  }
193 
194  // wait till we're done
195  waitForPipeline(endtime);
196  pipeline_.stop();
197 
198  last_plan_time_ = (ros::WallTime::now() - start_time).toSec();
199 
200  if (!getSuccessfulManipulationPlans().empty())
201  error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
202  else
203  {
204  if (last_plan_time_ > timeout)
205  error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
206  else
207  {
208  error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
209  if (goal.possible_grasps.size() > 0)
210  {
211  ROS_WARN_NAMED("manipulation", "All supplied grasps failed. Retrying last grasp in verbose mode.");
212  // everything failed. we now start the pipeline again in verbose mode for one grasp
213  initialize();
214  pipeline_.setVerbose(true);
215  pipeline_.start();
218  pipeline_.stop();
219  pipeline_.setVerbose(false);
220  }
221  }
222  }
223  ROS_INFO_NAMED("manipulation", "Pickup planning completed after %lf seconds", last_plan_time_);
224 
225  return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
226 }
227 
228 PickPlanPtr PickPlace::planPick(const planning_scene::PlanningSceneConstPtr& planning_scene,
229  const moveit_msgs::PickupGoal& goal) const
230 {
231  PickPlanPtr p(new PickPlan(shared_from_this()));
232 
233  if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff))
234  p->plan(planning_scene, goal);
235  else
236  p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal);
237 
238  if (display_computed_motion_plans_)
239  {
240  const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
241  if (!success.empty())
242  visualizePlan(success.back());
243  }
244 
245  if (display_grasps_)
246  {
247  const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
248  visualizeGrasps(success);
249  const std::vector<pick_place::ManipulationPlanPtr>& failed = p->getFailedManipulationPlans();
250  visualizeGrasps(failed);
251  }
252 
253  return p;
254 }
255 }
PickPlaceConstPtr pick_place_
Definition: pick_place.h:80
const std::vector< ManipulationPlanPtr > & getSuccessfulManipulationPlans() const
Definition: pick_place.h:60
#define ROS_INFO_NAMED(name,...)
#define ROS_ERROR_STREAM_NAMED(name, args)
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
#define ROS_WARN_NAMED(name,...)
planning_group
#define ROS_INFO_STREAM_NAMED(name, args)
moveit_msgs::MoveItErrorCodes error_code_
Definition: pick_place.h:88
ManipulationPipeline pipeline_
Definition: pick_place.h:81
void waitForPipeline(const ros::WallTime &endtime)
Definition: pick_place.cpp:87
void push(const ManipulationPlanPtr &grasp)
PickPlan(const PickPlaceConstPtr &pick_place)
Definition: pick.cpp:45
static WallTime now()
const std::vector< moveit_msgs::Grasp > & grasps_
Definition: pick.cpp:62
PickPlanPtr planPick(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PickupGoal &goal) const
Plan the sequence of motions that perform a pickup action.
Definition: pick.cpp:228
#define ROS_ERROR_NAMED(name,...)
ManipulationPipeline & addStage(const ManipulationStagePtr &next)
bool plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PickupGoal &goal)
Definition: pick.cpp:66
#define ROS_WARN_STREAM_NAMED(name, args)


manipulation
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:18:24