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 ConvexConstraintSolver::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit_msgs::GetMotionPlan::Request &req,
moveit_msgs::GetMotionPlan::Response &res) const
{
// Need to add in joint limit constraints
// Get the position constraints from the collision detector
// The raw algorithm laid out in Chan et. al. is as follows:
// 1. Compute unconstrained motion to go from proxy to goal.
// 2. Get the current contact set by moving the proxy toward the goal by some epsilon and get all collision points.
// 3. Compute constrained motion (convex solver).
// 4. Compute collisions along the constrained motion path.
// 5. Set proxy to stop at the first new contact along path.
// In our framework this will look more like this:
// Outside ths planner:
// 1. Compute error between cartesian end effector and cartesian goal.
// 2. Cap pose error (based on some heuristic?) because we are about to make a linear approximation.
// 3. Use Jinverse to compute the joint deltas for the pose error (or perhaps we should frame this as a velocity problem).
// 4. Add the joint deltas to the start state to get a goal state.
// Inside the planner:
// 1. Get the "current" contact set by moving the proxy toward the goal by some epsilon and get all collision points.
// 2. Compute constrained motion (convex solver).
// 3. Subdivide motion subject to some sort of minimum feature size.
// 4. Move proxy in steps, checking for colliding state along the way. Optionally use interval bisection to refine.
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -/
std::string group_name = req.motion_plan_request.group_name;
std::string ee_group_name;
std::string ee_control_frame;
if(group_name == "right_arm"){ ee_group_name = "r_end_effector"; ee_control_frame = "r_wrist_roll_link"; }
if(group_name == "left_arm") { ee_group_name = "l_end_effector"; ee_control_frame = "l_wrist_roll_link"; }
ROS_WARN("Solving for group [%s], end-effector [%s], control frame [%s] using ConvexConstraintSolver!", group_name.c_str(), ee_group_name.c_str(), ee_control_frame.c_str());
// We "getCurrentState" just to populate the structure with auxiliary info, then copy in the transform info from the planning request.
planning_models::KinematicState start_state = planning_scene->getCurrentState();
planning_models::robotStateToKinematicState(*(planning_scene->getTransforms()), req.motion_plan_request.start_state, start_state);
// constrained_goal_state is the optimization output before interval stepping, proxy_state will be used as we step around.
planning_models::KinematicState proxy_state = start_state;
planning_models::KinematicState constrained_goal_state = start_state;
std::string planning_frame = ros::names::resolve("/", planning_scene->getPlanningFrame());
const planning_models::KinematicState::JointStateGroup * jsg = proxy_state.getJointStateGroup(group_name);
const planning_models::KinematicModel::JointModelGroup * jmg = planning_scene->getKinematicModel()->getJointModelGroup(group_name);
// std::string end_effector_group_name = jmg->getAttachedEndEffectorGroupName();
// const std::vector<std::string>& subgroups = jmg->getSubgroupNames();
// ROS_INFO("End-effector group name is [%s].", end_effector_group_name.c_str());
// for(size_t i = 0; i< subgroups.size(); i++)
// ROS_INFO("Subgroup [%zd] is [%s].", i, subgroups[i].c_str());
const planning_models::KinematicModel::JointModelGroup * ee_jmg = planning_scene->getKinematicModel()->getJointModelGroup(ee_group_name);
// const std::vector<const planning_models::KinematicModel::JointModel*>& joint_models = jmg->getJointModels();
// std::map<std::string, unsigned int> joint_index_map;
// for(size_t i = 0; i < joint_models.size(); i++)
// {
// //ROS_INFO("Group [%s] has joint %d: [%s]", req.motion_plan_request.group_name.c_str(), i, joint_models[i]->getName().c_str());
// joint_index_map[joint_models[i]->getName()] = i;
// }
const std::map<std::string, unsigned int>& joint_index_map = jmg->getJointVariablesIndexMap();
std::vector<double> limits_min, limits_max, joint_vector;
std::vector<std::string> joint_names;
limits_min.resize(7);
limits_max.resize(7);
joint_vector.resize(7);
joint_names.resize(7);
{
const moveit_msgs::Constraints &c = req.motion_plan_request.goal_constraints[1];
for (std::size_t i = 0 ; i < c.joint_constraints.size() ; ++i)
{
std::string joint_name = c.joint_constraints[i].joint_name;
if(joint_index_map.find(joint_name) == joint_index_map.end())
{
ROS_WARN("Didin't find [%s] in the joint map, ignoring...", joint_name.c_str());
continue;
}
unsigned int joint_index = joint_index_map.find(joint_name)->second;
std::vector<moveit_msgs::JointLimits> limits = planning_scene->getKinematicModel()->getJointModel(joint_name)->getLimits();
moveit_msgs::JointLimits limit = limits[0];
if(limit.has_position_limits)
{
limits_min[joint_index] = limit.min_position;
limits_max[joint_index] = limit.max_position;
}
else
{
limits_min[joint_index] = -1E3;
limits_max[joint_index] = 1E3;
}
joint_vector[joint_index] = start_state.getJointState(joint_name)->getVariableValues()[0];
joint_names[joint_index] = joint_name;
//ROS_INFO("Joint [%d] [%s] has min %.2f, value %.2f, max %.2f", joint_index, joint_name.c_str(), limits_min[joint_index], joint_vector[joint_index], limits_max[joint_index]);
}
}
// ======== Extract all contact points and normals from previous collision state, get associated Jacobians ========
std::vector<Eigen::MatrixXd> contact_jacobians;
std::vector<Eigen::Vector3d> contact_normals;
for( collision_detection::CollisionResult::ContactMap::const_iterator it = last_collision_result.contacts.begin(); it != last_collision_result.contacts.end(); ++it)
{
std::string contact1 = it->first.first;
std::string contact2 = it->first.second;
const std::vector<collision_detection::Contact>& vec = it->second;
for(size_t contact_index = 0; contact_index < vec.size(); contact_index++)
{
Eigen::Vector3d point = vec[contact_index].pos;
Eigen::Vector3d normal = vec[contact_index].normal;
double depth = vec[contact_index].depth;
ROS_INFO("Contact between [%s] and [%s] point: %.2f %.2f %.2f normal: %.2f %.2f %.2f depth: %.3f",
contact1.c_str(), contact2.c_str(),
point(0), point(1), point(2),
normal(0), normal(1), normal(2),
depth);
// Contact point needs to be expressed with respect to the link; normals should stay in the common frame
std::string group_contact;
if( jmg->hasLinkModel(contact1) || ee_jmg->hasLinkModel(contact1) ) group_contact = contact1;
else if( jmg->hasLinkModel(contact2) || ee_jmg->hasLinkModel(contact2) ) group_contact = contact2;
else
{
ROS_WARN("Contact isn't on group [%s], skipping...", req.motion_plan_request.group_name.c_str());
continue;
}
planning_models::KinematicState::LinkState *link_state = start_state.getLinkState(group_contact);
Eigen::Affine3d link_T_world = link_state->getGlobalCollisionBodyTransform().inverse();
point = link_T_world*point;
Eigen::MatrixXd jacobian;
if(jsg->getJacobian(group_contact, point, jacobian))
{
contact_jacobians.push_back(jacobian);
contact_normals.push_back(normal);
}
}
}
// ======== Extract goal "constraints" ========
const moveit_msgs::Constraints &c = req.motion_plan_request.goal_constraints[0];
// Position and Orientation
if(c.position_constraints.size() != 1 || c.orientation_constraints.size() != 1)
{
ROS_ERROR("Currently require exactly one position and orientation constraint. Aborting...");
return false;
}
moveit_msgs::PositionConstraint pc = c.position_constraints[0];
moveit_msgs::OrientationConstraint oc = c.orientation_constraints[0];
pc.header.frame_id = ros::names::resolve("/", pc.header.frame_id);
oc.header.frame_id = ros::names::resolve("/", oc.header.frame_id);
if(pc.link_name != oc.link_name)
{
ROS_ERROR("Currently can't support position and orientation goals that are not for the same link. Aborting...");
return false;
}
if(pc.constraint_region.primitive_poses.size() == 0)
{
ROS_ERROR("Need to specify a single primitive_pose for position constraint region. Aborting...");
return false;
}
if(pc.constraint_region.primitive_poses.size() != 1)
{
ROS_ERROR("Need exactly one 'pose' for the end-effector goal region. Aborting...");
}
if(pc.header.frame_id != planning_frame)
ROS_WARN("The position goal header [%s] and planning_frame [%s] don't match, things are probably all wrong!",
pc.header.frame_id.c_str(), planning_frame.c_str() );
if(oc.header.frame_id != planning_frame)
ROS_WARN("The orientation goal header [%s] and planning_frame [%s] don't match, things are probably all wrong!",
oc.header.frame_id.c_str(), planning_frame.c_str() );
Eigen::Vector3d goal_point;
Eigen::Quaterniond goal_quaternion_e;
{ // scoped so we don't pollute function scope with these message temps
geometry_msgs::Point msg_goal_point = pc.constraint_region.primitive_poses[0].position;
geometry_msgs::Quaternion msg_goal_orientation = oc.orientation;
goal_point = Eigen::Vector3d(msg_goal_point.x, msg_goal_point.y, msg_goal_point.z);
goal_quaternion_e = Eigen::Quaterniond(msg_goal_orientation.w, msg_goal_orientation.x, msg_goal_orientation.y, msg_goal_orientation.z);
}
//ROS_INFO("Computing position and orientation error...");
planning_models::KinematicState::LinkState *link_state = start_state.getLinkState(pc.link_name);
Eigen::Affine3d planning_T_link = link_state->getGlobalLinkTransform();
Eigen::Vector3d ee_point_in_ee_frame = Eigen::Vector3d(pc.target_point_offset.x, pc.target_point_offset.y, pc.target_point_offset.z);
Eigen::Vector3d ee_point_in_planning_frame = planning_T_link*ee_point_in_ee_frame;
// TODO need to make sure these are expressed in the same frame.
Eigen::Vector3d x_error = goal_point - ee_point_in_planning_frame;
Eigen::Vector3d delta_x = x_error;
double x_error_mag = x_error.norm();
double LINEAR_CLIP = 0.02;
if(x_error_mag > LINEAR_CLIP)
delta_x = x_error/x_error_mag*LINEAR_CLIP;
// = = = = Rotations are gross = = = =
Eigen::Quaterniond link_quaternion_e = Eigen::Quaterniond(planning_T_link.rotation());
tf::Quaternion link_quaternion_tf, goal_quaternion_tf;
tf::RotationEigenToTF( link_quaternion_e, link_quaternion_tf );
tf::RotationEigenToTF( goal_quaternion_e, goal_quaternion_tf );
tf::Quaternion delta_quaternion = link_quaternion_tf.inverse()*goal_quaternion_tf;
double rotation_angle = delta_quaternion.getAngle();
//tf::Vector3 rotation_axis = delta_quaternion.getAxis();
//ROS_INFO("Delta is [%.3f] radians about [%.3f, %.3f, %.3f]", rotation_angle, rotation_axis.x(), rotation_axis.y(), rotation_axis.z());
double ANGLE_CLIP = 0.2;
double clipped_rotation_fraction = std::min<double>(1.0, ANGLE_CLIP/fabs(rotation_angle));
if(clipped_rotation_fraction < 0) ROS_ERROR("Clipped rotation fraction < 0, look into this!");
tf::Matrix3x3 clipped_delta_matrix;
tf::Quaternion clipped_goal_quaternion = link_quaternion_tf.slerp(goal_quaternion_tf, clipped_rotation_fraction);
clipped_delta_matrix.setRotation( link_quaternion_tf.inverse()*clipped_goal_quaternion );
tf::Vector3 delta_euler;
clipped_delta_matrix.getRPY(delta_euler[0], delta_euler[1], delta_euler[2]);
//ROS_INFO("Delta euler in link frame = [%.3f, %.3f, %.3f]", delta_euler[0], delta_euler[1], delta_euler[2]);
tf::Matrix3x3 link_matrix(link_quaternion_tf);
delta_euler = link_matrix * delta_euler;
//ROS_INFO("Delta euler in planning_frame = [%.3f, %.3f, %.3f]", delta_euler[0], delta_euler[1], delta_euler[2]);
// Get end-effector Jacobian
// std::string ee_control_frame = "r_wrist_roll_link";
// if(false && jmg->isChain())
// ee_control_frame = planning_scene->getKinematicModel()->getJointModelGroup(jmg->getAttachedEndEffectorGroupName())->getEndEffectorParentGroup().second;
// else
// ROS_WARN("Using r_wrist_roll_link as HARD_CODED value.");
if(ee_control_frame != pc.link_name)
ROS_WARN("ee_control_frame [%s] and position_goal link [%s] aren't the same, this could be bad!", ee_control_frame.c_str(), pc.link_name.c_str());
// ROS_INFO("Getting end-effector Jacobian for local point %.3f, %.3f, %.3f on link [%s]",
// ee_point_in_ee_frame(0),
// ee_point_in_ee_frame(1),
// ee_point_in_ee_frame(2),
// ee_control_frame.c_str());
Eigen::MatrixXd ee_jacobian;
if(!jsg->getJacobian(ee_control_frame , ee_point_in_ee_frame , ee_jacobian))
{
ROS_ERROR("Unable to get end-effector Jacobian! Can't plan, exiting...");
return false;
}
//ROS_INFO_STREAM("End-effector jacobian in planning frame is: \n" << ee_jacobian);
// ======== Pack into solver data structure, run solver. ========
//ROS_INFO("Packing data into the cvx solver...");
// Vars vars;
// Params params;
// Workspace work;
// Settings settings;
// CVX Settings
cvx.set_defaults();
cvx.setup_indexing();
cvx.settings.verbose = 0;
// - - - - - - - load all problem instance data - - - - - - - //
unsigned int N = 7; // number of joints in the chain
// end-effector Jacobian
// TODO magic numbers (though I suppose the CVX solver is already hard-coded)
for(unsigned int row = 0; row < 3; row++ )
{
for(unsigned int col = 0; col < N; col++ )
{
// CVX matrices are COLUMN-MAJOR!!!
cvx.params.J_v[col*3 + row] = ee_jacobian(row, col);
cvx.params.J_w[col*3 + row] = ee_jacobian(row+3, col);
}
}
// Set weights for objective terms
cvx.params.weight_x[0] = 1.0; // translational error
cvx.params.weight_w[0] = 0.1; // angular error (error in radians is numerically much larger than error in meters)
cvx.params.weight_q[0] = 0.001; // only want to barely encourage values to stay small...
// set up constraints from contact set
unsigned int MAX_CONSTRAINTS = 25;
unsigned int constraint_count = std::min<size_t>(MAX_CONSTRAINTS, contact_normals.size());
for(unsigned int constraint = 0; constraint < MAX_CONSTRAINTS; constraint++)
{
if(constraint < constraint_count)
{
// CVX matrices are COLUMN-MAJOR!!!
for (int j = 0; j < 3*7; j++) {
cvx.params.J_c[constraint][j] = contact_jacobians[constraint](j%3, j/3);
}
for (int j = 0; j < 3; j++) {
cvx.params.normal[constraint][j] = contact_normals[constraint][j];
}
}
else{
//printf("setting to zero\n");
for (int j = 0; j < 3*7; j++) {
cvx.params.J_c[constraint][j] = 0;
}
for (int j = 0; j < 3; j++) {
cvx.params.normal[constraint][j] = 0;
}
}
}
for(unsigned int index = 0; index < N; index++ )
{
cvx.params.q[index] = joint_vector[index];
cvx.params.q_min[index] = limits_min[index];
cvx.params.q_max[index] = limits_max[index];
}
// TODO should these be clipped down at all? :)
for(unsigned int index = 0; index < 3; index++ )
{
cvx.params.x_d[index] = delta_x(index); // TODO magic minus sign!!
cvx.params.w_d[index] = delta_euler[index];
}
// - - - - - - - Solve our problem at high speed! - - - - - - - //
long num_iters = 0;
num_iters = cvx.solve();
if(!cvx.work.converged)
{
printf("solving failed to converge in %ld iterations.\n", num_iters);
return false;
}
// ======== Unpack solver result into constrained goal state. ========
ROS_INFO("Finished solving in %ld iterations, unpacking data...", num_iters);
Eigen::VectorXd joint_deltas(N);
std::map<std::string, double> goal_update;
for(size_t joint_index = 0; joint_index < joint_names.size(); joint_index++)
{
std::string joint_name = joint_names[joint_index];
joint_deltas(joint_index) = cvx.vars.q_d[joint_index];
goal_update[joint_name] = cvx.vars.q_d[joint_index] + start_state.getJointState(joint_name)->getVariableValues()[0];
//ROS_INFO("Updated joint [%zd] [%s]: %.3f + %.3f", joint_index, joint_name.c_str(), start_state.getJointState(joint_name)->getVariableValues()[0], cvx.vars.q_d[joint_index]);
}
Eigen::VectorXd cartesian_deltas = ee_jacobian*joint_deltas;
//ROS_INFO("Raw Error: translate [%.3f, %.3f, %.3f]",
// x_error(0), x_error(1), x_error(2));
ROS_INFO("ClipError: translate [%.3f, %.3f, %.3f] euler [%.2f, %.2f, %.2f]",
delta_x(0), delta_x(1), delta_x(2),
delta_euler[0], delta_euler[1], delta_euler[2]);
ROS_INFO("Output: translate [%.3f, %.3f, %.3f] euler [%.2f, %.2f, %.2f]",
cartesian_deltas(0), cartesian_deltas(1), cartesian_deltas(2),
cartesian_deltas(3), cartesian_deltas(4), cartesian_deltas(5));
//goal_update[vars.qdd_c[index] + ]
constrained_goal_state.setStateValues(goal_update);
// ======== Step toward constrained goal, checking for new collisions along the way ========
//double proxy_goal_tolerance = 0.1;
double interpolation_progress = 0.0;
double interpolation_step = 0.34;
collision_detection::CollisionResult collision_result;
while(interpolation_progress <= 1.0)
{
//ROS_INFO("Doing interpolation, with progress %.2f ", interpolation_progress);
// TODO this interpolation scheme might not allow the arm to slide along contacts very well...
planning_models::KinematicStatePtr point;
point.reset(new planning_models::KinematicState(constrained_goal_state));
start_state.interpolate(constrained_goal_state, interpolation_progress, *point);
// get contact set
collision_detection::CollisionRequest collision_request;
// TODO magic number
collision_request.max_contacts = 50;
collision_request.contacts = true;
collision_request.distance = false;
collision_request.verbose = false;
collision_result.clear();
planning_scene->checkCollision(collision_request, collision_result, *point);
if(collision_result.collision)
{
break;
}
else
{
proxy_state = *point;
}
// TODO Use proxy_goal_tolerance to exit if we are close enough!
interpolation_progress += interpolation_step;
}
// Store the last collision result!
last_collision_result = collision_result;
// ======== Convert proxy state to a "trajectory" ========
//ROS_INFO("Converting proxy to a trajectory, and returning...");
moveit_msgs::RobotTrajectory rt;
trajectory_msgs::JointTrajectory traj;
trajectory_msgs::JointTrajectoryPoint pt;
sensor_msgs::JointState js;
planning_models::kinematicStateToJointState(proxy_state, js);
//const planning_models::KinematicModel::JointModelGroup *jmg = planning_scene->getKinematicModel()->getJointModelGroup(req.motion_plan_request.group_name);
// getJointNames
for(size_t i = 0 ; i < js.name.size(); i++)
{
std::string name = js.name[i];
if( !jmg->hasJointModel(name) ) continue;
traj.joint_names.push_back(js.name[i]);
pt.positions.push_back(js.position[i]);
if(js.velocity.size()) pt.velocities.push_back(js.velocity[i]);
}
pt.time_from_start = ros::Duration(req.motion_plan_request.allowed_planning_time*1.0);
traj.points.push_back(pt);
traj.header.stamp = ros::Time::now();
traj.header.frame_id = "odom_combined";
res.trajectory.joint_trajectory = traj;
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);
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;
}
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);
// if the start state is otherwise valid but does not meet path constraints
if (planning_scene->isStateValid(start_state, req.group_name) &&
!planning_scene->isStateValid(start_state, req.path_constraints, req.group_name))
{
ROS_INFO("Path constraints not satisfied for start state...");
planning_scene->isStateValid(start_state, req.path_constraints, req.group_name, true);
ROS_INFO("Planning to path constraints...");
planning_interface::MotionPlanRequest req2 = req;
req2.goal_constraints.resize(1);
req2.goal_constraints[0] = req.path_constraints;
req2.path_constraints = moveit_msgs::Constraints();
planning_interface::MotionPlanResponse res2;
// we call the planner for this additional request, but we do not want to include potential
// index information from that call
std::vector<std::size_t> added_path_index_temp;
added_path_index_temp.swap(added_path_index);
bool solved1 = planner(planning_scene, req2, res2);
added_path_index_temp.swap(added_path_index);
if (solved1)
{
planning_interface::MotionPlanRequest req3 = req;
ROS_INFO("Planned to path constraints. Resuming original planning request.");
// extract the last state of the computed motion plan and set it as the new start state
robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state);
bool solved2 = planner(planning_scene, req3, res);
res.planning_time_ += res2.planning_time_;
if (solved2)
{
// since we add a prefix, we need to correct any existing index positions
for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
added_path_index[i] += res2.trajectory_->getWayPointCount();
// we mark the fact we insert a prefix path (we specify the index position we just added)
for (std::size_t i = 0 ; i < res2.trajectory_->getWayPointCount() ; ++i)
added_path_index.push_back(i);
// we need to append the solution paths.
res2.trajectory_->append(*res.trajectory_, 0.0);
res2.trajectory_->swap(*res.trajectory_);
return true;
}
else
return false;
}
else
{
ROS_WARN("Unable to plan to path constraints. Running usual motion plan.");
bool result = planner(planning_scene, req, res);
res.planning_time_ += res2.planning_time_;
return result;
}
}
else
{
ROS_DEBUG("Path constraints are OK. Running usual motion plan.");
return planner(planning_scene, req, res);
}
}
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 SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit_msgs::GetMotionPlan::Request &req,
moveit_msgs::GetMotionPlan::Response &res,
const PlanningParameters& params) const
{
res.trajectory.joint_trajectory.points.clear();
(const_cast<SBPLInterface*>(this))->last_planning_statistics_ = PlanningStatistics();
planning_models::RobotState *start_state(planning_scene->getCurrentState());
planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state);
ros::WallTime wt = ros::WallTime::now();
boost::shared_ptr<EnvironmentChain3D> env_chain(new EnvironmentChain3D(planning_scene));
if(!env_chain->setupForMotionPlan(planning_scene,
req,
res,
params)) {
//std::cerr << "Env chain setup failing" << std::endl;
return false;
}
//std::cerr << "Creation with params " << params.use_bfs_ << " took " << (ros::WallTime::now()-wt).toSec() << std::endl;
boost::this_thread::interruption_point();
//DummyEnvironment* dummy_env = new DummyEnvironment();
boost::shared_ptr<ARAPlanner> planner(new ARAPlanner(env_chain.get(), true));
planner->set_initialsolution_eps(100.0);
planner->set_search_mode(true);
planner->force_planning_from_scratch();
planner->set_start(env_chain->getPlanningData().start_hash_entry_->stateID);
planner->set_goal(env_chain->getPlanningData().goal_hash_entry_->stateID);
//std::cerr << "Creation took " << (ros::WallTime::now()-wt) << std::endl;
std::vector<int> solution_state_ids;
int solution_cost;
wt = ros::WallTime::now();
//CALLGRIND_START_INSTRUMENTATION;
bool b_ret = planner->replan(10.0, &solution_state_ids, &solution_cost);
//CALLGRIND_STOP_INSTRUMENTATION;
double el = (ros::WallTime::now()-wt).toSec();
std::cerr << "B ret is " << b_ret << " planning time " << el << std::endl;
std::cerr << "Expansions " << env_chain->getPlanningStatistics().total_expansions_
<< " average time " << (env_chain->getPlanningStatistics().total_expansion_time_.toSec()/(env_chain->getPlanningStatistics().total_expansions_*1.0))
<< " hz " << 1.0/(env_chain->getPlanningStatistics().total_expansion_time_.toSec()/(env_chain->getPlanningStatistics().total_expansions_*1.0))
<< std::endl;
std::cerr << "Total coll checks " << env_chain->getPlanningStatistics().coll_checks_ << " hz " << 1.0/(env_chain->getPlanningStatistics().total_coll_check_time_.toSec()/(env_chain->getPlanningStatistics().coll_checks_*1.0)) << std::endl;
std::cerr << "Path length is " << solution_state_ids.size() << std::endl;
if(!b_ret) {
res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
return false;
}
if(solution_state_ids.size() == 0) {
std::cerr << "Success but no path" << std::endl;
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
return false;
}
if(!env_chain->populateTrajectoryFromStateIDSequence(solution_state_ids,
res.trajectory.joint_trajectory)) {
std::cerr << "Success but path bad" << std::endl;
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
return false;
}
ros::WallTime pre_short = ros::WallTime::now();
//std::cerr << "Num traj points before " << res.trajectory.joint_trajectory.points.size() << std::endl;
trajectory_msgs::JointTrajectory traj = res.trajectory.joint_trajectory;
env_chain->attemptShortcut(traj, res.trajectory.joint_trajectory);
//std::cerr << "Num traj points after " << res.trajectory.joint_trajectory.points.size() << std::endl;
//std::cerr << "Time " << (ros::WallTime::now()-pre_short).toSec() << std::endl;
//env_chain->getPlaneBFSMarker(mark, env_chain->getGoalPose().translation().z());
res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
PlanningStatistics stats = env_chain->getPlanningStatistics();
stats.total_planning_time_ = ros::WallDuration(el);
(const_cast<SBPLInterface*>(this))->last_planning_statistics_ = stats;
return true;
}