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 
00048 PlacePlan::PlacePlan(const PickPlaceConstPtr &pick_place) : PickPlacePlanBase(pick_place, "place")
00049 {
00050 }
00051 
00052 namespace 
00053 {
00054 bool transformToEndEffectorGoal(const geometry_msgs::PoseStamped &goal_pose,
00055                                 const robot_state::AttachedBody* attached_body,
00056                                 geometry_msgs::PoseStamped &place_pose)
00057 {
00058   const EigenSTL::vector_Affine3d& fixed_transforms = attached_body->getFixedTransforms();
00059   if (fixed_transforms.empty())
00060     return false; 
00061   
00062   Eigen::Affine3d end_effector_transform;
00063   tf::poseMsgToEigen(goal_pose.pose, end_effector_transform);
00064   end_effector_transform = end_effector_transform * fixed_transforms[0].inverse();
00065   place_pose.header = goal_pose.header;
00066   tf::poseEigenToMsg(end_effector_transform, place_pose.pose);
00067   return true;
00068 }
00069 }
00070 
00071 bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal)
00072 {
00073   double timeout = goal.allowed_planning_time;
00074   ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
00075   std::string attached_object_name = goal.attached_object_name;
00076   const robot_model::JointModelGroup *jmg = NULL;
00077   const robot_model::JointModelGroup *eef = NULL;
00078 
00079   // if the group specified is actually an end-effector, we use it as such
00080   if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name))
00081   {
00082     eef = planning_scene->getRobotModel()->getEndEffector(goal.group_name);
00083     if (eef)
00084     { // if we correctly found the eef, then we try to find out what the planning group is
00085       const std::string &eef_parent = eef->getEndEffectorParentGroup().first;
00086       if (eef_parent.empty())
00087       {
00088         ROS_ERROR_STREAM("No parent group to plan in was identified based on end-effector '" << goal.group_name << "'. 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("There are no end-effectors specified for group '" << goal.group_name << "'");
00107         error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00108         return false;
00109       }
00110       else
00111         // check to see if there is an end effector that has attached objects associaded, so we can complete the place
00112         for (std::size_t i = 0 ; i < eef_names.size() ; ++i) 
00113         {
00114           std::vector<const robot_state::AttachedBody*> attached_bodies;
00115           const robot_model::JointModelGroup *eg = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
00116           if (eg)
00117           {
00118             // see if there are objects attached to links in the eef
00119             planning_scene->getCurrentState().getAttachedBodies(attached_bodies, eg);
00120             
00121             // is is often possible that the objects are attached to the same link that the eef itself is attached,
00122             // so we check for attached bodies there as well
00123             const robot_model::LinkModel *attached_link_model = planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second);
00124             if (attached_link_model)
00125             {
00126               std::vector<const robot_state::AttachedBody*> attached_bodies2;
00127               planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model);
00128               attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end());
00129             }
00130           }
00131           
00132           // if this end effector has attached objects, we go on
00133           if (!attached_bodies.empty())
00134           {
00135             // if the user specified the name of the attached object to place, we check that indeed
00136             // the group contains this attachd body
00137             if (!attached_object_name.empty())
00138             {
00139               bool found = false;
00140               for (std::size_t j = 0 ; j < attached_bodies.size() ; ++j)
00141                 if (attached_bodies[j]->getName() == attached_object_name)
00142                 {
00143                   found = true;
00144                   break;
00145                 }
00146               // if the attached body this group has is not the same as the one specified,
00147               // we cannot use this eef
00148               if (!found)
00149                 continue;
00150             }
00151             
00152             // if we previoulsy have set the eef it means we have more options we could use, so things are ambiguous
00153             if (eef)
00154             {
00155               ROS_ERROR_STREAM("There are multiple end-effectors for group '" << goal.group_name <<
00156                                "' that are currently holding objects. It is ambiguous which end-effector to use. Please specify it explicitly.");
00157               error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00158               return false;
00159             }
00160             // set the end effector (this was initialized to NULL above)
00161             eef = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
00162           }
00163         }
00164     }
00165   }
00166   
00167   // if we know the attached object, but not the eef, we can try to identify that
00168   if (!attached_object_name.empty() && !eef)
00169   {
00170     const robot_state::AttachedBody *attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name);
00171     if (attached_body)
00172     {
00173       // get the robot model link this attached body is associated to
00174       const robot_model::LinkModel *link = attached_body->getAttachedLink();
00175       // check to see if there is a unique end effector containing the link
00176       const std::vector<const robot_model::JointModelGroup*> &eefs = planning_scene->getRobotModel()->getEndEffectors();
00177       for (std::size_t i = 0 ; i < eefs.size() ; ++i)
00178         if (eefs[i]->hasLinkModel(link->getName()))
00179         {
00180           if (eef)
00181           {
00182             ROS_ERROR_STREAM("There are multiple end-effectors that include the link '" << link->getName() <<
00183                              "' which is where the body '" << attached_object_name << "' is attached. It is unclear which end-effector to use.");
00184             error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00185             return false;
00186           }
00187           eef = eefs[i];
00188         }
00189     }
00190     // if the group is also unknown, but we just found out the eef
00191     if (!jmg && eef)
00192     {
00193       const std::string &eef_parent = eef->getEndEffectorParentGroup().first;
00194       if (eef_parent.empty())
00195       {
00196         ROS_ERROR_STREAM("No parent group to plan in was identified based on end-effector '" << goal.group_name << "'. Please define a parent group in the SRDF.");
00197         error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00198         return false;
00199       }
00200       else
00201         jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
00202     }
00203   }
00204   
00205   if (!jmg || !eef)
00206   { 
00207     error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00208     return false;
00209   }
00210   
00211   // try to infer attached body name if possible
00212   int loop_count = 0;
00213   while (attached_object_name.empty() && loop_count < 2)
00214   {
00215     // in the first try, look for objects attached to the eef, if the eef is known;
00216     // otherwise, look for attached bodies in the planning group itself
00217     std::vector<const robot_state::AttachedBody*> attached_bodies;
00218     planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg);
00219     
00220     loop_count++;
00221     if (attached_bodies.size() > 1)
00222     {
00223       ROS_ERROR("Multiple attached bodies for group '%s' but no explicit attached object to place was specified", goal.group_name.c_str());
00224       error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
00225       return false;
00226     }
00227     else
00228       attached_object_name = attached_bodies[0]->getName();
00229   }
00230 
00231   const robot_state::AttachedBody *attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name);
00232   if (!attached_body)
00233   {
00234     ROS_ERROR("There is no object to detach for place action");
00235     error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
00236     return false;
00237   }
00238 
00239   ros::WallTime start_time = ros::WallTime::now();
00240 
00241   // construct common data for possible manipulation plans
00242   ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
00243   ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
00244   plan_data->planning_group_ = jmg;
00245   plan_data->end_effector_group_ = eef;
00246   plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(eef->getEndEffectorParentGroup().second);
00247   
00248   plan_data->timeout_ = endtime;
00249   plan_data->path_constraints_ = goal.path_constraints;
00250   plan_data->planner_id_ = goal.planner_id;
00251   plan_data->minimize_object_distance_ = false;
00252   plan_data->max_goal_sampling_attempts_ = std::max(2u, jmg->getDefaultIKAttempts());
00253   moveit_msgs::AttachedCollisionObject &detach_object_msg = plan_data->diff_attached_object_;
00254 
00255   // construct the attached object message that will change the world to what it would become after a placement
00256   detach_object_msg.link_name = attached_body->getAttachedLinkName();
00257   detach_object_msg.object.id = attached_object_name;
00258   detach_object_msg.object.operation = moveit_msgs::CollisionObject::REMOVE;
00259 
00260   collision_detection::AllowedCollisionMatrixPtr approach_place_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
00261 
00262   // we are allowed to touch certain other objects with the gripper
00263   approach_place_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
00264 
00265   // we are allowed to touch the target object slightly while retreating the end effector
00266   std::vector<std::string> touch_links(attached_body->getTouchLinks().begin(), attached_body->getTouchLinks().end());
00267   approach_place_acm->setEntry(attached_object_name, touch_links, true);
00268 
00269   if (!goal.support_surface_name.empty())
00270   {
00271     // we are allowed to have contact between the target object and the support surface before the place
00272     approach_place_acm->setEntry(goal.support_surface_name, attached_object_name, true);
00273 
00274     // optionally, it may be allowed to touch the support surface with the gripper
00275     if (goal.allow_gripper_support_collision)
00276       approach_place_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
00277   }
00278 
00279 
00280   // configure the manipulation pipeline
00281   pipeline_.reset();
00282 
00283   ManipulationStagePtr stage1(new ReachableAndValidPoseFilter(planning_scene, approach_place_acm, pick_place_->getConstraintsSamplerManager()));
00284   ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_place_acm));
00285   ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
00286   pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
00287 
00288   initialize();
00289 
00290   pipeline_.start();
00291 
00292   // add possible place locations
00293   for (std::size_t i = 0 ; i < goal.place_locations.size() ; ++i)
00294   {
00295     ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
00296     const moveit_msgs::PlaceLocation &pl = goal.place_locations[i];
00297     
00298     if (goal.place_eef)
00299       p->goal_pose_ = pl.place_pose;
00300     else
00301       // The goals are specified for the attached body
00302       // but we want to transform them into goals for the end-effector instead
00303       if (!transformToEndEffectorGoal(pl.place_pose, attached_body, p->goal_pose_))
00304       {    
00305         p->goal_pose_ = pl.place_pose;
00306         ROS_ERROR("Unable to transform the desired pose of the object to the pose of the end-effector");
00307       }
00308     
00309     p->approach_ = pl.pre_place_approach;
00310     p->retreat_ = pl.post_place_retreat;
00311     p->retreat_posture_ = pl.post_place_posture;
00312     p->id_ = i;
00313     if (p->retreat_posture_.joint_names.empty())
00314       p->retreat_posture_ = attached_body->getDetachPosture();
00315     pipeline_.push(p);
00316   }
00317   ROS_INFO("Added %d place locations", (int) goal.place_locations.size());
00318 
00319   // wait till we're done
00320   waitForPipeline(endtime);
00321 
00322   pipeline_.stop();
00323 
00324   last_plan_time_ = (ros::WallTime::now() - start_time).toSec();
00325 
00326   if (!getSuccessfulManipulationPlans().empty())
00327     error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00328   else
00329   {
00330     if (last_plan_time_ > timeout)
00331       error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
00332     else
00333     {
00334       error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00335       if (goal.place_locations.size() > 0)
00336       {
00337         ROS_WARN("All supplied place locations failed. Retrying last location in verbose mode.");
00338         // everything failed. we now start the pipeline again in verbose mode for one grasp
00339         initialize();
00340         pipeline_.setVerbose(true);
00341         pipeline_.start();
00342         pipeline_.reprocessLastFailure();
00343         waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0));
00344         pipeline_.stop();
00345         pipeline_.setVerbose(false);
00346       }
00347     }
00348   }
00349   ROS_INFO("Place planning completed after %lf seconds", last_plan_time_);
00350 
00351   return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
00352 }
00353 
00354 PlacePlanPtr PickPlace::planPlace(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal) const
00355 {
00356   PlacePlanPtr p(new PlacePlan(shared_from_this()));
00357   if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff))
00358     p->plan(planning_scene, goal);
00359   else
00360     p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal);
00361 
00362   if (display_computed_motion_plans_)
00363   {
00364     const std::vector<pick_place::ManipulationPlanPtr> &success = p->getSuccessfulManipulationPlans();
00365     if (!success.empty())
00366       visualizePlan(success.back());
00367   }
00368 
00369   if (display_grasps_)
00370   {
00371     const std::vector<pick_place::ManipulationPlanPtr> &success = p->getSuccessfulManipulationPlans();
00372     visualizeGrasps(success);
00373     const std::vector<pick_place::ManipulationPlanPtr> &failed = p->getFailedManipulationPlans();
00374     visualizeGrasps(failed);
00375   }
00376 
00377   return p;
00378 }
00379 
00380 }


manipulation
Author(s): Ioan Sucan , Sachin Chitta , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:44:04