53 bool transformToEndEffectorGoal(
const geometry_msgs::PoseStamped& goal_pose,
54 const robot_state::AttachedBody* attached_body, geometry_msgs::PoseStamped& place_pose)
57 if (fixed_transforms.empty())
60 Eigen::Affine3d end_effector_transform;
62 end_effector_transform = end_effector_transform * fixed_transforms[0].inverse(Eigen::Isometry);
63 place_pose.header = goal_pose.header;
71 double timeout = goal.allowed_planning_time;
73 std::string attached_object_name = goal.attached_object_name;
74 const robot_model::JointModelGroup* jmg = NULL;
75 const robot_model::JointModelGroup* eef = NULL;
78 if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name))
80 eef = planning_scene->getRobotModel()->getEndEffector(goal.group_name);
83 const std::string& eef_parent = eef->getEndEffectorParentGroup().first;
84 if (eef_parent.empty())
88 <<
"'. Please define a parent group in the SRDF.");
89 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
93 jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
99 jmg = goal.group_name.empty() ? NULL : planning_scene->getRobotModel()->getJointModelGroup(goal.group_name);
103 const std::vector<std::string>& eef_names = jmg->getAttachedEndEffectorNames();
104 if (eef_names.empty())
106 ROS_ERROR_STREAM_NAMED(
"manipulation",
"There are no end-effectors specified for group '" << goal.group_name
108 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
113 for (std::size_t i = 0; i < eef_names.size(); ++i)
115 std::vector<const robot_state::AttachedBody*> attached_bodies;
116 const robot_model::JointModelGroup* eg = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
120 planning_scene->getCurrentState().getAttachedBodies(attached_bodies, eg);
124 const robot_model::LinkModel* attached_link_model =
125 planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second);
126 if (attached_link_model)
128 std::vector<const robot_state::AttachedBody*> attached_bodies2;
129 planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model);
130 attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end());
135 if (!attached_bodies.empty())
139 if (!attached_object_name.empty())
142 for (std::size_t j = 0; j < attached_bodies.size(); ++j)
143 if (attached_bodies[j]->
getName() == attached_object_name)
159 <<
"' that are currently holding objects. It is ambiguous " 160 "which end-effector to use. Please specify it explicitly.");
161 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
165 eef = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
172 if (!attached_object_name.empty() && !eef)
174 const robot_state::AttachedBody* attached_body =
175 planning_scene->getCurrentState().getAttachedBody(attached_object_name);
179 const robot_model::LinkModel* link = attached_body->getAttachedLink();
181 const std::vector<const robot_model::JointModelGroup*>& eefs = planning_scene->getRobotModel()->getEndEffectors();
182 for (std::size_t i = 0; i < eefs.size(); ++i)
183 if (eefs[i]->hasLinkModel(link->getName()))
188 << link->getName() <<
"' which is where the body '" 189 << attached_object_name
190 <<
"' is attached. It is unclear which end-effector to use.");
191 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
200 const std::string& eef_parent = eef->getEndEffectorParentGroup().first;
201 if (eef_parent.empty())
203 ROS_ERROR_STREAM_NAMED(
"manipulation",
"No parent group to plan in was identified based on end-effector '" 205 <<
"'. Please define a parent group in the SRDF.");
206 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
210 jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
216 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
222 while (attached_object_name.empty() && loop_count < 2)
226 std::vector<const robot_state::AttachedBody*> attached_bodies;
227 planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg);
230 if (attached_bodies.size() > 1)
233 "Multiple attached bodies for group '%s' but no explicit attached object to place was specified",
234 goal.group_name.c_str());
235 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
239 attached_object_name = attached_bodies[0]->getName();
242 const robot_state::AttachedBody* attached_body =
243 planning_scene->getCurrentState().getAttachedBody(attached_object_name);
246 ROS_ERROR_NAMED(
"manipulation",
"There is no object to detach for place action");
247 error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
255 ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
256 plan_data->planning_group_ = jmg;
257 plan_data->end_effector_group_ = eef;
258 plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(eef->getEndEffectorParentGroup().second);
260 plan_data->timeout_ = endtime;
261 plan_data->path_constraints_ = goal.path_constraints;
262 plan_data->planner_id_ = goal.planner_id;
263 plan_data->minimize_object_distance_ =
false;
264 plan_data->max_goal_sampling_attempts_ = std::max(2u, jmg->getDefaultIKAttempts());
265 moveit_msgs::AttachedCollisionObject& detach_object_msg = plan_data->diff_attached_object_;
268 detach_object_msg.link_name = attached_body->getAttachedLinkName();
269 detach_object_msg.object.id = attached_object_name;
270 detach_object_msg.object.operation = moveit_msgs::CollisionObject::REMOVE;
272 collision_detection::AllowedCollisionMatrixPtr approach_place_acm(
276 approach_place_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects,
true);
279 std::vector<std::string> touch_links(attached_body->getTouchLinks().begin(), attached_body->getTouchLinks().end());
280 approach_place_acm->setEntry(attached_object_name, touch_links,
true);
282 if (!goal.support_surface_name.empty())
285 approach_place_acm->setEntry(goal.support_surface_name, attached_object_name,
true);
288 if (goal.allow_gripper_support_collision)
289 approach_place_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(),
true);
295 ManipulationStagePtr stage1(
306 for (std::size_t i = 0; i < goal.place_locations.size(); ++i)
309 const moveit_msgs::PlaceLocation& pl = goal.place_locations[i];
312 p->goal_pose_ = pl.place_pose;
316 if (!transformToEndEffectorGoal(pl.place_pose, attached_body, p->goal_pose_))
318 p->goal_pose_ = pl.place_pose;
319 ROS_ERROR_NAMED(
"manipulation",
"Unable to transform the desired pose of the object to the pose of the " 323 p->approach_ = pl.pre_place_approach;
324 p->retreat_ = pl.post_place_retreat;
325 p->retreat_posture_ = pl.post_place_posture;
327 if (p->retreat_posture_.joint_names.empty())
328 p->retreat_posture_ = attached_body->getDetachPosture();
331 ROS_INFO_NAMED(
"manipulation",
"Added %d place locations", (
int)goal.place_locations.size());
341 error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
345 error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
348 error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
349 if (goal.place_locations.size() > 0)
351 ROS_WARN_NAMED(
"manipulation",
"All supplied place locations failed. Retrying last location in verbose mode.");
365 return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
369 const moveit_msgs::PlaceGoal& goal)
const 371 PlacePlanPtr p(
new PlacePlan(shared_from_this()));
373 p->plan(planning_scene, goal);
375 p->plan(planning_scene->diff(goal.planning_options.planning_scene_diff), goal);
377 if (display_computed_motion_plans_)
379 const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
380 if (!success.empty())
381 visualizePlan(success.back());
386 const std::vector<pick_place::ManipulationPlanPtr>& success = p->getSuccessfulManipulationPlans();
387 visualizeGrasps(success);
388 const std::vector<pick_place::ManipulationPlanPtr>& failed = p->getFailedManipulationPlans();
389 visualizeGrasps(failed);
PickPlaceConstPtr pick_place_
const std::vector< ManipulationPlanPtr > & getSuccessfulManipulationPlans() const
#define ROS_INFO_NAMED(name,...)
#define ROS_ERROR_STREAM_NAMED(name, args)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
#define ROS_WARN_NAMED(name,...)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
std::string getName(void *handle)
bool plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal)
PlacePlanPtr planPlace(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal) const
Plan the sequence of motions that perform a placement action.
moveit_msgs::MoveItErrorCodes error_code_
void setVerbose(bool flag)
ManipulationPipeline pipeline_
void waitForPipeline(const ros::WallTime &endtime)
void push(const ManipulationPlanPtr &grasp)
#define ROS_ERROR_NAMED(name,...)
ManipulationPipeline & addStage(const ManipulationStagePtr &next)
PlacePlan(const PickPlaceConstPtr &pick_place)
void reprocessLastFailure()