bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit_msgs::GetMotionPlan::Request &req,
const chomp::ChompParameters& params,
moveit_msgs::GetMotionPlan::Response &res) const
{
ros::WallTime start_time = ros::WallTime::now();
ChompTrajectory trajectory(planning_scene->getRobotModel(),
3.0,
.03,
req.motion_plan_request.group_name);
jointStateToArray(planning_scene->getRobotModel(),
req.motion_plan_request.start_state.joint_state,
req.motion_plan_request.group_name,
trajectory.getTrajectoryPoint(0));
int goal_index = trajectory.getNumPoints()- 1;
trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
sensor_msgs::JointState js;
for(unsigned int i = 0; i < req.motion_plan_request.goal_constraints[0].joint_constraints.size(); i++) {
js.name.push_back(req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name);
js.position.push_back(req.motion_plan_request.goal_constraints[0].joint_constraints[i].position);
ROS_INFO_STREAM("Setting joint " << req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name
<< " to position " << req.motion_plan_request.goal_constraints[0].joint_constraints[i].position);
}
jointStateToArray(planning_scene->getRobotModel(),
js,
req.motion_plan_request.group_name,
trajectory.getTrajectoryPoint(goal_index));
const planning_models::RobotModel::JointModelGroup* model_group =
planning_scene->getRobotModel()->getJointModelGroup(req.motion_plan_request.group_name);
// fix the goal to move the shortest angular distance for wrap-around joints:
for (size_t i = 0; i < model_group->getJointModels().size(); i++)
{
const planning_models::RobotModel::JointModel* model = model_group->getJointModels()[i];
const planning_models::RobotModel::RevoluteJointModel* revolute_joint = dynamic_cast<const planning_models::RobotModel::RevoluteJointModel*>(model);
if (revolute_joint != NULL)
{
if(revolute_joint->isContinuous())
{
double start = (trajectory)(0, i);
double end = (trajectory)(goal_index, i);
ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
(trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
}
}
}
// fill in an initial quintic spline trajectory
trajectory.fillInMinJerk();
// optimize!
planning_models::RobotState *start_state(planning_scene->getCurrentState());
planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state);
ros::WallTime create_time = ros::WallTime::now();
ChompOptimizer optimizer(&trajectory,
planning_scene,
req.motion_plan_request.group_name,
¶ms,
start_state);
if(!optimizer.isInitialized()) {
ROS_WARN_STREAM("Could not initialize optimizer");
res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
return false;
}
ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec());
ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec());
optimizer.optimize();
ROS_INFO("Optimization actually took %f sec to run", (ros::WallTime::now() - create_time).toSec());
create_time = ros::WallTime::now();
// assume that the trajectory is now optimized, fill in the output structure:
ROS_INFO("Output trajectory has %d joints", trajectory.getNumJoints());
// fill in joint names:
res.trajectory.joint_trajectory.joint_names.resize(trajectory.getNumJoints());
for (size_t i = 0; i < model_group->getJointModels().size(); i++)
{
res.trajectory.joint_trajectory.joint_names[i] = model_group->getJointModels()[i]->getName();
}
res.trajectory.joint_trajectory.header = req.motion_plan_request.start_state.joint_state.header; // @TODO this is probably a hack
// fill in the entire trajectory
res.trajectory.joint_trajectory.points.resize(trajectory.getNumPoints());
for (int i=0; i < trajectory.getNumPoints(); i++)
{
res.trajectory.joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
for (size_t j=0; j < res.trajectory.joint_trajectory.points[i].positions.size(); j++)
{
res.trajectory.joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
if(i == trajectory.getNumPoints()-1) {
ROS_INFO_STREAM("Joint " << j << " " << res.trajectory.joint_trajectory.points[i].positions[j]);
}
}
// Setting invalid timestamps.
// Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints.
res.trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
}
ROS_INFO("Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec());
ROS_INFO("Serviced planning request in %f wall-seconds, trajectory duration is %f", (ros::WallTime::now() - start_time).toSec(), res.trajectory.joint_trajectory.points[goal_index].time_from_start.toSec());
res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
res.planning_time = ros::Duration((ros::WallTime::now() - start_time).toSec());
return true;
}
constraint_samplers::ConstraintSamplerPtr constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planning_scene::PlanningSceneConstPtr &scene,
const std::string &group_name,
const moveit_msgs::Constraints &constr)
{
const robot_model::JointModelGroup *jmg = scene->getRobotModel()->getJointModelGroup(group_name);
if (!jmg)
return constraint_samplers::ConstraintSamplerPtr();
std::stringstream ss; ss << constr;
logDebug("Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n", jmg->getName().c_str(), ss.str().c_str());
ConstraintSamplerPtr joint_sampler;
// if there are joint constraints, we could possibly get a sampler from those
if (!constr.joint_constraints.empty())
{
logDebug("There are joint constraints specified. Attempting to construct a JointConstraintSampler for group '%s'", jmg->getName().c_str());
std::map<std::string, bool> joint_coverage;
for(std::size_t i = 0; i < jmg->getVariableNames().size() ; ++i)
joint_coverage[jmg->getVariableNames()[i]] = false;
// construct the constraints
std::vector<kinematic_constraints::JointConstraint> jc;
for (std::size_t i = 0 ; i < constr.joint_constraints.size() ; ++i)
{
kinematic_constraints::JointConstraint j(scene->getRobotModel(), scene->getTransforms());
if (j.configure(constr.joint_constraints[i]))
{
if (joint_coverage.find(j.getJointVariableName()) != joint_coverage.end())
{
joint_coverage[j.getJointVariableName()] = true;
jc.push_back(j);
}
}
}
bool full_coverage = true;
for (std::map<std::string, bool>::iterator it = joint_coverage.begin(); it != joint_coverage.end(); ++it)
if (!it->second)
{
full_coverage = false;
break;
}
// if we have constrained every joint, then we just use a sampler using these constraints
if (full_coverage)
{
boost::shared_ptr<JointConstraintSampler> sampler(new JointConstraintSampler(scene, jmg->getName()));
if (sampler->configure(jc))
{
logDebug("Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
return sampler;
}
}
else
// if a smaller set of joints has been specified, keep the constraint sampler around, but use it only if no IK sampler has been specified.
if (!jc.empty())
{
boost::shared_ptr<JointConstraintSampler> sampler(new JointConstraintSampler(scene, jmg->getName()));
if (sampler->configure(jc))
{
logDebug("Temporary sampler satisfying joint constraints for group '%s' allocated. Looking for different types of constraints before returning though.", jmg->getName().c_str());
joint_sampler = sampler;
}
}
}
std::vector<ConstraintSamplerPtr> samplers;
if (joint_sampler)
samplers.push_back(joint_sampler);
// read the ik allocators, if any
robot_model::SolverAllocatorFn ik_alloc = jmg->getSolverAllocators().first;
std::map<const robot_model::JointModelGroup*, robot_model::SolverAllocatorFn> ik_subgroup_alloc = jmg->getSolverAllocators().second;
// if we have a means of computing complete states for the group using IK, then we try to see if any IK constraints should be used
if (ik_alloc)
{
logDebug("There is an IK allocator for '%s'. Checking for corresponding position and/or orientation constraints", jmg->getName().c_str());
// keep track of which links we constrained
std::map<std::string, boost::shared_ptr<IKConstraintSampler> > usedL;
// if we have position and/or orientation constraints on links that we can perform IK for,
// we will use a sampleable goal region that employs IK to sample goals;
// if there are multiple constraints for the same link, we keep the one with the smallest
// volume for sampling
for (std::size_t p = 0 ; p < constr.position_constraints.size() ; ++p)
for (std::size_t o = 0 ; o < constr.orientation_constraints.size() ; ++o)
if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
{
boost::shared_ptr<kinematic_constraints::PositionConstraint> pc(new kinematic_constraints::PositionConstraint(scene->getRobotModel(), scene->getTransforms()));
boost::shared_ptr<kinematic_constraints::OrientationConstraint> oc(new kinematic_constraints::OrientationConstraint(scene->getRobotModel(), scene->getTransforms()));
if (pc->configure(constr.position_constraints[p]) && oc->configure(constr.orientation_constraints[o]))
{
boost::shared_ptr<IKConstraintSampler> iks(new IKConstraintSampler(scene, jmg->getName()));
if(iks->configure(IKSamplingPose(pc, oc))) {
bool use = true;
if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
use = false;
if (use)
{
usedL[constr.position_constraints[p].link_name] = iks;
logDebug("Allocated an IK-based sampler for group '%s' satisfying position and orientation constraints on link '%s'",
jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
}
}
}
}
// keep track of links constrained with a full pose
std::map<std::string, boost::shared_ptr<IKConstraintSampler> > usedL_fullPose = usedL;
for (std::size_t p = 0 ; p < constr.position_constraints.size() ; ++p)
{
// if we are constraining this link with a full pose, we do not attempt to constrain it with a position constraint only
if (usedL_fullPose.find(constr.position_constraints[p].link_name) != usedL_fullPose.end())
continue;
boost::shared_ptr<kinematic_constraints::PositionConstraint> pc(new kinematic_constraints::PositionConstraint(scene->getRobotModel(), scene->getTransforms()));
if (pc->configure(constr.position_constraints[p]))
{
boost::shared_ptr<IKConstraintSampler> iks(new IKConstraintSampler(scene, jmg->getName()));
if(iks->configure(IKSamplingPose(pc)))
{
bool use = true;
if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
use = false;
if (use)
{
usedL[constr.position_constraints[p].link_name] = iks;
logDebug("Allocated an IK-based sampler for group '%s' satisfying position constraints on link '%s'",
jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
}
}
}
}
for (std::size_t o = 0 ; o < constr.orientation_constraints.size() ; ++o)
{
// if we are constraining this link with a full pose, we do not attempt to constrain it with an orientation constraint only
if (usedL_fullPose.find(constr.orientation_constraints[o].link_name) != usedL_fullPose.end())
continue;
boost::shared_ptr<kinematic_constraints::OrientationConstraint> oc(new kinematic_constraints::OrientationConstraint(scene->getRobotModel(), scene->getTransforms()));
if (oc->configure(constr.orientation_constraints[o]))
{
boost::shared_ptr<IKConstraintSampler> iks(new IKConstraintSampler(scene, jmg->getName()));
if(iks->configure(IKSamplingPose(oc)))
{
bool use = true;
if (usedL.find(constr.orientation_constraints[o].link_name) != usedL.end())
if (usedL[constr.orientation_constraints[o].link_name]->getSamplingVolume() < iks->getSamplingVolume())
use = false;
if (use)
{
usedL[constr.orientation_constraints[o].link_name] = iks;
logDebug("Allocated an IK-based sampler for group '%s' satisfying orientation constraints on link '%s'",
jmg->getName().c_str(), constr.orientation_constraints[o].link_name.c_str());
}
}
}
}
if (usedL.size() == 1)
{
if (samplers.empty())
return usedL.begin()->second;
else
{
samplers.push_back(usedL.begin()->second);
return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
}
}
else
if (usedL.size() > 1)
{
logDebug("Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume", jmg->getName().c_str());
// find the sampler with the smallest sampling volume; delete the rest
boost::shared_ptr<IKConstraintSampler> iks = usedL.begin()->second;
double msv = iks->getSamplingVolume();
for (std::map<std::string, boost::shared_ptr<IKConstraintSampler> >::const_iterator it = ++usedL.begin() ; it != usedL.end() ; ++it)
{
double v = it->second->getSamplingVolume();
if (v < msv)
{
iks = it->second;
msv = v;
}
}
if (samplers.empty())
{
return iks;
}
else
{
samplers.push_back(iks);
return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
}
}
}
// if we got to this point, we have not decided on a sampler.
// we now check to see if we can use samplers from subgroups
if (!ik_subgroup_alloc.empty())
{
logDebug("There are IK allocators for subgroups of group '%s'. Checking for corresponding position and/or orientation constraints", jmg->getName().c_str());
bool some_sampler_valid = false;
std::set<std::size_t> usedP, usedO;
for (std::map<const robot_model::JointModelGroup*, robot_model::SolverAllocatorFn>::const_iterator it = ik_subgroup_alloc.begin() ; it != ik_subgroup_alloc.end() ; ++it)
{
// construct a sub-set of constraints that operate on the sub-group for which we have an IK allocator
moveit_msgs::Constraints sub_constr;
for (std::size_t p = 0 ; p < constr.position_constraints.size() ; ++p)
if (it->first->hasLinkModel(constr.position_constraints[p].link_name))
if (usedP.find(p) == usedP.end())
{
sub_constr.position_constraints.push_back(constr.position_constraints[p]);
usedP.insert(p);
}
for (std::size_t o = 0 ; o < constr.orientation_constraints.size() ; ++o)
if (it->first->hasLinkModel(constr.orientation_constraints[o].link_name))
if (usedO.find(o) == usedO.end())
{
sub_constr.orientation_constraints.push_back(constr.orientation_constraints[o]);
usedO.insert(o);
}
// if some matching constraints were found, construct the allocator
if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty())
{
logDebug("Attempting to construct a sampler for the '%s' subgroup of '%s'", it->first->getName().c_str(), jmg->getName().c_str());
ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr);
if (cs)
{
logDebug("Constructed a sampler for the joints corresponding to group '%s', but part of group '%s'",
it->first->getName().c_str(), jmg->getName().c_str());
some_sampler_valid = true;
samplers.push_back(cs);
}
}
}
if (some_sampler_valid)
{
logDebug("Constructing sampler for group '%s' as a union of %u samplers", jmg->getName().c_str(), (unsigned int)samplers.size());
return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
}
}
//if we've gotten here, just return joint sampler
if (joint_sampler)
{
logDebug("Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
return joint_sampler;
}
logDebug("No constraints sampler allocated for group '%s'", jmg->getName().c_str());
return ConstraintSamplerPtr();
}
bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal)
{
double timeout = goal.allowed_planning_time;
ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
std::string attached_object_name = goal.attached_object_name;
const robot_model::JointModelGroup *jmg = NULL;
const robot_model::JointModelGroup *eef = NULL;
// if the group specified is actually an end-effector, we use it as such
if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name))
{
eef = planning_scene->getRobotModel()->getEndEffector(goal.group_name);
if (eef)
{ // if we correctly found the eef, then we try to find out what the planning group is
const std::string &eef_parent = eef->getEndEffectorParentGroup().first;
if (eef_parent.empty())
{
ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << goal.group_name << "'. Please define a parent group in the SRDF.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
else
jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
}
}
else
{
// if a group name was specified, try to use it
jmg = goal.group_name.empty() ? NULL : planning_scene->getRobotModel()->getJointModelGroup(goal.group_name);
if (jmg)
{
// we also try to find the corresponding eef
const std::vector<std::string> &eef_names = jmg->getAttachedEndEffectorNames();
if (eef_names.empty())
{
ROS_ERROR_STREAM_NAMED("manipulation", "There are no end-effectors specified for group '" << goal.group_name << "'");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
else
// check to see if there is an end effector that has attached objects associaded, so we can complete the place
for (std::size_t i = 0 ; i < eef_names.size() ; ++i)
{
std::vector<const robot_state::AttachedBody*> attached_bodies;
const robot_model::JointModelGroup *eg = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
if (eg)
{
// see if there are objects attached to links in the eef
planning_scene->getCurrentState().getAttachedBodies(attached_bodies, eg);
// is is often possible that the objects are attached to the same link that the eef itself is attached,
// so we check for attached bodies there as well
const robot_model::LinkModel *attached_link_model = planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second);
if (attached_link_model)
{
std::vector<const robot_state::AttachedBody*> attached_bodies2;
planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model);
attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end());
}
}
// if this end effector has attached objects, we go on
if (!attached_bodies.empty())
{
// if the user specified the name of the attached object to place, we check that indeed
// the group contains this attachd body
if (!attached_object_name.empty())
{
bool found = false;
for (std::size_t j = 0 ; j < attached_bodies.size() ; ++j)
if (attached_bodies[j]->getName() == attached_object_name)
{
found = true;
break;
}
// if the attached body this group has is not the same as the one specified,
// we cannot use this eef
if (!found)
continue;
}
// if we previoulsy have set the eef it means we have more options we could use, so things are ambiguous
if (eef)
{
ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors for group '" << goal.group_name <<
"' that are currently holding objects. It is ambiguous which end-effector to use. Please specify it explicitly.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
// set the end effector (this was initialized to NULL above)
eef = planning_scene->getRobotModel()->getEndEffector(eef_names[i]);
}
}
}
}
// if we know the attached object, but not the eef, we can try to identify that
if (!attached_object_name.empty() && !eef)
{
const robot_state::AttachedBody *attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name);
if (attached_body)
{
// get the robot model link this attached body is associated to
const robot_model::LinkModel *link = attached_body->getAttachedLink();
// check to see if there is a unique end effector containing the link
const std::vector<const robot_model::JointModelGroup*> &eefs = planning_scene->getRobotModel()->getEndEffectors();
for (std::size_t i = 0 ; i < eefs.size() ; ++i)
if (eefs[i]->hasLinkModel(link->getName()))
{
if (eef)
{
ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors that include the link '" << link->getName() <<
"' which is where the body '" << attached_object_name << "' is attached. It is unclear which end-effector to use.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
eef = eefs[i];
}
}
// if the group is also unknown, but we just found out the eef
if (!jmg && eef)
{
const std::string &eef_parent = eef->getEndEffectorParentGroup().first;
if (eef_parent.empty())
{
ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << goal.group_name << "'. Please define a parent group in the SRDF.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
else
jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent);
}
}
if (!jmg || !eef)
{
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
// try to infer attached body name if possible
int loop_count = 0;
while (attached_object_name.empty() && loop_count < 2)
{
// in the first try, look for objects attached to the eef, if the eef is known;
// otherwise, look for attached bodies in the planning group itself
std::vector<const robot_state::AttachedBody*> attached_bodies;
planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg);
loop_count++;
if (attached_bodies.size() > 1)
{
ROS_ERROR_NAMED("manipulation", "Multiple attached bodies for group '%s' but no explicit attached object to place was specified", goal.group_name.c_str());
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
return false;
}
else
attached_object_name = attached_bodies[0]->getName();
}
const robot_state::AttachedBody *attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name);
if (!attached_body)
{
ROS_ERROR_NAMED("manipulation", "There is no object to detach for place action");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME;
return false;
}
ros::WallTime start_time = ros::WallTime::now();
// construct common data for possible manipulation plans
ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
plan_data->planning_group_ = jmg;
plan_data->end_effector_group_ = eef;
plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(eef->getEndEffectorParentGroup().second);
plan_data->timeout_ = endtime;
plan_data->path_constraints_ = goal.path_constraints;
plan_data->planner_id_ = goal.planner_id;
plan_data->minimize_object_distance_ = false;
plan_data->max_goal_sampling_attempts_ = std::max(2u, jmg->getDefaultIKAttempts());
moveit_msgs::AttachedCollisionObject &detach_object_msg = plan_data->diff_attached_object_;
// construct the attached object message that will change the world to what it would become after a placement
detach_object_msg.link_name = attached_body->getAttachedLinkName();
detach_object_msg.object.id = attached_object_name;
detach_object_msg.object.operation = moveit_msgs::CollisionObject::REMOVE;
collision_detection::AllowedCollisionMatrixPtr approach_place_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
// we are allowed to touch certain other objects with the gripper
approach_place_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
// we are allowed to touch the target object slightly while retreating the end effector
std::vector<std::string> touch_links(attached_body->getTouchLinks().begin(), attached_body->getTouchLinks().end());
approach_place_acm->setEntry(attached_object_name, touch_links, true);
if (!goal.support_surface_name.empty())
{
// we are allowed to have contact between the target object and the support surface before the place
approach_place_acm->setEntry(goal.support_surface_name, attached_object_name, true);
// optionally, it may be allowed to touch the support surface with the gripper
if (goal.allow_gripper_support_collision)
approach_place_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
}
// configure the manipulation pipeline
pipeline_.reset();
ManipulationStagePtr stage1(new ReachableAndValidPoseFilter(planning_scene, approach_place_acm, pick_place_->getConstraintsSamplerManager()));
ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_place_acm));
ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
initialize();
pipeline_.start();
// add possible place locations
for (std::size_t i = 0 ; i < goal.place_locations.size() ; ++i)
{
ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
const moveit_msgs::PlaceLocation &pl = goal.place_locations[i];
if (goal.place_eef)
p->goal_pose_ = pl.place_pose;
else
// The goals are specified for the attached body
// but we want to transform them into goals for the end-effector instead
if (!transformToEndEffectorGoal(pl.place_pose, attached_body, p->goal_pose_))
{
p->goal_pose_ = pl.place_pose;
ROS_ERROR_NAMED("manipulation", "Unable to transform the desired pose of the object to the pose of the end-effector");
}
p->approach_ = pl.pre_place_approach;
p->retreat_ = pl.post_place_retreat;
p->retreat_posture_ = pl.post_place_posture;
p->id_ = i;
if (p->retreat_posture_.joint_names.empty())
p->retreat_posture_ = attached_body->getDetachPosture();
pipeline_.push(p);
}
ROS_INFO_NAMED("manipulation", "Added %d place locations", (int) goal.place_locations.size());
// wait till we're done
waitForPipeline(endtime);
pipeline_.stop();
last_plan_time_ = (ros::WallTime::now() - start_time).toSec();
if (!getSuccessfulManipulationPlans().empty())
error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
else
{
if (last_plan_time_ > timeout)
error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
else
{
error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
if (goal.place_locations.size() > 0)
{
ROS_WARN_NAMED("manipulation", "All supplied place locations failed. Retrying last location in verbose mode.");
// everything failed. we now start the pipeline again in verbose mode for one grasp
initialize();
pipeline_.setVerbose(true);
pipeline_.start();
pipeline_.reprocessLastFailure();
waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0));
pipeline_.stop();
pipeline_.setVerbose(false);
}
}
}
ROS_INFO_NAMED("manipulation", "Place planning completed after %lf seconds", last_plan_time_);
return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
}
virtual bool adaptAndPlan(const PlannerFn &planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest &req,
planning_interface::MotionPlanResponse &res,
std::vector<std::size_t> &added_path_index) const
{
ROS_DEBUG("Running '%s'", getDescription().c_str());
// get the specified start state
robot_state::RobotState start_state = planning_scene->getCurrentState();
robot_state::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.start_state, start_state);
const std::vector<robot_state::JointState*> &jstates =
planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
start_state.getJointStateGroup(req.group_name)->getJointStateVector() :
start_state.getJointStateVector();
bool change_req = false;
for (std::size_t i = 0 ; i < jstates.size() ; ++i)
{
// Check if we have a revolute, continuous joint. If we do, then we only need to make sure
// it is within de model's declared bounds (usually -Pi, Pi), since the values wrap around.
// It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform
// how many times the joint was wrapped. Because of this, we remember the offsets for continuous
// joints, and we un-do them when the plan comes from the planner
const robot_model::JointModel* jm = jstates[i]->getJointModel();
if (jm->getType() == robot_model::JointModel::REVOLUTE)
{
if (static_cast<const robot_model::RevoluteJointModel*>(jm)->isContinuous())
{
double initial = jstates[i]->getVariableValues()[0];
jstates[i]->enforceBounds();
double after = jstates[i]->getVariableValues()[0];
if (fabs(initial - after) > std::numeric_limits<double>::epsilon())
change_req = true;
}
}
else
// Normalize yaw; no offset needs to be remembered
if (jm->getType() == robot_model::JointModel::PLANAR)
{
double initial = jstates[i]->getVariableValues()[2];
if (static_cast<const robot_model::PlanarJointModel*>(jm)->normalizeRotation(jstates[i]->getVariableValues()))
change_req = true;
}
else
// Normalize quaternions
if (jm->getType() == robot_model::JointModel::FLOATING)
{
if (static_cast<const robot_model::FloatingJointModel*>(jm)->normalizeRotation(jstates[i]->getVariableValues()))
change_req = true;
}
}
// pointer to a prefix state we could possibly add, if we detect we have to make changes
robot_state::RobotStatePtr prefix_state;
for (std::size_t i = 0 ; i < jstates.size() ; ++i)
{
if (!jstates[i]->satisfiesBounds())
{
if (jstates[i]->satisfiesBounds(bounds_dist_))
{
if (!prefix_state)
prefix_state.reset(new robot_state::RobotState(start_state));
jstates[i]->enforceBounds();
change_req = true;
ROS_INFO("Starting state is just outside bounds (joint '%s'). Assuming within bounds.", jstates[i]->getName().c_str());
}
else
{
std::stringstream joint_values;
std::stringstream joint_bounds_low;
std::stringstream joint_bounds_hi;
for (std::size_t k = 0 ; k < jstates[i]->getVariableValues().size() ; ++k)
joint_values << jstates[i]->getVariableValues()[k] << " ";
for (std::size_t k = 0 ; k < jstates[i]->getVariableBounds().size() ; ++k)
{
joint_bounds_low << jstates[i]->getVariableBounds()[k].first << " ";
joint_bounds_hi << jstates[i]->getVariableBounds()[k].second << " ";
}
ROS_WARN_STREAM("Joint '" << jstates[i]->getName() << "' from the starting state is outside bounds by a significant margin: [ " << joint_values.str() << "] should be in the range [ " << joint_bounds_low.str() <<
"], [ " << joint_bounds_hi.str() << "] but the error above the ~" << BOUNDS_PARAM_NAME << " parameter (currently set to " << bounds_dist_ << ")");
}
}
}
bool solved;
// if we made any changes, use them
if (change_req)
{
planning_interface::MotionPlanRequest req2 = req;
robot_state::robotStateToRobotStateMsg(start_state, req2.start_state);
solved = planner(planning_scene, req2, res);
}
else
solved = planner(planning_scene, req, res);
// re-add the prefix state, if it was constructed
if (prefix_state && res.trajectory_)
{
if (!res.trajectory_->empty())
// heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to the computed trajectory)
res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration()));
res.trajectory_->addPrefixWayPoint(prefix_state, 0.0);
added_path_index.push_back(0);
}
return solved;
}
bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit_msgs::MotionPlanRequest& req, const chomp::ChompParameters& params,
moveit_msgs::MotionPlanDetailedResponse& res) const
{
if (!planning_scene)
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "No planning scene initialized.");
res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
return false;
}
if (req.start_state.joint_state.position.empty())
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state is empty");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
return false;
}
if (not planning_scene->getRobotModel()->satisfiesPositionBounds(req.start_state.joint_state.position.data()))
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state violates joint limits");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
return false;
}
ros::WallTime start_time = ros::WallTime::now();
ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name);
jointStateToArray(planning_scene->getRobotModel(), req.start_state.joint_state, req.group_name,
trajectory.getTrajectoryPoint(0));
if (req.goal_constraints.empty())
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "No goal constraints specified!");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
return false;
}
if (req.goal_constraints[0].joint_constraints.empty())
{
ROS_ERROR_STREAM("Only joint-space goals are supported");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
return false;
}
int goal_index = trajectory.getNumPoints() - 1;
trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
sensor_msgs::JointState js;
for (const moveit_msgs::JointConstraint& joint_constraint : req.goal_constraints[0].joint_constraints)
{
js.name.push_back(joint_constraint.joint_name);
js.position.push_back(joint_constraint.position);
ROS_INFO_STREAM_NAMED("chomp_planner", "Setting joint " << joint_constraint.joint_name << " to position "
<< joint_constraint.position);
}
jointStateToArray(planning_scene->getRobotModel(), js, req.group_name, trajectory.getTrajectoryPoint(goal_index));
const moveit::core::JointModelGroup* model_group =
planning_scene->getRobotModel()->getJointModelGroup(req.group_name);
// fix the goal to move the shortest angular distance for wrap-around joints:
for (size_t i = 0; i < model_group->getActiveJointModels().size(); i++)
{
const moveit::core::JointModel* model = model_group->getActiveJointModels()[i];
const moveit::core::RevoluteJointModel* revolute_joint =
dynamic_cast<const moveit::core::RevoluteJointModel*>(model);
if (revolute_joint != nullptr)
{
if (revolute_joint->isContinuous())
{
double start = (trajectory)(0, i);
double end = (trajectory)(goal_index, i);
ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
(trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
}
}
}
const std::vector<std::string>& active_joint_names = model_group->getActiveJointModelNames();
const Eigen::MatrixXd goal_state = trajectory.getTrajectoryPoint(goal_index);
moveit::core::RobotState goal_robot_state = planning_scene->getCurrentState();
goal_robot_state.setVariablePositions(
active_joint_names, std::vector<double>(goal_state.data(), goal_state.data() + active_joint_names.size()));
if (not goal_robot_state.satisfiesBounds())
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal state violates joint limits");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
return false;
}
// fill in an initial trajectory based on user choice from the chomp_config.yaml file
if (params.trajectory_initialization_method_.compare("quintic-spline") == 0)
trajectory.fillInMinJerk();
else if (params.trajectory_initialization_method_.compare("linear") == 0)
trajectory.fillInLinearInterpolation();
else if (params.trajectory_initialization_method_.compare("cubic") == 0)
trajectory.fillInCubicInterpolation();
else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0)
{
if (!(trajectory.fillInFromTrajectory(res)))
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Input trajectory has less than 2 points, "
"trajectory must contain at least start and goal state");
return false;
}
}
else
ROS_ERROR_STREAM_NAMED("chomp_planner", "invalid interpolation method specified in the chomp_planner file");
ROS_INFO_NAMED("chomp_planner", "CHOMP trajectory initialized using method: %s ",
(params.trajectory_initialization_method_).c_str());
// optimize!
moveit::core::RobotState start_state(planning_scene->getCurrentState());
moveit::core::robotStateMsgToRobotState(req.start_state, start_state);
start_state.update();
ros::WallTime create_time = ros::WallTime::now();
int replan_count = 0;
bool replan_flag = false;
double org_learning_rate = 0.04, org_ridge_factor = 0.0, org_planning_time_limit = 10;
int org_max_iterations = 200;
// storing the initial chomp parameters values
org_learning_rate = params.learning_rate_;
org_ridge_factor = params.ridge_factor_;
org_planning_time_limit = params.planning_time_limit_;
org_max_iterations = params.max_iterations_;
std::unique_ptr<ChompOptimizer> optimizer;
// create a non_const_params variable which stores the non constant version of the const params variable
ChompParameters params_nonconst = params;
// while loop for replanning (recovery behaviour) if collision free optimized solution not found
while (true)
{
if (replan_flag)
{
// increase learning rate in hope to find a successful path; increase ridge factor to avoid obstacles; add 5
// additional secs in hope to find a solution; increase maximum iterations
params_nonconst.setRecoveryParams(params_nonconst.learning_rate_ + 0.02, params_nonconst.ridge_factor_ + 0.002,
params_nonconst.planning_time_limit_ + 5, params_nonconst.max_iterations_ + 50);
}
// initialize a ChompOptimizer object to load up the optimizer with default parameters or with updated parameters in
// case of a recovery behaviour
optimizer.reset(new ChompOptimizer(&trajectory, planning_scene, req.group_name, ¶ms_nonconst, start_state));
if (!optimizer->isInitialized())
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Could not initialize optimizer");
res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
return false;
}
ROS_DEBUG_NAMED("chomp_planner", "Optimization took %f sec to create",
(ros::WallTime::now() - create_time).toSec());
bool optimization_result = optimizer->optimize();
// replan with updated parameters if no solution is found
if (params_nonconst.enable_failure_recovery_)
{
ROS_INFO_NAMED("chomp_planner", "Planned with Chomp Parameters (learning_rate, ridge_factor, "
"planning_time_limit, max_iterations), attempt: # %d ",
(replan_count + 1));
ROS_INFO_NAMED("chomp_planner", "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ",
params_nonconst.learning_rate_, params_nonconst.ridge_factor_,
params_nonconst.planning_time_limit_, params_nonconst.max_iterations_);
if (!optimization_result && replan_count < params_nonconst.max_recovery_attempts_)
{
replan_count++;
replan_flag = true;
}
else
{
break;
}
}
else
break;
} // end of while loop
// resetting the CHOMP Parameters to the original values after a successful plan
params_nonconst.setRecoveryParams(org_learning_rate, org_ridge_factor, org_planning_time_limit, org_max_iterations);
ROS_DEBUG_NAMED("chomp_planner", "Optimization actually took %f sec to run",
(ros::WallTime::now() - create_time).toSec());
create_time = ros::WallTime::now();
// assume that the trajectory is now optimized, fill in the output structure:
ROS_DEBUG_NAMED("chomp_planner", "Output trajectory has %d joints", trajectory.getNumJoints());
res.trajectory.resize(1);
res.trajectory[0].joint_trajectory.joint_names = active_joint_names;
res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header; // @TODO this is probably a hack
// fill in the entire trajectory
res.trajectory[0].joint_trajectory.points.resize(trajectory.getNumPoints());
for (int i = 0; i < trajectory.getNumPoints(); i++)
{
res.trajectory[0].joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
for (size_t j = 0; j < res.trajectory[0].joint_trajectory.points[i].positions.size(); j++)
{
res.trajectory[0].joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
}
// Setting invalid timestamps.
// Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints.
res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
}
ROS_DEBUG_NAMED("chomp_planner", "Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec());
ROS_DEBUG_NAMED("chomp_planner", "Serviced planning request in %f wall-seconds, trajectory duration is %f",
(ros::WallTime::now() - start_time).toSec(),
res.trajectory[0].joint_trajectory.points[goal_index].time_from_start.toSec());
res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
res.processing_time.push_back((ros::WallTime::now() - start_time).toSec());
// report planning failure if path has collisions
if (not optimizer->isCollisionFree())
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Motion plan is invalid.");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
return false;
}
// check that final state is within goal tolerances
kinematic_constraints::JointConstraint jc(planning_scene->getRobotModel());
robot_state::RobotState last_state(start_state);
last_state.setVariablePositions(res.trajectory[0].joint_trajectory.joint_names,
res.trajectory[0].joint_trajectory.points.back().positions);
bool constraints_are_ok = true;
for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints[0].joint_constraints)
{
constraints_are_ok = constraints_are_ok and jc.configure(constraint);
constraints_are_ok = constraints_are_ok and jc.decide(last_state).satisfied;
if (not constraints_are_ok)
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal constraints are violated: " << constraint.joint_name);
res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED;
return false;
}
}
return true;
}
virtual bool adaptAndPlan(const PlannerFn &planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest &req,
planning_interface::MotionPlanResponse &res,
std::vector<std::size_t> &added_path_index) const
{
ROS_DEBUG("Running '%s'", getDescription().c_str());
// get the specified start state
robot_state::RobotState start_state = planning_scene->getCurrentState();
robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
collision_detection::CollisionRequest creq;
creq.group_name = req.group_name;
collision_detection::CollisionResult cres;
planning_scene->checkCollision(creq, cres, start_state);
if (cres.collision)
{
// Rerun in verbose mode
collision_detection::CollisionRequest vcreq = creq;
collision_detection::CollisionResult vcres;
vcreq.verbose = true;
planning_scene->checkCollision(vcreq, vcres, start_state);
if (creq.group_name.empty())
ROS_INFO("Start state appears to be in collision");
else
ROS_INFO_STREAM("Start state appears to be in collision with respect to group " << creq.group_name);
robot_state::RobotStatePtr prefix_state(new robot_state::RobotState(start_state));
random_numbers::RandomNumberGenerator &rng = prefix_state->getRandomNumberGenerator();
const std::vector<const robot_model::JointModel*> &jmodels =
planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
planning_scene->getRobotModel()->getJointModels();
bool found = false;
for (int c = 0 ; !found && c < sampling_attempts_ ; ++c)
{
for (std::size_t i = 0 ; !found && i < jmodels.size() ; ++i)
{
std::vector<double> sampled_variable_values(jmodels[i]->getVariableCount());
const double *original_values = prefix_state->getJointPositions(jmodels[i]);
jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values, jmodels[i]->getMaximumExtent() * jiggle_fraction_);
start_state.setJointPositions(jmodels[i], sampled_variable_values);
collision_detection::CollisionResult cres;
planning_scene->checkCollision(creq, cres, start_state);
if (!cres.collision)
{
found = true;
ROS_INFO("Found a valid state near the start state at distance %lf after %d attempts", prefix_state->distance(start_state), c);
}
}
}
if (found)
{
planning_interface::MotionPlanRequest req2 = req;
robot_state::robotStateToRobotStateMsg(start_state, req2.start_state);
bool solved = planner(planning_scene, req2, res);
if (solved && !res.trajectory_->empty())
{
// heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to the computed trajectory)
res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration()));
res.trajectory_->addPrefixWayPoint(prefix_state, 0.0);
// we add a prefix point, so we need to bump any previously added index positions
for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
added_path_index[i]++;
added_path_index.push_back(0);
}
return solved;
}
else
{
ROS_WARN("Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %u sampling attempts). Passing the original planning request to the planner.",
jiggle_fraction_, sampling_attempts_);
return planner(planning_scene, req, res);
}
}
else
{
if (creq.group_name.empty())
ROS_DEBUG("Start state is valid");
else
ROS_DEBUG_STREAM("Start state is valid with respect to group " << creq.group_name);
return planner(planning_scene, req, res);
}
}
bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PickupGoal &goal)
{
double timeout = goal.allowed_planning_time;
ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
std::string planning_group = goal.group_name;
std::string end_effector = goal.end_effector;
if (end_effector.empty() && !planning_group.empty())
{
const robot_model::JointModelGroup *jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
if (!jmg)
{
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
const std::vector<std::string> &eefs = jmg->getAttachedEndEffectorNames();
if (!eefs.empty())
{
end_effector = eefs.front();
if (eefs.size() > 1)
ROS_WARN_STREAM_NAMED("manipulation", "Choice of end-effector for group '" << planning_group << "' is ambiguous. Assuming '" << end_effector << "'");
}
}
else
if (!end_effector.empty() && planning_group.empty())
{
const robot_model::JointModelGroup *jmg = planning_scene->getRobotModel()->getEndEffector(end_effector);
if (!jmg)
{
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
planning_group = jmg->getEndEffectorParentGroup().first;
if (planning_group.empty())
{
ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << end_effector << "'. Please define a parent group in the SRDF.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
else
ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '" << planning_group << "'");
}
const robot_model::JointModelGroup *eef = end_effector.empty() ? NULL : planning_scene->getRobotModel()->getEndEffector(end_effector);
if (!eef)
{
ROS_ERROR_NAMED("manipulation", "No end-effector specified for pick action");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
const std::string &ik_link = eef->getEndEffectorParentGroup().second;
ros::WallTime start_time = ros::WallTime::now();
// construct common data for possible manipulation plans
ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
plan_data->end_effector_group_ = eef;
plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(ik_link);
plan_data->timeout_ = endtime;
plan_data->path_constraints_ = goal.path_constraints;
plan_data->planner_id_ = goal.planner_id;
plan_data->minimize_object_distance_ = goal.minimize_object_distance;
plan_data->max_goal_sampling_attempts_ = std::max(2u, plan_data->planning_group_->getDefaultIKAttempts());
moveit_msgs::AttachedCollisionObject &attach_object_msg = plan_data->diff_attached_object_;
// construct the attached object message that will change the world to what it would become after a pick
attach_object_msg.link_name = ik_link;
attach_object_msg.object.id = goal.target_name;
attach_object_msg.object.operation = moveit_msgs::CollisionObject::ADD;
attach_object_msg.touch_links = goal.attached_object_touch_links.empty() ? eef->getLinkModelNames() : goal.attached_object_touch_links;
collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
// we are allowed to touch the target object
approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true);
// we are allowed to touch certain other objects with the gripper
approach_grasp_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
if (!goal.support_surface_name.empty())
{
// we are allowed to have contact between the target object and the support surface before the grasp
approach_grasp_acm->setEntry(goal.support_surface_name, goal.target_name, true);
// optionally, it may be allowed to touch the support surface with the gripper
if (goal.allow_gripper_support_collision)
approach_grasp_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
}
// configure the manipulation pipeline
pipeline_.reset();
ManipulationStagePtr stage1(new ReachableAndValidPoseFilter(planning_scene, approach_grasp_acm, pick_place_->getConstraintsSamplerManager()));
ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_grasp_acm));
ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
initialize();
pipeline_.start();
// order the grasps by quality
std::vector<std::size_t> grasp_order(goal.possible_grasps.size());
for (std::size_t i = 0 ; i < goal.possible_grasps.size() ; ++i)
grasp_order[i] = i;
OrderGraspQuality oq(goal.possible_grasps);
std::sort(grasp_order.begin(), grasp_order.end(), oq);
// feed the available grasps to the stages we set up
for (std::size_t i = 0 ; i < goal.possible_grasps.size() ; ++i)
{
ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
const moveit_msgs::Grasp &g = goal.possible_grasps[grasp_order[i]];
p->approach_ = g.pre_grasp_approach;
p->retreat_ = g.post_grasp_retreat;
p->goal_pose_ = g.grasp_pose;
p->id_ = grasp_order[i];
// if no frame of reference was specified, assume the transform to be in the reference frame of the object
if (p->goal_pose_.header.frame_id.empty())
p->goal_pose_.header.frame_id = goal.target_name;
p->approach_posture_ = g.pre_grasp_posture;
p->retreat_posture_ = g.grasp_posture;
pipeline_.push(p);
}
// wait till we're done
waitForPipeline(endtime);
pipeline_.stop();
last_plan_time_ = (ros::WallTime::now() - start_time).toSec();
if (!getSuccessfulManipulationPlans().empty())
error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
else
{
if (last_plan_time_ > timeout)
error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
else
{
error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
if (goal.possible_grasps.size() > 0)
{
ROS_WARN_NAMED("manipulation", "All supplied grasps failed. Retrying last grasp in verbose mode.");
// everything failed. we now start the pipeline again in verbose mode for one grasp
initialize();
pipeline_.setVerbose(true);
pipeline_.start();
pipeline_.reprocessLastFailure();
waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0));
pipeline_.stop();
pipeline_.setVerbose(false);
}
}
}
ROS_INFO_NAMED("manipulation", "Pickup planning completed after %lf seconds", last_plan_time_);
return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
}
