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; }