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