place.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 
43 #include <ros/console.h>
44 
45 namespace pick_place
46 {
47 PlacePlan::PlacePlan(const PickPlaceConstPtr& pick_place) : PickPlacePlanBase(pick_place, "place")
48 {
49 }
50 
51 namespace
52 {
53 bool transformToEndEffectorGoal(const geometry_msgs::PoseStamped& goal_pose,
54  const robot_state::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose)
55 {
56  const EigenSTL::vector_Affine3d& fixed_transforms = attached_body->getFixedTransforms();
57  if (fixed_transforms.empty())
58  return false;
59 
60  Eigen::Affine3d end_effector_transform;
61  tf::poseMsgToEigen(goal_pose.pose, end_effector_transform);
62  end_effector_transform = end_effector_transform * fixed_transforms[0].inverse(Eigen::Isometry);
63  place_pose.header = goal_pose.header;
64  tf::poseEigenToMsg(end_effector_transform, place_pose.pose);
65  return true;
66 }
67 }
68 
69 bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::PlaceGoal& goal)
70 {
71  double timeout = goal.allowed_planning_time;
72  ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
73  std::string attached_object_name = goal.attached_object_name;
74  const robot_model::JointModelGroup* jmg = NULL;
75  const robot_model::JointModelGroup* eef = NULL;
76 
77  // if the group specified is actually an end-effector, we use it as such
78  if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name))
79  {
80  eef = planning_scene->getRobotModel()->getEndEffector(goal.group_name);
81  if (eef)
82  { // if we correctly found the eef, then we try to find out what the planning group is
83  const std::string& eef_parent = eef->getEndEffectorParentGroup().first;
84  if (eef_parent.empty())
85  {
86  ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '"
87  << goal.group_name
88  << "'. Please define a parent group in the SRDF.");
89  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
90  return false;
91  }
92  else
93  jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
94  }
95  }
96  else
97  {
98  // if a group name was specified, try to use it
99  jmg = goal.group_name.empty() ? NULL : planning_scene->getRobotModel()->getJointModelGroup(goal.group_name);
100  if (jmg)
101  {
102  // we also try to find the corresponding eef
103  const std::vector<std::string>& eef_names = jmg->getAttachedEndEffectorNames();
104  if (eef_names.empty())
105  {
106  ROS_ERROR_STREAM_NAMED("manipulation", "There are no end-effectors specified for group '" << goal.group_name
107  << "'");
108  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
109  return false;
110  }
111  else
112  // check to see if there is an end effector that has attached objects associaded, so we can complete the place
113  for (std::size_t i = 0; i < eef_names.size(); ++i)
114  {
115  std::vector<const robot_state::AttachedBody*> attached_bodies;
116  const robot_model::JointModelGroup* eg = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
117  if (eg)
118  {
119  // see if there are objects attached to links in the eef
120  planning_scene->getCurrentState().getAttachedBodies(attached_bodies, eg);
121 
122  // is is often possible that the objects are attached to the same link that the eef itself is attached,
123  // so we check for attached bodies there as well
124  const robot_model::LinkModel* attached_link_model =
125  planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second);
126  if (attached_link_model)
127  {
128  std::vector<const robot_state::AttachedBody*> attached_bodies2;
129  planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model);
130  attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end());
131  }
132  }
133 
134  // if this end effector has attached objects, we go on
135  if (!attached_bodies.empty())
136  {
137  // if the user specified the name of the attached object to place, we check that indeed
138  // the group contains this attachd body
139  if (!attached_object_name.empty())
140  {
141  bool found = false;
142  for (std::size_t j = 0; j < attached_bodies.size(); ++j)
143  if (attached_bodies[j]->getName() == attached_object_name)
144  {
145  found = true;
146  break;
147  }
148  // if the attached body this group has is not the same as the one specified,
149  // we cannot use this eef
150  if (!found)
151  continue;
152  }
153 
154  // if we previoulsy have set the eef it means we have more options we could use, so things are ambiguous
155  if (eef)
156  {
157  ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors for group '"
158  << goal.group_name
159  << "' that are currently holding objects. It is ambiguous "
160  "which end-effector to use. Please specify it explicitly.");
161  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
162  return false;
163  }
164  // set the end effector (this was initialized to NULL above)
165  eef = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
166  }
167  }
168  }
169  }
170 
171  // if we know the attached object, but not the eef, we can try to identify that
172  if (!attached_object_name.empty() && !eef)
173  {
174  const robot_state::AttachedBody* attached_body =
175  planning_scene->getCurrentState().getAttachedBody(attached_object_name);
176  if (attached_body)
177  {
178  // get the robot model link this attached body is associated to
179  const robot_model::LinkModel* link = attached_body->getAttachedLink();
180  // check to see if there is a unique end effector containing the link
181  const std::vector<const robot_model::JointModelGroup*>& eefs = planning_scene->getRobotModel()->getEndEffectors();
182  for (std::size_t i = 0; i < eefs.size(); ++i)
183  if (eefs[i]->hasLinkModel(link->getName()))
184  {
185  if (eef)
186  {
187  ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors that include the link '"
188  << link->getName() << "' which is where the body '"
189  << attached_object_name
190  << "' is attached. It is unclear which end-effector to use.");
191  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
192  return false;
193  }
194  eef = eefs[i];
195  }
196  }
197  // if the group is also unknown, but we just found out the eef
198  if (!jmg && eef)
199  {
200  const std::string& eef_parent = eef->getEndEffectorParentGroup().first;
201  if (eef_parent.empty())
202  {
203  ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '"
204  << goal.group_name
205  << "'. Please define a parent group in the SRDF.");
206  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
207  return false;
208  }
209  else
210  jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
211  }
212  }
213 
214  if (!jmg || !eef)
215  {
216  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
217  return false;
218  }
219 
220  // try to infer attached body name if possible
221  int loop_count = 0;
222  while (attached_object_name.empty() && loop_count < 2)
223  {
224  // in the first try, look for objects attached to the eef, if the eef is known;
225  // otherwise, look for attached bodies in the planning group itself
226  std::vector<const robot_state::AttachedBody*> attached_bodies;
227  planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg);
228 
229  loop_count++;
230  if (attached_bodies.size() > 1)
231  {
232  ROS_ERROR_NAMED("manipulation",
233  "Multiple attached bodies for group '%s' but no explicit attached object to place was specified",
234  goal.group_name.c_str());
235  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
236  return false;
237  }
238  else
239  attached_object_name = attached_bodies[0]->getName();
240  }
241 
242  const robot_state::AttachedBody* attached_body =
243  planning_scene->getCurrentState().getAttachedBody(attached_object_name);
244  if (!attached_body)
245  {
246  ROS_ERROR_NAMED("manipulation", "There is no object to detach for place action");
247  error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
248  return false;
249  }
250 
251  ros::WallTime start_time = ros::WallTime::now();
252 
253  // construct common data for possible manipulation plans
254  ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
255  ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
256  plan_data->planning_group_ = jmg;
257  plan_data->end_effector_group_ = eef;
258  plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(eef->getEndEffectorParentGroup().second);
259 
260  plan_data->timeout_ = endtime;
261  plan_data->path_constraints_ = goal.path_constraints;
262  plan_data->planner_id_ = goal.planner_id;
263  plan_data->minimize_object_distance_ = false;
264  plan_data->max_goal_sampling_attempts_ = std::max(2u, jmg->getDefaultIKAttempts());
265  moveit_msgs::AttachedCollisionObject& detach_object_msg = plan_data->diff_attached_object_;
266 
267  // construct the attached object message that will change the world to what it would become after a placement
268  detach_object_msg.link_name = attached_body->getAttachedLinkName();
269  detach_object_msg.object.id = attached_object_name;
270  detach_object_msg.object.operation = moveit_msgs::CollisionObject::REMOVE;
271 
272  collision_detection::AllowedCollisionMatrixPtr approach_place_acm(
273  new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
274 
275  // we are allowed to touch certain other objects with the gripper
276  approach_place_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
277 
278  // we are allowed to touch the target object slightly while retreating the end effector
279  std::vector<std::string> touch_links(attached_body->getTouchLinks().begin(), attached_body->getTouchLinks().end());
280  approach_place_acm->setEntry(attached_object_name, touch_links, true);
281 
282  if (!goal.support_surface_name.empty())
283  {
284  // we are allowed to have contact between the target object and the support surface before the place
285  approach_place_acm->setEntry(goal.support_surface_name, attached_object_name, true);
286 
287  // optionally, it may be allowed to touch the support surface with the gripper
288  if (goal.allow_gripper_support_collision)
289  approach_place_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
290  }
291 
292  // configure the manipulation pipeline
293  pipeline_.reset();
294 
295  ManipulationStagePtr stage1(
296  new ReachableAndValidPoseFilter(planning_scene, approach_place_acm, pick_place_->getConstraintsSamplerManager()));
297  ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_place_acm));
298  ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
299  pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
300 
301  initialize();
302 
303  pipeline_.start();
304 
305  // add possible place locations
306  for (std::size_t i = 0; i < goal.place_locations.size(); ++i)
307  {
308  ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
309  const moveit_msgs::PlaceLocation& pl = goal.place_locations[i];
310 
311  if (goal.place_eef)
312  p->goal_pose_ = pl.place_pose;
313  else
314  // The goals are specified for the attached body
315  // but we want to transform them into goals for the end-effector instead
316  if (!transformToEndEffectorGoal(pl.place_pose, attached_body, p->goal_pose_))
317  {
318  p->goal_pose_ = pl.place_pose;
319  ROS_ERROR_NAMED("manipulation", "Unable to transform the desired pose of the object to the pose of the "
320  "end-effector");
321  }
322 
323  p->approach_ = pl.pre_place_approach;
324  p->retreat_ = pl.post_place_retreat;
325  p->retreat_posture_ = pl.post_place_posture;
326  p->id_ = i;
327  if (p->retreat_posture_.joint_names.empty())
328  p->retreat_posture_ = attached_body->getDetachPosture();
329  pipeline_.push(p);
330  }
331  ROS_INFO_NAMED("manipulation", "Added %d place locations", (int)goal.place_locations.size());
332 
333  // wait till we're done
334  waitForPipeline(endtime);
335 
336  pipeline_.stop();
337 
338  last_plan_time_ = (ros::WallTime::now() - start_time).toSec();
339 
340  if (!getSuccessfulManipulationPlans().empty())
341  error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
342  else
343  {
344  if (last_plan_time_ > timeout)
345  error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
346  else
347  {
348  error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
349  if (goal.place_locations.size() > 0)
350  {
351  ROS_WARN_NAMED("manipulation", "All supplied place locations failed. Retrying last location in verbose mode.");
352  // everything failed. we now start the pipeline again in verbose mode for one grasp
353  initialize();
354  pipeline_.setVerbose(true);
355  pipeline_.start();
358  pipeline_.stop();
359  pipeline_.setVerbose(false);
360  }
361  }
362  }
363  ROS_INFO_NAMED("manipulation", "Place planning completed after %lf seconds", last_plan_time_);
364 
365  return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
366 }
367 
368 PlacePlanPtr PickPlace::planPlace(const planning_scene::PlanningSceneConstPtr& planning_scene,
369  const moveit_msgs::PlaceGoal& goal) const
370 {
371  PlacePlanPtr p(new PlacePlan(shared_from_this()));
372  if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff))
373  p->plan(planning_scene, goal);
374  else
375  p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal);
376 
377  if (display_computed_motion_plans_)
378  {
379  const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
380  if (!success.empty())
381  visualizePlan(success.back());
382  }
383 
384  if (display_grasps_)
385  {
386  const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
387  visualizeGrasps(success);
388  const std::vector<pick_place::ManipulationPlanPtr>& failed = p->getFailedManipulationPlans();
389  visualizeGrasps(failed);
390  }
391 
392  return p;
393 }
394 }
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)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
#define ROS_WARN_NAMED(name,...)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
std::string getName(void *handle)
bool found
bool plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal)
Definition: place.cpp:69
PlacePlanPtr planPlace(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal) const
Plan the sequence of motions that perform a placement action.
Definition: place.cpp:368
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)
static WallTime now()
#define ROS_ERROR_NAMED(name,...)
ManipulationPipeline & addStage(const ManipulationStagePtr &next)
PlacePlan(const PickPlaceConstPtr &pick_place)
Definition: place.cpp:47


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