Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
00146 approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true);
00147
00148 approach_grasp_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
00149 if (!goal.support_surface_name.empty())
00150 {
00151
00152 approach_grasp_acm->setEntry(goal.support_surface_name, goal.target_name, true);
00153
00154
00155 if (goal.allow_gripper_support_collision)
00156 approach_grasp_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
00157 }
00158
00159
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
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
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
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
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
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 }