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;
}
bool ChompPlannerNode::planKinematicPath(arm_navigation_msgs::GetMotionPlan::Request &reqIn, arm_navigation_msgs::GetMotionPlan::Response &res)
{
arm_navigation_msgs::GetMotionPlan::Request req = reqIn;
if (!(req.motion_plan_request.goal_constraints.position_constraints.empty() && req.motion_plan_request.goal_constraints.orientation_constraints.empty()))
{
ROS_ERROR("CHOMP cannot handle pose contraints yet.");
res.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_GOAL_POSITION_CONSTRAINTS;
return false;
}
sensor_msgs::JointState joint_goal_chomp = arm_navigation_msgs::jointConstraintsToJointState(req.motion_plan_request.goal_constraints.joint_constraints);
ROS_INFO("Chomp goal");
if(joint_goal_chomp.name.size() != joint_goal_chomp.position.size())
{
ROS_ERROR("Invalid chomp goal");
res.error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::INVALID_GOAL_JOINT_CONSTRAINTS;
return false;
}
for(unsigned int i=0; i<joint_goal_chomp.name.size(); i++)
{
ROS_INFO("%s %f",joint_goal_chomp.name[i].c_str(),joint_goal_chomp.position[i]);
}
ros::WallTime start_time = ros::WallTime::now();
ROS_INFO("Received planning request...");
string group_name;
group_name = req.motion_plan_request.group_name;
vector<string> linkNames;
vector<string> attachedBodies;
ros::WallTime start = ros::WallTime::now();
collision_proximity_space_->setupForGroupQueries(group_name,
req.motion_plan_request.start_state,
linkNames,
attachedBodies);
ROS_INFO_STREAM("Setting up for queries time is " << (ros::WallTime::now() - start));
//collision_proximity_space_->visualizeObjectSpheres(collision_proximity_space_->getCurrentLinkNames());
ChompTrajectory trajectory(robot_model_, trajectory_duration_, trajectory_discretization_, group_name);
ROS_INFO("Initial trajectory has %d points", trajectory.getNumPoints());
// set the start state:
jointStateToArray(req.motion_plan_request.start_state.joint_state, group_name, trajectory.getTrajectoryPoint(0));
ROS_INFO_STREAM("Joint state has " << req.motion_plan_request.start_state.joint_state.name.size() << " joints");
// set the goal state equal to start state, and override the joints specified in the goal
// joint constraints
int goal_index = trajectory.getNumPoints()- 1;
trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
jointStateToArray(arm_navigation_msgs::jointConstraintsToJointState(req.motion_plan_request.goal_constraints.joint_constraints), group_name, trajectory.getTrajectoryPoint(goal_index));
map<string, KinematicModel::JointModelGroup*> groupMap = robot_model_->getJointModelGroupMap();
KinematicModel::JointModelGroup* modelGroup = groupMap[group_name];
// fix the goal to move the shortest angular distance for wrap-around joints:
for (size_t i = 0; i < modelGroup->getJointModels().size(); i++)
{
const KinematicModel::JointModel* model = modelGroup->getJointModels()[i];
const KinematicModel::RevoluteJointModel* revoluteJoint = dynamic_cast<const KinematicModel::RevoluteJointModel*>(model);
if (revoluteJoint != NULL)
{
if(revoluteJoint->continuous_)
{
double start = (trajectory)(0, i);
double end = (trajectory)(goal_index, i);
(trajectory)(goal_index, i) = start + angles::shortest_angular_distance(start, end);
}
}
}
// fill in an initial quintic spline trajectory
trajectory.fillInMinJerk();
// set the max planning time:
chomp_parameters_.setPlanningTimeLimit(req.motion_plan_request.allowed_planning_time.toSec());
// optimize!
ros::WallTime create_time = ros::WallTime::now();
ChompOptimizer optimizer(&trajectory, robot_model_, group_name, &chomp_parameters_,
vis_marker_array_publisher_, vis_marker_publisher_, collision_proximity_space_);
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());
vector<double> velocity_limits(trajectory.getNumJoints(), numeric_limits<double>::max());
// fill in joint names:
res.trajectory.joint_trajectory.joint_names.resize(trajectory.getNumJoints());
for (size_t i = 0; i < modelGroup->getJointModels().size(); i++)
{
res.trajectory.joint_trajectory.joint_names[i] = modelGroup->getJointModels()[i]->getName();
// try to retrieve the joint limits:
if (joint_velocity_limits_.find(res.trajectory.joint_trajectory.joint_names[i])==joint_velocity_limits_.end())
{
joint_velocity_limits_[res.trajectory.joint_trajectory.joint_names[i]] = numeric_limits<double>::max();
}
velocity_limits[i] = joint_velocity_limits_[res.trajectory.joint_trajectory.joint_names[i]];
}
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);
}
// 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 = arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS;
res.planning_time = ros::Duration((ros::WallTime::now() - start_time).toSec());
return true;
}
bool ChompPlannerNode::filterJointTrajectory(arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request &request, arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response &res)
{
arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request req = request;
ros::WallTime start_time = ros::WallTime::now();
ROS_INFO_STREAM("Received filtering request with trajectory size " << req.trajectory.points.size());
if(req.path_constraints.joint_constraints.size() > 0 ||
req.path_constraints.position_constraints.size() > 0 ||
req.path_constraints.orientation_constraints.size() > 0 ||
req.path_constraints.visibility_constraints.size() > 0) {
if(use_trajectory_filter_) {
ROS_INFO("Chomp can't handle path constraints, passing through to other trajectory filters");
if(!filter_trajectory_client_.call(req,res)) {
ROS_INFO("Pass through failed");
} else {
ROS_INFO("Pass through succeeded");
}
} else {
ROS_INFO("Chomp can't handle path constraints, and not set up to use additional filter");
}
return true;
}
for (unsigned int i=0; i< req.trajectory.points.size(); i++)
{
req.trajectory.points[i].velocities.resize(req.trajectory.joint_names.size(),0.0);
}
getLimits(req.trajectory, req.limits);
trajectory_msgs::JointTrajectory jtraj;
int num_points = req.trajectory.points.size();
if(num_points > maximum_spline_points_) {
num_points = maximum_spline_points_;
} else if(num_points < minimum_spline_points_) {
num_points = minimum_spline_points_;
}
//create a spline from the trajectory
spline_smoother::CubicTrajectory trajectory_solver;
spline_smoother::SplineTrajectory spline;
planning_environment::setRobotStateAndComputeTransforms(req.start_state,
*collision_proximity_space_->getCollisionModelsInterface()->getPlanningSceneState());
trajectory_solver.parameterize(req.trajectory,req.limits,spline);
double smoother_time;
spline_smoother::getTotalTime(spline, smoother_time);
ROS_INFO_STREAM("Total time given is " << smoother_time);
double t = 0.0;
vector<double> times(num_points);
for(int i = 0; i < num_points; i++,t += smoother_time/(1.0*(num_points-1))) {
times[i] = t;
}
spline_smoother::sampleSplineTrajectory(spline, times, jtraj);
//double planner_time = req.trajectory.points.back().time_from_start.toSec();
t = 0.0;
for(unsigned int i = 0; i < jtraj.points.size(); i++, t += smoother_time/(1.0*(num_points-1))) {
jtraj.points[i].time_from_start = ros::Duration(t);
}
ROS_INFO_STREAM("Sampled trajectory has " << jtraj.points.size() << " points with " << jtraj.points[0].positions.size() << " joints");
string group_name;
group_name = req.group_name;
vector<string> linkNames;
vector<string> attachedBodies;
collision_proximity_space_->setupForGroupQueries(group_name,
req.start_state,
linkNames,
attachedBodies);
ChompTrajectory trajectory(robot_model_, group_name, jtraj);
//configure the distance field - this should just use current state
arm_navigation_msgs::RobotState robot_state = req.start_state;
jointStateToArray(arm_navigation_msgs::createJointState(req.trajectory.joint_names, jtraj.points[0].positions), group_name, trajectory.getTrajectoryPoint(0));
//set the goal state equal to start state, and override the joints specified in the goal
//joint constraints
int goal_index = trajectory.getNumPoints()-1;
trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
sensor_msgs::JointState goal_state = arm_navigation_msgs::createJointState(req.trajectory.joint_names, jtraj.points.back().positions);
jointStateToArray(goal_state, group_name,trajectory.getTrajectoryPoint(goal_index));
map<string, KinematicModel::JointModelGroup*> groupMap = robot_model_->getJointModelGroupMap();
KinematicModel::JointModelGroup* modelGroup = groupMap[group_name];
// fix the goal to move the shortest angular distance for wrap-around joints:
for (size_t i = 0; i < modelGroup->getJointModels().size(); i++)
{
const KinematicModel::JointModel* model = modelGroup->getJointModels()[i];
const KinematicModel::RevoluteJointModel* revoluteJoint = dynamic_cast<const KinematicModel::RevoluteJointModel*>(model);
if (revoluteJoint != NULL)
{
if(revoluteJoint->continuous_)
{
double start = trajectory(0, i);
double end = trajectory(goal_index, i);
trajectory(goal_index, i) = start + angles::shortest_angular_distance(start, end);
}
}
}
//sets other joints
trajectory.fillInMinJerk();
trajectory.overwriteTrajectory(jtraj);
// set the max planning time:
chomp_parameters_.setPlanningTimeLimit(req.allowed_time.toSec());
chomp_parameters_.setFilterMode(true);
// optimize!
ChompOptimizer optimizer(&trajectory, robot_model_, group_name, &chomp_parameters_,
vis_marker_array_publisher_, vis_marker_publisher_, collision_proximity_space_);
optimizer.optimize();
// assume that the trajectory is now optimized, fill in the output structure:
vector<double> velocity_limits(trajectory.getNumJoints(), numeric_limits<double>::max());
res.trajectory.points.resize(trajectory.getNumPoints());
// fill in joint names:
res.trajectory.joint_names.resize(trajectory.getNumJoints());
for (size_t i = 0; i < modelGroup->getJointModels().size(); i++)
{
res.trajectory.joint_names[i] = modelGroup->getJointModels()[i]->getName();
velocity_limits[i] = joint_limits_[res.trajectory.joint_names[i]].max_velocity;
}
res.trajectory.header.stamp = ros::Time::now();
res.trajectory.header.frame_id = reference_frame_;
// fill in the entire trajectory
for (size_t i = 0; i < (unsigned int)trajectory.getNumPoints(); i++)
{
res.trajectory.points[i].positions.resize(trajectory.getNumJoints());
res.trajectory.points[i].velocities.resize(trajectory.getNumJoints());
for (size_t j=0; j < res.trajectory.points[i].positions.size(); j++)
{
res.trajectory.points[i].positions[j] = trajectory(i, j);
}
if (i==0)
res.trajectory.points[i].time_from_start = ros::Duration(0.0);
else
{
double duration = trajectory.getDiscretization();
// check with all the joints if this duration is ok, else push it up
for (int j=0; j < trajectory.getNumJoints(); j++)
{
double d = fabs(res.trajectory.points[i].positions[j] - res.trajectory.points[i-1].positions[j]) / velocity_limits[j];
if (d > duration)
duration = d;
}
try {
res.trajectory.points[i].time_from_start = res.trajectory.points[i-1].time_from_start + ros::Duration(duration);
} catch(...) {
ROS_INFO_STREAM("Potentially weird duration of " << duration);
}
}
}
arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request next_req;
arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response next_res;
if(use_trajectory_filter_) {
next_req = req;
next_req.trajectory = res.trajectory;
next_req.allowed_time=ros::Duration(1.0);//req.allowed_time/2.0;
if(filter_trajectory_client_.call(next_req, next_res)) {
ROS_INFO_STREAM("Filter call ok. Sent trajectory had " << res.trajectory.points.size() << " points. Returned trajectory has " << next_res.trajectory.points.size() << " points ");
} else {
ROS_INFO("Filter call not ok");
}
res.trajectory = next_res.trajectory;
res.error_code = next_res.error_code;
res.trajectory.header.stamp = ros::Time::now();
res.trajectory.header.frame_id = reference_frame_;
} else {
res.error_code.val = res.error_code.val = res.error_code.SUCCESS;
}
// for every point in time:
for (unsigned int i=1; i<res.trajectory.points.size()-1; ++i)
{
double dt1 = (res.trajectory.points[i].time_from_start - res.trajectory.points[i-1].time_from_start).toSec();
double dt2 = (res.trajectory.points[i+1].time_from_start - res.trajectory.points[i].time_from_start).toSec();
// for every (joint) trajectory
for (int j=0; j < trajectory.getNumJoints(); ++j)
{
double dx1 = res.trajectory.points[i].positions[j] - res.trajectory.points[i-1].positions[j];
double dx2 = res.trajectory.points[i+1].positions[j] - res.trajectory.points[i].positions[j];
double v1 = dx1/dt1;
double v2 = dx2/dt2;
res.trajectory.points[i].velocities[j] = 0.5*(v1 + v2);
}
}
ROS_INFO("Serviced filter request in %f wall-seconds, trajectory duration is %f", (ros::WallTime::now() - start_time).toSec(), res.trajectory.points.back().time_from_start.toSec());
return true;
}
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;
}
