51 struct OrderGraspQuality
53 OrderGraspQuality(
const std::vector<moveit_msgs::Grasp>& grasps) :
grasps_(grasps)
57 bool operator()(
const std::size_t a,
const std::size_t b)
const 62 const std::vector<moveit_msgs::Grasp>&
grasps_;
68 double timeout = goal.allowed_planning_time;
72 std::string end_effector = goal.end_effector;
73 if (end_effector.empty() && !planning_group.empty())
75 const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
78 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
81 const std::vector<std::string>& eefs = jmg->getAttachedEndEffectorNames();
84 end_effector = eefs.front();
87 << planning_group <<
"' is ambiguous. Assuming '" << end_effector
91 else if (!end_effector.empty() && planning_group.empty())
93 const robot_model::JointModelGroup* jmg = planning_scene->getRobotModel()->getEndEffector(end_effector);
96 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
99 planning_group = jmg->getEndEffectorParentGroup().first;
100 if (planning_group.empty())
102 ROS_ERROR_STREAM_NAMED(
"manipulation",
"No parent group to plan in was identified based on end-effector '" 103 << end_effector <<
"'. Please define a parent group in the SRDF.");
104 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
108 ROS_INFO_STREAM_NAMED(
"manipulation",
"Assuming the planning group for end effector '" << end_effector <<
"' is '" 109 << planning_group <<
"'");
111 const robot_model::JointModelGroup* eef =
112 end_effector.empty() ? NULL : planning_scene->getRobotModel()->getEndEffector(end_effector);
115 ROS_ERROR_NAMED(
"manipulation",
"No end-effector specified for pick action");
116 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
119 const std::string& ik_link = eef->getEndEffectorParentGroup().second;
125 ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
126 plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
127 plan_data->end_effector_group_ = eef;
128 plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(ik_link);
129 plan_data->timeout_ = endtime;
130 plan_data->path_constraints_ = goal.path_constraints;
131 plan_data->planner_id_ = goal.planner_id;
132 plan_data->minimize_object_distance_ = goal.minimize_object_distance;
133 plan_data->max_goal_sampling_attempts_ = std::max(2u, plan_data->planning_group_->getDefaultIKAttempts());
134 moveit_msgs::AttachedCollisionObject& attach_object_msg = plan_data->diff_attached_object_;
137 attach_object_msg.link_name = ik_link;
138 attach_object_msg.object.id = goal.target_name;
139 attach_object_msg.object.operation = moveit_msgs::CollisionObject::ADD;
140 attach_object_msg.touch_links =
141 goal.attached_object_touch_links.empty() ? eef->getLinkModelNames() : goal.attached_object_touch_links;
142 collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(
146 approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links,
true);
148 approach_grasp_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects,
true);
149 if (!goal.support_surface_name.empty())
152 approach_grasp_acm->setEntry(goal.support_surface_name, goal.target_name,
true);
155 if (goal.allow_gripper_support_collision)
156 approach_grasp_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(),
true);
161 ManipulationStagePtr stage1(
171 std::vector<std::size_t> grasp_order(goal.possible_grasps.size());
172 for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i)
174 OrderGraspQuality oq(goal.possible_grasps);
175 std::sort(grasp_order.begin(), grasp_order.end(), oq);
178 for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i)
181 const moveit_msgs::Grasp& g = goal.possible_grasps[grasp_order[i]];
182 p->approach_ = g.pre_grasp_approach;
183 p->retreat_ = g.post_grasp_retreat;
184 p->goal_pose_ = g.grasp_pose;
185 p->id_ = grasp_order[i];
187 if (p->goal_pose_.header.frame_id.empty())
188 p->goal_pose_.header.frame_id = goal.target_name;
189 p->approach_posture_ = g.pre_grasp_posture;
190 p->retreat_posture_ = g.grasp_posture;
201 error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
205 error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
208 error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
209 if (goal.possible_grasps.size() > 0)
211 ROS_WARN_NAMED(
"manipulation",
"All supplied grasps failed. Retrying last grasp in verbose mode.");
225 return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
229 const moveit_msgs::PickupGoal& goal)
const 231 PickPlanPtr p(
new PickPlan(shared_from_this()));
234 p->plan(planning_scene, goal);
236 p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal);
238 if (display_computed_motion_plans_)
240 const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
241 if (!success.empty())
242 visualizePlan(success.back());
247 const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
248 visualizeGrasps(success);
249 const std::vector<pick_place::ManipulationPlanPtr>& failed = p->getFailedManipulationPlans();
250 visualizeGrasps(failed);
PickPlaceConstPtr pick_place_
const std::vector< ManipulationPlanPtr > & getSuccessfulManipulationPlans() const
#define ROS_INFO_NAMED(name,...)
#define ROS_ERROR_STREAM_NAMED(name, args)
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
#define ROS_WARN_NAMED(name,...)
#define ROS_INFO_STREAM_NAMED(name, args)
moveit_msgs::MoveItErrorCodes error_code_
void setVerbose(bool flag)
ManipulationPipeline pipeline_
void waitForPipeline(const ros::WallTime &endtime)
void push(const ManipulationPlanPtr &grasp)
PickPlan(const PickPlaceConstPtr &pick_place)
const std::vector< moveit_msgs::Grasp > & grasps_
PickPlanPtr planPick(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PickupGoal &goal) const
Plan the sequence of motions that perform a pickup action.
#define ROS_ERROR_NAMED(name,...)
ManipulationPipeline & addStage(const ManipulationStagePtr &next)
bool plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PickupGoal &goal)
void reprocessLastFailure()
#define ROS_WARN_STREAM_NAMED(name, args)