pick_place_action_capability.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 "pick_place_action_capability.h"
00038 #include <moveit/plan_execution/plan_execution.h>
00039 #include <moveit/plan_execution/plan_with_sensing.h>
00040 #include <moveit/move_group_pick_place_capability/capability_names.h>
00041 
00042 #include <manipulation_msgs/GraspPlanning.h>
00043 #include <eigen_conversions/eigen_msg.h>
00044 
00045 move_group::MoveGroupPickPlaceAction::MoveGroupPickPlaceAction()
00046   : MoveGroupCapability("PickPlaceAction"), pickup_state_(IDLE)
00047 {
00048 }
00049 
00050 void move_group::MoveGroupPickPlaceAction::initialize()
00051 {
00052   pick_place_.reset(new pick_place::PickPlace(context_->planning_pipeline_));
00053   pick_place_->displayComputedMotionPlans(true);
00054 
00055   if (context_->debug_)
00056     pick_place_->displayProcessedGrasps(true);
00057 
00058   // start the pickup action server
00059   pickup_action_server_.reset(new actionlib::SimpleActionServer<moveit_msgs::PickupAction>(
00060       root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1),
00061       false));
00062   pickup_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this));
00063   pickup_action_server_->start();
00064 
00065   // start the place action server
00066   place_action_server_.reset(new actionlib::SimpleActionServer<moveit_msgs::PlaceAction>(
00067       root_node_handle_, PLACE_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, _1), false));
00068   place_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this));
00069   place_action_server_->start();
00070 
00071   grasp_planning_service_ =
00072       root_node_handle_.serviceClient<manipulation_msgs::GraspPlanning>("database_grasp_planning");
00073 }
00074 
00075 void move_group::MoveGroupPickPlaceAction::startPickupExecutionCallback()
00076 {
00077   setPickupState(MONITOR);
00078 }
00079 
00080 void move_group::MoveGroupPickPlaceAction::startPickupLookCallback()
00081 {
00082   setPickupState(LOOK);
00083 }
00084 
00085 void move_group::MoveGroupPickPlaceAction::startPlaceExecutionCallback()
00086 {
00087   setPlaceState(MONITOR);
00088 }
00089 
00090 void move_group::MoveGroupPickPlaceAction::startPlaceLookCallback()
00091 {
00092   setPlaceState(LOOK);
00093 }
00094 
00095 void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanOnly(const moveit_msgs::PickupGoalConstPtr& goal,
00096                                                                           moveit_msgs::PickupResult& action_res)
00097 {
00098   pick_place::PickPlanPtr plan;
00099   try
00100   {
00101     planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);
00102     plan = pick_place_->planPick(ps, *goal);
00103   }
00104   catch (std::runtime_error& ex)
00105   {
00106     ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
00107   }
00108   catch (...)
00109   {
00110     ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception");
00111   }
00112 
00113   if (plan)
00114   {
00115     const std::vector<pick_place::ManipulationPlanPtr>& success = plan->getSuccessfulManipulationPlans();
00116     if (success.empty())
00117     {
00118       action_res.error_code = plan->getErrorCode();
00119     }
00120     else
00121     {
00122       const pick_place::ManipulationPlanPtr& result = success.back();
00123       convertToMsg(result->trajectories_, action_res.trajectory_start, action_res.trajectory_stages);
00124       action_res.trajectory_descriptions.resize(result->trajectories_.size());
00125       for (std::size_t i = 0; i < result->trajectories_.size(); ++i)
00126         action_res.trajectory_descriptions[i] = result->trajectories_[i].description_;
00127       if (result->id_ < goal->possible_grasps.size())
00128         action_res.grasp = goal->possible_grasps[result->id_];
00129       action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00130     }
00131   }
00132   else
00133   {
00134     action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00135   }
00136 }
00137 
00138 void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanOnly(const moveit_msgs::PlaceGoalConstPtr& goal,
00139                                                                          moveit_msgs::PlaceResult& action_res)
00140 {
00141   pick_place::PlacePlanPtr plan;
00142   try
00143   {
00144     planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);
00145     plan = pick_place_->planPlace(ps, *goal);
00146   }
00147   catch (std::runtime_error& ex)
00148   {
00149     ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
00150   }
00151   catch (...)
00152   {
00153     ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception");
00154   }
00155 
00156   if (plan)
00157   {
00158     const std::vector<pick_place::ManipulationPlanPtr>& success = plan->getSuccessfulManipulationPlans();
00159     if (success.empty())
00160     {
00161       action_res.error_code = plan->getErrorCode();
00162     }
00163     else
00164     {
00165       const pick_place::ManipulationPlanPtr& result = success.back();
00166       convertToMsg(result->trajectories_, action_res.trajectory_start, action_res.trajectory_stages);
00167       action_res.trajectory_descriptions.resize(result->trajectories_.size());
00168       for (std::size_t i = 0; i < result->trajectories_.size(); ++i)
00169         action_res.trajectory_descriptions[i] = result->trajectories_[i].description_;
00170       if (result->id_ < goal->place_locations.size())
00171         action_res.place_location = goal->place_locations[result->id_];
00172       action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00173     }
00174   }
00175   else
00176   {
00177     action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00178   }
00179 }
00180 
00181 bool move_group::MoveGroupPickPlaceAction::planUsingPickPlace_Pickup(const moveit_msgs::PickupGoal& goal,
00182                                                                      moveit_msgs::PickupResult* action_res,
00183                                                                      plan_execution::ExecutableMotionPlan& plan)
00184 {
00185   setPickupState(PLANNING);
00186 
00187   planning_scene_monitor::LockedPlanningSceneRO ps(plan.planning_scene_monitor_);
00188 
00189   pick_place::PickPlanPtr pick_plan;
00190   try
00191   {
00192     pick_plan = pick_place_->planPick(plan.planning_scene_, goal);
00193   }
00194   catch (std::runtime_error& ex)
00195   {
00196     ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
00197   }
00198   catch (...)
00199   {
00200     ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception");
00201   }
00202 
00203   if (pick_plan)
00204   {
00205     const std::vector<pick_place::ManipulationPlanPtr>& success = pick_plan->getSuccessfulManipulationPlans();
00206     if (success.empty())
00207     {
00208       plan.error_code_ = pick_plan->getErrorCode();
00209     }
00210     else
00211     {
00212       const pick_place::ManipulationPlanPtr& result = success.back();
00213       plan.plan_components_ = result->trajectories_;
00214       if (result->id_ < goal.possible_grasps.size())
00215         action_res->grasp = goal.possible_grasps[result->id_];
00216       plan.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00217     }
00218   }
00219   else
00220   {
00221     plan.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00222   }
00223 
00224   return plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
00225 }
00226 
00227 bool move_group::MoveGroupPickPlaceAction::planUsingPickPlace_Place(const moveit_msgs::PlaceGoal& goal,
00228                                                                     moveit_msgs::PlaceResult* action_res,
00229                                                                     plan_execution::ExecutableMotionPlan& plan)
00230 {
00231   setPlaceState(PLANNING);
00232 
00233   planning_scene_monitor::LockedPlanningSceneRO ps(plan.planning_scene_monitor_);
00234 
00235   pick_place::PlacePlanPtr place_plan;
00236   try
00237   {
00238     place_plan = pick_place_->planPlace(plan.planning_scene_, goal);
00239   }
00240   catch (std::runtime_error& ex)
00241   {
00242     ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
00243   }
00244   catch (...)
00245   {
00246     ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception");
00247   }
00248 
00249   if (place_plan)
00250   {
00251     const std::vector<pick_place::ManipulationPlanPtr>& success = place_plan->getSuccessfulManipulationPlans();
00252     if (success.empty())
00253     {
00254       plan.error_code_ = place_plan->getErrorCode();
00255     }
00256     else
00257     {
00258       const pick_place::ManipulationPlanPtr& result = success.back();
00259       plan.plan_components_ = result->trajectories_;
00260       if (result->id_ < goal.place_locations.size())
00261         action_res->place_location = goal.place_locations[result->id_];
00262       plan.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00263     }
00264   }
00265   else
00266   {
00267     plan.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
00268   }
00269 
00270   return plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
00271 }
00272 
00273 void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanAndExecute(
00274     const moveit_msgs::PickupGoalConstPtr& goal, moveit_msgs::PickupResult& action_res)
00275 {
00276   plan_execution::PlanExecution::Options opt;
00277 
00278   opt.replan_ = goal->planning_options.replan;
00279   opt.replan_attempts_ = goal->planning_options.replan_attempts;
00280   opt.replan_delay_ = goal->planning_options.replan_delay;
00281   opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPickupExecutionCallback, this);
00282 
00283   opt.plan_callback_ =
00284       boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlace_Pickup, this, boost::cref(*goal), &action_res, _1);
00285   if (goal->planning_options.look_around && context_->plan_with_sensing_)
00286   {
00287     opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
00288                                      _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
00289                                      goal->planning_options.max_safe_execution_cost);
00290     context_->plan_with_sensing_->setBeforeLookCallback(
00291         boost::bind(&MoveGroupPickPlaceAction::startPickupLookCallback, this));
00292   }
00293 
00294   plan_execution::ExecutableMotionPlan plan;
00295   context_->plan_execution_->planAndExecute(plan, goal->planning_options.planning_scene_diff, opt);
00296 
00297   convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.trajectory_stages);
00298   action_res.trajectory_descriptions.resize(plan.plan_components_.size());
00299   for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
00300     action_res.trajectory_descriptions[i] = plan.plan_components_[i].description_;
00301   action_res.error_code = plan.error_code_;
00302 }
00303 
00304 void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanAndExecute(
00305     const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult& action_res)
00306 {
00307   plan_execution::PlanExecution::Options opt;
00308 
00309   opt.replan_ = goal->planning_options.replan;
00310   opt.replan_attempts_ = goal->planning_options.replan_attempts;
00311   opt.replan_delay_ = goal->planning_options.replan_delay;
00312   opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this);
00313   opt.plan_callback_ =
00314       boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlace_Place, this, boost::cref(*goal), &action_res, _1);
00315   if (goal->planning_options.look_around && context_->plan_with_sensing_)
00316   {
00317     opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
00318                                      _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
00319                                      goal->planning_options.max_safe_execution_cost);
00320     context_->plan_with_sensing_->setBeforeLookCallback(
00321         boost::bind(&MoveGroupPickPlaceAction::startPlaceLookCallback, this));
00322   }
00323 
00324   plan_execution::ExecutableMotionPlan plan;
00325   context_->plan_execution_->planAndExecute(plan, goal->planning_options.planning_scene_diff, opt);
00326 
00327   convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.trajectory_stages);
00328   action_res.trajectory_descriptions.resize(plan.plan_components_.size());
00329   for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
00330     action_res.trajectory_descriptions[i] = plan.plan_components_[i].description_;
00331   action_res.error_code = plan.error_code_;
00332 }
00333 
00334 void move_group::MoveGroupPickPlaceAction::executePickupCallback(const moveit_msgs::PickupGoalConstPtr& input_goal)
00335 {
00336   setPickupState(PLANNING);
00337 
00338   // before we start planning, ensure that we have the latest robot state received...
00339   context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
00340   context_->planning_scene_monitor_->updateFrameTransforms();
00341 
00342   moveit_msgs::PickupGoalConstPtr goal;
00343   if (input_goal->possible_grasps.empty())
00344   {
00345     moveit_msgs::PickupGoal* copy(new moveit_msgs::PickupGoal(*input_goal));
00346     goal.reset(copy);
00347     fillGrasps(*copy);
00348   }
00349   else
00350     goal = input_goal;
00351 
00352   moveit_msgs::PickupResult action_res;
00353 
00354   if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
00355   {
00356     if (!goal->planning_options.plan_only)
00357       ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the pick "
00358                                      "goal request has plan_only set to false. Only a motion plan will be computed "
00359                                      "anyway.");
00360     executePickupCallback_PlanOnly(goal, action_res);
00361   }
00362   else
00363     executePickupCallback_PlanAndExecute(goal, action_res);
00364 
00365   bool planned_trajectory_empty = action_res.trajectory_stages.empty();
00366   std::string response =
00367       getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only);
00368   if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00369     pickup_action_server_->setSucceeded(action_res, response);
00370   else
00371   {
00372     if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
00373       pickup_action_server_->setPreempted(action_res, response);
00374     else
00375       pickup_action_server_->setAborted(action_res, response);
00376   }
00377 
00378   setPickupState(IDLE);
00379 }
00380 
00381 void move_group::MoveGroupPickPlaceAction::executePlaceCallback(const moveit_msgs::PlaceGoalConstPtr& goal)
00382 {
00383   setPlaceState(PLANNING);
00384 
00385   // before we start planning, ensure that we have the latest robot state received...
00386   context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
00387   context_->planning_scene_monitor_->updateFrameTransforms();
00388 
00389   moveit_msgs::PlaceResult action_res;
00390 
00391   if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
00392   {
00393     if (!goal->planning_options.plan_only)
00394       ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the place "
00395                                      "goal request has plan_only set to false. Only a motion plan will be computed "
00396                                      "anyway.");
00397     executePlaceCallback_PlanOnly(goal, action_res);
00398   }
00399   else
00400     executePlaceCallback_PlanAndExecute(goal, action_res);
00401 
00402   bool planned_trajectory_empty = action_res.trajectory_stages.empty();
00403   std::string response =
00404       getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only);
00405   if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00406     place_action_server_->setSucceeded(action_res, response);
00407   else
00408   {
00409     if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
00410       place_action_server_->setPreempted(action_res, response);
00411     else
00412       place_action_server_->setAborted(action_res, response);
00413   }
00414 
00415   setPlaceState(IDLE);
00416 }
00417 
00418 void move_group::MoveGroupPickPlaceAction::preemptPickupCallback()
00419 {
00420 }
00421 
00422 void move_group::MoveGroupPickPlaceAction::preemptPlaceCallback()
00423 {
00424 }
00425 
00426 void move_group::MoveGroupPickPlaceAction::setPickupState(MoveGroupState state)
00427 {
00428   pickup_state_ = state;
00429   pickup_feedback_.state = stateToStr(state);
00430   pickup_action_server_->publishFeedback(pickup_feedback_);
00431 }
00432 
00433 void move_group::MoveGroupPickPlaceAction::setPlaceState(MoveGroupState state)
00434 {
00435   place_state_ = state;
00436   place_feedback_.state = stateToStr(state);
00437   place_action_server_->publishFeedback(place_feedback_);
00438 }
00439 
00440 void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& goal)
00441 {
00442   if (grasp_planning_service_)
00443   {
00444     manipulation_msgs::GraspPlanning::Request request;
00445     manipulation_msgs::GraspPlanning::Response response;
00446     bool valid = true;
00447     std::string planning_frame;
00448 
00449     // the planning_scene should be unlocked when calling the service below
00450     {
00451       planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
00452       planning_frame = lscene->getPlanningFrame();
00453 
00454       collision_detection::World::ObjectConstPtr object = lscene->getWorld()->getObject(goal.target_name);
00455       if (!object || object->shape_poses_.empty())
00456       {
00457         ROS_ERROR_NAMED("manipulation", "Object '%s' does not exist or has no pose", goal.target_name.c_str());
00458         return;
00459       }
00460 
00461       request.arm_name = goal.group_name;
00462       request.collision_object_name = goal.target_name;
00463       request.target.reference_frame_id = planning_frame;
00464 
00465       if (lscene->hasObjectType(goal.target_name) && !lscene->getObjectType(goal.target_name).key.empty())
00466       {
00467         household_objects_database_msgs::DatabaseModelPose dbp;
00468         dbp.pose.header.frame_id = planning_frame;
00469         dbp.pose.header.stamp = ros::Time::now();
00470         tf::poseEigenToMsg(object->shape_poses_[0], dbp.pose.pose);
00471         dbp.type = lscene->getObjectType(goal.target_name);
00472         try
00473         {
00474           dbp.model_id = boost::lexical_cast<int>(dbp.type.key);
00475           ROS_DEBUG_NAMED("manipulation", "Asking database for grasps for '%s' with model id: %d", dbp.type.key.c_str(),
00476                           dbp.model_id);
00477           request.target.potential_models.push_back(dbp);
00478         }
00479         catch (boost::bad_lexical_cast&)
00480         {
00481           valid = false;
00482           ROS_ERROR_NAMED("manipulation", "Expected an integer object id, not '%s'", dbp.type.key.c_str());
00483         }
00484       }
00485     }
00486 
00487     if (valid)
00488     {
00489       ROS_DEBUG_NAMED("manipulation", "Calling grasp planner...");
00490       if (grasp_planning_service_.call(request, response))
00491       {
00492         goal.possible_grasps.resize(response.grasps.size());
00493         for (std::size_t i = 0; i < response.grasps.size(); ++i)
00494         {
00495           // construct a moveit grasp from a grasp planner grasp
00496           goal.possible_grasps[i].id = response.grasps[i].id;
00497 
00498           goal.possible_grasps[i].pre_grasp_posture.header = response.grasps[i].pre_grasp_posture.header;
00499           goal.possible_grasps[i].pre_grasp_posture.joint_names = response.grasps[i].pre_grasp_posture.name;
00500           goal.possible_grasps[i].pre_grasp_posture.points.resize(1);
00501           goal.possible_grasps[i].pre_grasp_posture.points[0].positions = response.grasps[i].pre_grasp_posture.position;
00502           goal.possible_grasps[i].pre_grasp_posture.points[0].velocities =
00503               response.grasps[i].pre_grasp_posture.velocity;
00504           goal.possible_grasps[i].pre_grasp_posture.points[0].effort = response.grasps[i].pre_grasp_posture.effort;
00505 
00506           goal.possible_grasps[i].grasp_posture.header = response.grasps[i].grasp_posture.header;
00507           goal.possible_grasps[i].grasp_posture.joint_names = response.grasps[i].grasp_posture.name;
00508           goal.possible_grasps[i].grasp_posture.points.resize(1);
00509           goal.possible_grasps[i].grasp_posture.points[0].positions = response.grasps[i].grasp_posture.position;
00510           goal.possible_grasps[i].grasp_posture.points[0].velocities = response.grasps[i].grasp_posture.velocity;
00511           goal.possible_grasps[i].grasp_posture.points[0].effort = response.grasps[i].grasp_posture.effort;
00512 
00513           goal.possible_grasps[i].grasp_pose = response.grasps[i].grasp_pose;
00514           goal.possible_grasps[i].grasp_quality = response.grasps[i].grasp_quality;
00515 
00516           goal.possible_grasps[i].pre_grasp_approach.direction = response.grasps[i].approach.direction;
00517           goal.possible_grasps[i].pre_grasp_approach.desired_distance = response.grasps[i].approach.desired_distance;
00518           goal.possible_grasps[i].pre_grasp_approach.min_distance = response.grasps[i].approach.min_distance;
00519 
00520           //  here we hard-code in the decision that after grasping, the object is lifted "up" (against gravity)
00521           //  we expect that in the planning frame Z points up
00522           goal.possible_grasps[i].post_grasp_retreat.direction.vector.x = 0.0;
00523           goal.possible_grasps[i].post_grasp_retreat.direction.vector.y = 0.0;
00524           goal.possible_grasps[i].post_grasp_retreat.direction.vector.z = 1.0;
00525           goal.possible_grasps[i].post_grasp_retreat.desired_distance = 0.1;
00526           goal.possible_grasps[i].post_grasp_retreat.min_distance = 0.0;
00527           goal.possible_grasps[i].post_grasp_retreat.direction.header.frame_id = planning_frame;
00528 
00529           goal.possible_grasps[i].post_place_retreat.direction = response.grasps[i].retreat.direction;
00530           goal.possible_grasps[i].post_place_retreat.desired_distance = response.grasps[i].retreat.desired_distance;
00531           goal.possible_grasps[i].post_place_retreat.min_distance = response.grasps[i].retreat.min_distance;
00532 
00533           goal.possible_grasps[i].max_contact_force = response.grasps[i].max_contact_force;
00534           goal.possible_grasps[i].allowed_touch_objects = response.grasps[i].allowed_touch_objects;
00535         }
00536       }
00537     }
00538   }
00539 
00540   if (goal.possible_grasps.empty())
00541   {
00542     planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
00543 
00544     ROS_DEBUG_NAMED("manipulation", "Using default grasp poses");
00545     goal.minimize_object_distance = true;
00546 
00547     // add a number of default grasp points
00548     // \todo add more!
00549     moveit_msgs::Grasp g;
00550     g.grasp_pose.header.frame_id = goal.target_name;
00551     g.grasp_pose.pose.position.x = -0.2;
00552     g.grasp_pose.pose.position.y = 0.0;
00553     g.grasp_pose.pose.position.z = 0.0;
00554     g.grasp_pose.pose.orientation.x = 0.0;
00555     g.grasp_pose.pose.orientation.y = 0.0;
00556     g.grasp_pose.pose.orientation.z = 0.0;
00557     g.grasp_pose.pose.orientation.w = 1.0;
00558 
00559     g.pre_grasp_approach.direction.header.frame_id = lscene->getPlanningFrame();
00560     g.pre_grasp_approach.direction.vector.x = 1.0;
00561     g.pre_grasp_approach.min_distance = 0.1;
00562     g.pre_grasp_approach.desired_distance = 0.2;
00563 
00564     g.post_grasp_retreat.direction.header.frame_id = lscene->getPlanningFrame();
00565     g.post_grasp_retreat.direction.vector.z = 1.0;
00566     g.post_grasp_retreat.min_distance = 0.1;
00567     g.post_grasp_retreat.desired_distance = 0.2;
00568 
00569     if (lscene->getRobotModel()->hasEndEffector(goal.end_effector))
00570     {
00571       g.pre_grasp_posture.joint_names =
00572           lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames();
00573       g.pre_grasp_posture.points.resize(1);
00574       g.pre_grasp_posture.points[0].positions.resize(g.pre_grasp_posture.joint_names.size(),
00575                                                      std::numeric_limits<double>::max());
00576 
00577       g.grasp_posture.joint_names = g.pre_grasp_posture.joint_names;
00578       g.grasp_posture.points.resize(1);
00579       g.grasp_posture.points[0].positions.resize(g.grasp_posture.joint_names.size(),
00580                                                  -std::numeric_limits<double>::max());
00581     }
00582     goal.possible_grasps.push_back(g);
00583   }
00584 }
00585 
00586 #include <class_loader/class_loader.h>
00587 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupPickPlaceAction, move_group::MoveGroupCapability)


manipulation
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:24:40