pick.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 <ros/console.h>
00042 
00043 namespace pick_place
00044 {
00045 PickPlan::PickPlan(const PickPlaceConstPtr& pick_place) : PickPlacePlanBase(pick_place, "pick")
00046 {
00047 }
00048 
00049 namespace
00050 {
00051 struct OrderGraspQuality
00052 {
00053   OrderGraspQuality(const std::vector<moveit_msgs::Grasp>& grasps) : grasps_(grasps)
00054   {
00055   }
00056 
00057   bool operator()(const std::size_t a, const std::size_t b) const
00058   {
00059     return grasps_[a].grasp_quality > grasps_[b].grasp_quality;
00060   }
00061 
00062   const std::vector<moveit_msgs::Grasp>& grasps_;
00063 };
00064 }
00065 
00066 bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::PickupGoal& goal)
00067 {
00068   double timeout = goal.allowed_planning_time;
00069   ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
00070 
00071   std::string planning_group = goal.group_name;
00072   std::string end_effector = goal.end_effector;
00073   if (end_effector.empty() && !planning_group.empty())
00074   {
00075     const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
00076     if (!jmg)
00077     {
00078       error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00079       return false;
00080     }
00081     const std::vector<std::string>& eefs = jmg->getAttachedEndEffectorNames();
00082     if (!eefs.empty())
00083     {
00084       end_effector = eefs.front();
00085       if (eefs.size() > 1)
00086         ROS_WARN_STREAM_NAMED("manipulation", "Choice of end-effector for group '"
00087                                                   << planning_group << "' is ambiguous. Assuming '" << end_effector
00088                                                   << "'");
00089     }
00090   }
00091   else if (!end_effector.empty() && planning_group.empty())
00092   {
00093     const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getEndEffector(end_effector);
00094     if (!jmg)
00095     {
00096       error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00097       return false;
00098     }
00099     planning_group = jmg->getEndEffectorParentGroup().first;
00100     if (planning_group.empty())
00101     {
00102       ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '"
00103                                                  << end_effector << "'. Please define a parent group in the SRDF.");
00104       error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00105       return false;
00106     }
00107     else
00108       ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '"
00109                                                                                              << planning_group << "'");
00110   }
00111   const robot_model::JointModelGroup* eef =
00112       end_effector.empty() ? NULL : planning_scene->getRobotModel()->getEndEffector(end_effector);
00113   if (!eef)
00114   {
00115     ROS_ERROR_NAMED("manipulation", "No end-effector specified for pick action");
00116     error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00117     return false;
00118   }
00119   const std::string& ik_link = eef->getEndEffectorParentGroup().second;
00120 
00121   ros::WallTime start_time = ros::WallTime::now();
00122 
00123   // construct common data for possible manipulation plans
00124   ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
00125   ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
00126   plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
00127   plan_data->end_effector_group_ = eef;
00128   plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(ik_link);
00129   plan_data->timeout_ = endtime;
00130   plan_data->path_constraints_ = goal.path_constraints;
00131   plan_data->planner_id_ = goal.planner_id;
00132   plan_data->minimize_object_distance_ = goal.minimize_object_distance;
00133   plan_data->max_goal_sampling_attempts_ = std::max(2u, plan_data->planning_group_->getDefaultIKAttempts());
00134   moveit_msgs::AttachedCollisionObject& attach_object_msg = plan_data->diff_attached_object_;
00135 
00136   // construct the attached object message that will change the world to what it would become after a pick
00137   attach_object_msg.link_name = ik_link;
00138   attach_object_msg.object.id = goal.target_name;
00139   attach_object_msg.object.operation = moveit_msgs::CollisionObject::ADD;
00140   attach_object_msg.touch_links =
00141       goal.attached_object_touch_links.empty() ? eef->getLinkModelNames() : goal.attached_object_touch_links;
00142   collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(
00143       new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
00144 
00145   // we are allowed to touch the target object
00146   approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true);
00147   // we are allowed to touch certain other objects with the gripper
00148   approach_grasp_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
00149   if (!goal.support_surface_name.empty())
00150   {
00151     // we are allowed to have contact between the target object and the support surface before the grasp
00152     approach_grasp_acm->setEntry(goal.support_surface_name, goal.target_name, true);
00153 
00154     // optionally, it may be allowed to touch the support surface with the gripper
00155     if (goal.allow_gripper_support_collision)
00156       approach_grasp_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
00157   }
00158 
00159   // configure the manipulation pipeline
00160   pipeline_.reset();
00161   ManipulationStagePtr stage1(
00162       new ReachableAndValidPoseFilter(planning_scene, approach_grasp_acm, pick_place_->getConstraintsSamplerManager()));
00163   ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_grasp_acm));
00164   ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
00165   pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
00166 
00167   initialize();
00168   pipeline_.start();
00169 
00170   // order the grasps by quality
00171   std::vector<std::size_t> grasp_order(goal.possible_grasps.size());
00172   for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i)
00173     grasp_order[i] = i;
00174   OrderGraspQuality oq(goal.possible_grasps);
00175   std::sort(grasp_order.begin(), grasp_order.end(), oq);
00176 
00177   // feed the available grasps to the stages we set up
00178   for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i)
00179   {
00180     ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
00181     const moveit_msgs::Grasp& g = goal.possible_grasps[grasp_order[i]];
00182     p->approach_ = g.pre_grasp_approach;
00183     p->retreat_ = g.post_grasp_retreat;
00184     p->goal_pose_ = g.grasp_pose;
00185     p->id_ = grasp_order[i];
00186     // if no frame of reference was specified, assume the transform to be in the reference frame of the object
00187     if (p->goal_pose_.header.frame_id.empty())
00188       p->goal_pose_.header.frame_id = goal.target_name;
00189     p->approach_posture_ = g.pre_grasp_posture;
00190     p->retreat_posture_ = g.grasp_posture;
00191     pipeline_.push(p);
00192   }
00193 
00194   // wait till we're done
00195   waitForPipeline(endtime);
00196   pipeline_.stop();
00197 
00198   last_plan_time_ = (ros::WallTime::now() - start_time).toSec();
00199 
00200   if (!getSuccessfulManipulationPlans().empty())
00201     error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00202   else
00203   {
00204     if (last_plan_time_ > timeout)
00205       error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
00206     else
00207     {
00208       error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00209       if (goal.possible_grasps.size() > 0)
00210       {
00211         ROS_WARN_NAMED("manipulation", "All supplied grasps failed. Retrying last grasp in verbose mode.");
00212         // everything failed. we now start the pipeline again in verbose mode for one grasp
00213         initialize();
00214         pipeline_.setVerbose(true);
00215         pipeline_.start();
00216         pipeline_.reprocessLastFailure();
00217         waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0));
00218         pipeline_.stop();
00219         pipeline_.setVerbose(false);
00220       }
00221     }
00222   }
00223   ROS_INFO_NAMED("manipulation", "Pickup planning completed after %lf seconds", last_plan_time_);
00224 
00225   return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
00226 }
00227 
00228 PickPlanPtr PickPlace::planPick(const planning_scene::PlanningSceneConstPtr& planning_scene,
00229                                 const moveit_msgs::PickupGoal& goal) const
00230 {
00231   PickPlanPtr p(new PickPlan(shared_from_this()));
00232 
00233   if (planning_scene::PlanningScene::isEmpty(goal.planning_options.planning_scene_diff))
00234     p->plan(planning_scene, goal);
00235   else
00236     p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal);
00237 
00238   if (display_computed_motion_plans_)
00239   {
00240     const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
00241     if (!success.empty())
00242       visualizePlan(success.back());
00243   }
00244 
00245   if (display_grasps_)
00246   {
00247     const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
00248     visualizeGrasps(success);
00249     const std::vector<pick_place::ManipulationPlanPtr>& failed = p->getFailedManipulationPlans();
00250     visualizeGrasps(failed);
00251   }
00252 
00253   return p;
00254 }
00255 }


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