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 <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
00080 if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name))
00081 {
00082 eef = planning_scene->getRobotModel()->getEndEffector(goal.group_name);
00083 if (eef)
00084 {
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
00099 jmg = goal.group_name.empty() ? NULL : planning_scene->getRobotModel()->getJointModelGroup(goal.group_name);
00100 if (jmg)
00101 {
00102
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
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
00119 planning_scene->getCurrentState().getAttachedBodies(attached_bodies, eg);
00120
00121
00122
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
00133 if (!attached_bodies.empty())
00134 {
00135
00136
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
00147
00148 if (!found)
00149 continue;
00150 }
00151
00152
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
00161 eef = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
00162 }
00163 }
00164 }
00165 }
00166
00167
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
00174 const robot_model::LinkModel *link = attached_body->getAttachedLink();
00175
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
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
00212 int loop_count = 0;
00213 while (attached_object_name.empty() && loop_count < 2)
00214 {
00215
00216
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
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
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
00263 approach_place_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
00264
00265
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
00272 approach_place_acm->setEntry(goal.support_surface_name, attached_object_name, true);
00273
00274
00275 if (goal.allow_gripper_support_collision)
00276 approach_place_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
00277 }
00278
00279
00280
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
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
00302
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
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
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 }