place.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/pick_place/pick_place.h>
00038 #include <moveit/pick_place/reachable_valid_pose_filter.h>
00039 #include <moveit/pick_place/approach_and_translate_stage.h>
00040 #include <moveit/pick_place/plan_stage.h>
00041 #include <moveit/robot_state/conversions.h>
00042 #include <eigen_conversions/eigen_msg.h>
00043 #include <ros/console.h>
00044 
00045 namespace pick_place
00046 {
00047 PlacePlan::PlacePlan(const PickPlaceConstPtr& pick_place) : PickPlacePlanBase(pick_place, "place")
00048 {
00049 }
00050 
00051 namespace
00052 {
00053 bool transformToEndEffectorGoal(const geometry_msgs::PoseStamped& goal_pose,
00054                                 const robot_state::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose)
00055 {
00056   const EigenSTL::vector_Affine3d& fixed_transforms = attached_body->getFixedTransforms();
00057   if (fixed_transforms.empty())
00058     return false;
00059 
00060   Eigen::Affine3d end_effector_transform;
00061   tf::poseMsgToEigen(goal_pose.pose, end_effector_transform);
00062   end_effector_transform = end_effector_transform * fixed_transforms[0].inverse();
00063   place_pose.header = goal_pose.header;
00064   tf::poseEigenToMsg(end_effector_transform, place_pose.pose);
00065   return true;
00066 }
00067 }
00068 
00069 bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::PlaceGoal& goal)
00070 {
00071   double timeout = goal.allowed_planning_time;
00072   ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
00073   std::string attached_object_name = goal.attached_object_name;
00074   const robot_model::JointModelGroup* jmg = NULL;
00075   const robot_model::JointModelGroup* eef = NULL;
00076 
00077   // if the group specified is actually an end-effector, we use it as such
00078   if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name))
00079   {
00080     eef = planning_scene->getRobotModel()->getEndEffector(goal.group_name);
00081     if (eef)
00082     {  // if we correctly found the eef, then we try to find out what the planning group is
00083       const std::string& eef_parent = eef->getEndEffectorParentGroup().first;
00084       if (eef_parent.empty())
00085       {
00086         ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '"
00087                                                    << goal.group_name
00088                                                    << "'. Please define a parent group in the SRDF.");
00089         error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00090         return false;
00091       }
00092       else
00093         jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
00094     }
00095   }
00096   else
00097   {
00098     // if a group name was specified, try to use it
00099     jmg = goal.group_name.empty() ? NULL : planning_scene->getRobotModel()->getJointModelGroup(goal.group_name);
00100     if (jmg)
00101     {
00102       // we also try to find the corresponding eef
00103       const std::vector<std::string>& eef_names = jmg->getAttachedEndEffectorNames();
00104       if (eef_names.empty())
00105       {
00106         ROS_ERROR_STREAM_NAMED("manipulation", "There are no end-effectors specified for group '" << goal.group_name
00107                                                                                                   << "'");
00108         error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00109         return false;
00110       }
00111       else
00112         // check to see if there is an end effector that has attached objects associaded, so we can complete the place
00113         for (std::size_t i = 0; i < eef_names.size(); ++i)
00114         {
00115           std::vector<const robot_state::AttachedBody*> attached_bodies;
00116           const robot_model::JointModelGroup* eg = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
00117           if (eg)
00118           {
00119             // see if there are objects attached to links in the eef
00120             planning_scene->getCurrentState().getAttachedBodies(attached_bodies, eg);
00121 
00122             // is is often possible that the objects are attached to the same link that the eef itself is attached,
00123             // so we check for attached bodies there as well
00124             const robot_model::LinkModel* attached_link_model =
00125                 planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second);
00126             if (attached_link_model)
00127             {
00128               std::vector<const robot_state::AttachedBody*> attached_bodies2;
00129               planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model);
00130               attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end());
00131             }
00132           }
00133 
00134           // if this end effector has attached objects, we go on
00135           if (!attached_bodies.empty())
00136           {
00137             // if the user specified the name of the attached object to place, we check that indeed
00138             // the group contains this attachd body
00139             if (!attached_object_name.empty())
00140             {
00141               bool found = false;
00142               for (std::size_t j = 0; j < attached_bodies.size(); ++j)
00143                 if (attached_bodies[j]->getName() == attached_object_name)
00144                 {
00145                   found = true;
00146                   break;
00147                 }
00148               // if the attached body this group has is not the same as the one specified,
00149               // we cannot use this eef
00150               if (!found)
00151                 continue;
00152             }
00153 
00154             // if we previoulsy have set the eef it means we have more options we could use, so things are ambiguous
00155             if (eef)
00156             {
00157               ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors for group '"
00158                                                          << goal.group_name
00159                                                          << "' that are currently holding objects. It is ambiguous "
00160                                                             "which end-effector to use. Please specify it explicitly.");
00161               error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00162               return false;
00163             }
00164             // set the end effector (this was initialized to NULL above)
00165             eef = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
00166           }
00167         }
00168     }
00169   }
00170 
00171   // if we know the attached object, but not the eef, we can try to identify that
00172   if (!attached_object_name.empty() && !eef)
00173   {
00174     const robot_state::AttachedBody* attached_body =
00175         planning_scene->getCurrentState().getAttachedBody(attached_object_name);
00176     if (attached_body)
00177     {
00178       // get the robot model link this attached body is associated to
00179       const robot_model::LinkModel* link = attached_body->getAttachedLink();
00180       // check to see if there is a unique end effector containing the link
00181       const std::vector<const robot_model::JointModelGroup*>& eefs = planning_scene->getRobotModel()->getEndEffectors();
00182       for (std::size_t i = 0; i < eefs.size(); ++i)
00183         if (eefs[i]->hasLinkModel(link->getName()))
00184         {
00185           if (eef)
00186           {
00187             ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors that include the link '"
00188                                                        << link->getName() << "' which is where the body '"
00189                                                        << attached_object_name
00190                                                        << "' is attached. It is unclear which end-effector to use.");
00191             error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00192             return false;
00193           }
00194           eef = eefs[i];
00195         }
00196     }
00197     // if the group is also unknown, but we just found out the eef
00198     if (!jmg && eef)
00199     {
00200       const std::string& eef_parent = eef->getEndEffectorParentGroup().first;
00201       if (eef_parent.empty())
00202       {
00203         ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '"
00204                                                    << goal.group_name
00205                                                    << "'. Please define a parent group in the SRDF.");
00206         error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00207         return false;
00208       }
00209       else
00210         jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
00211     }
00212   }
00213 
00214   if (!jmg || !eef)
00215   {
00216     error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00217     return false;
00218   }
00219 
00220   // try to infer attached body name if possible
00221   int loop_count = 0;
00222   while (attached_object_name.empty() && loop_count < 2)
00223   {
00224     // in the first try, look for objects attached to the eef, if the eef is known;
00225     // otherwise, look for attached bodies in the planning group itself
00226     std::vector<const robot_state::AttachedBody*> attached_bodies;
00227     planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg);
00228 
00229     loop_count++;
00230     if (attached_bodies.size() > 1)
00231     {
00232       ROS_ERROR_NAMED("manipulation",
00233                       "Multiple attached bodies for group '%s' but no explicit attached object to place was specified",
00234                       goal.group_name.c_str());
00235       error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
00236       return false;
00237     }
00238     else
00239       attached_object_name = attached_bodies[0]->getName();
00240   }
00241 
00242   const robot_state::AttachedBody* attached_body =
00243       planning_scene->getCurrentState().getAttachedBody(attached_object_name);
00244   if (!attached_body)
00245   {
00246     ROS_ERROR_NAMED("manipulation", "There is no object to detach for place action");
00247     error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
00248     return false;
00249   }
00250 
00251   ros::WallTime start_time = ros::WallTime::now();
00252 
00253   // construct common data for possible manipulation plans
00254   ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
00255   ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
00256   plan_data->planning_group_ = jmg;
00257   plan_data->end_effector_group_ = eef;
00258   plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(eef->getEndEffectorParentGroup().second);
00259 
00260   plan_data->timeout_ = endtime;
00261   plan_data->path_constraints_ = goal.path_constraints;
00262   plan_data->planner_id_ = goal.planner_id;
00263   plan_data->minimize_object_distance_ = false;
00264   plan_data->max_goal_sampling_attempts_ = std::max(2u, jmg->getDefaultIKAttempts());
00265   moveit_msgs::AttachedCollisionObject& detach_object_msg = plan_data->diff_attached_object_;
00266 
00267   // construct the attached object message that will change the world to what it would become after a placement
00268   detach_object_msg.link_name = attached_body->getAttachedLinkName();
00269   detach_object_msg.object.id = attached_object_name;
00270   detach_object_msg.object.operation = moveit_msgs::CollisionObject::REMOVE;
00271 
00272   collision_detection::AllowedCollisionMatrixPtr approach_place_acm(
00273       new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
00274 
00275   // we are allowed to touch certain other objects with the gripper
00276   approach_place_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
00277 
00278   // we are allowed to touch the target object slightly while retreating the end effector
00279   std::vector<std::string> touch_links(attached_body->getTouchLinks().begin(), attached_body->getTouchLinks().end());
00280   approach_place_acm->setEntry(attached_object_name, touch_links, true);
00281 
00282   if (!goal.support_surface_name.empty())
00283   {
00284     // we are allowed to have contact between the target object and the support surface before the place
00285     approach_place_acm->setEntry(goal.support_surface_name, attached_object_name, true);
00286 
00287     // optionally, it may be allowed to touch the support surface with the gripper
00288     if (goal.allow_gripper_support_collision)
00289       approach_place_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
00290   }
00291 
00292   // configure the manipulation pipeline
00293   pipeline_.reset();
00294 
00295   ManipulationStagePtr stage1(
00296       new ReachableAndValidPoseFilter(planning_scene, approach_place_acm, pick_place_->getConstraintsSamplerManager()));
00297   ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_place_acm));
00298   ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
00299   pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
00300 
00301   initialize();
00302 
00303   pipeline_.start();
00304 
00305   // add possible place locations
00306   for (std::size_t i = 0; i < goal.place_locations.size(); ++i)
00307   {
00308     ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
00309     const moveit_msgs::PlaceLocation& pl = goal.place_locations[i];
00310 
00311     if (goal.place_eef)
00312       p->goal_pose_ = pl.place_pose;
00313     else
00314         // The goals are specified for the attached body
00315         // but we want to transform them into goals for the end-effector instead
00316         if (!transformToEndEffectorGoal(pl.place_pose, attached_body, p->goal_pose_))
00317     {
00318       p->goal_pose_ = pl.place_pose;
00319       ROS_ERROR_NAMED("manipulation", "Unable to transform the desired pose of the object to the pose of the "
00320                                       "end-effector");
00321     }
00322 
00323     p->approach_ = pl.pre_place_approach;
00324     p->retreat_ = pl.post_place_retreat;
00325     p->retreat_posture_ = pl.post_place_posture;
00326     p->id_ = i;
00327     if (p->retreat_posture_.joint_names.empty())
00328       p->retreat_posture_ = attached_body->getDetachPosture();
00329     pipeline_.push(p);
00330   }
00331   ROS_INFO_NAMED("manipulation", "Added %d place locations", (int)goal.place_locations.size());
00332 
00333   // wait till we're done
00334   waitForPipeline(endtime);
00335 
00336   pipeline_.stop();
00337 
00338   last_plan_time_ = (ros::WallTime::now() - start_time).toSec();
00339 
00340   if (!getSuccessfulManipulationPlans().empty())
00341     error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00342   else
00343   {
00344     if (last_plan_time_ > timeout)
00345       error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
00346     else
00347     {
00348       error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00349       if (goal.place_locations.size() > 0)
00350       {
00351         ROS_WARN_NAMED("manipulation", "All supplied place locations failed. Retrying last location in verbose mode.");
00352         // everything failed. we now start the pipeline again in verbose mode for one grasp
00353         initialize();
00354         pipeline_.setVerbose(true);
00355         pipeline_.start();
00356         pipeline_.reprocessLastFailure();
00357         waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0));
00358         pipeline_.stop();
00359         pipeline_.setVerbose(false);
00360       }
00361     }
00362   }
00363   ROS_INFO_NAMED("manipulation", "Place planning completed after %lf seconds", last_plan_time_);
00364 
00365   return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
00366 }
00367 
00368 PlacePlanPtr PickPlace::planPlace(const planning_scene::PlanningSceneConstPtr& planning_scene,
00369                                   const moveit_msgs::PlaceGoal& goal) const
00370 {
00371   PlacePlanPtr p(new PlacePlan(shared_from_this()));
00372   if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff))
00373     p->plan(planning_scene, goal);
00374   else
00375     p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal);
00376 
00377   if (display_computed_motion_plans_)
00378   {
00379     const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
00380     if (!success.empty())
00381       visualizePlan(success.back());
00382   }
00383 
00384   if (display_grasps_)
00385   {
00386     const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
00387     visualizeGrasps(success);
00388     const std::vector<pick_place::ManipulationPlanPtr>& failed = p->getFailedManipulationPlans();
00389     visualizeGrasps(failed);
00390   }
00391 
00392   return p;
00393 }
00394 }


manipulation
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:50