void AdaptiveTimeStepping::
stepImpl( const SimulatorTimer& simulatorTimer,
Solver& solver, State& state, WState& well_state,
OutputWriter* outputWriter )
{
const double timestep = simulatorTimer.currentStepLength();
// init last time step as a fraction of the given time step
if( last_timestep_ < 0 ) {
last_timestep_ = restart_factor_ * timestep;
}
// TODO
// take change in well state into account
// create adaptive step timer with previously used sub step size
AdaptiveSimulatorTimer substepTimer( simulatorTimer, last_timestep_, max_time_step_ );
// copy states in case solver has to be restarted (to be revised)
State last_state( state );
WState last_well_state( well_state );
// counter for solver restarts
int restarts = 0;
// sub step time loop
while( ! substepTimer.done() )
{
// get current delta t
const double dt = substepTimer.currentStepLength() ;
// initialize time step control in case current state is needed later
timeStepControl_->initialize( state );
if( timestep_verbose_ )
{
std::cout <<"Substep( " << substepTimer.currentStepNum() << " ), try with stepsize "
<< unit::convert::to(substepTimer.currentStepLength(), unit::day) << " (days)." << std::endl;
}
int linearIterations = -1;
try {
// (linearIterations < 0 means on convergence in solver)
linearIterations = solver.step( dt, state, well_state);
if( solver_verbose_ ) {
// report number of linear iterations
std::cout << "Overall linear iterations used: " << linearIterations << std::endl;
}
}
catch (const Opm::NumericalProblem& e) {
std::cerr << e.what() << std::endl;
// since linearIterations is < 0 this will restart the solver
}
catch (const std::runtime_error& e) {
std::cerr << e.what() << std::endl;
// also catch linear solver not converged
}
// (linearIterations < 0 means no convergence in solver)
if( linearIterations >= 0 )
{
// advance by current dt
++substepTimer;
// compute new time step estimate
double dtEstimate =
timeStepControl_->computeTimeStepSize( dt, linearIterations, state );
// avoid time step size growth
if( restarts > 0 ) {
dtEstimate = std::min( growth_factor_ * dt, dtEstimate );
// solver converged, reset restarts counter
restarts = 0;
}
if( timestep_verbose_ )
{
std::cout << "Substep( " << substepTimer.currentStepNum()-1 // it was already advanced by ++
<< " ) finished at time " << unit::convert::to(substepTimer.simulationTimeElapsed(),unit::day) << " (days)." << std::endl << std::endl;
}
// write data if outputWriter was provided
if( outputWriter ) {
outputWriter->writeTimeStep( substepTimer, state, well_state );
}
// set new time step length
substepTimer.provideTimeStepEstimate( dtEstimate );
// update states
last_state = state ;
last_well_state = well_state;
}
else // in case of no convergence (linearIterations < 0)
{
// increase restart counter
if( restarts >= solver_restart_max_ ) {
OPM_THROW(Opm::NumericalProblem,"Solver failed to converge after " << restarts << " restarts.");
}
const double newTimeStep = restart_factor_ * dt;
// we need to revise this
substepTimer.provideTimeStepEstimate( newTimeStep );
if( solver_verbose_ )
std::cerr << "Solver convergence failed, restarting solver with new time step ("
<< unit::convert::to( newTimeStep, unit::day ) <<" days)." << std::endl;
// reset states
state = last_state;
well_state = last_well_state;
++restarts;
}
}
// store max of the small time step for next reportStep
last_timestep_ = substepTimer.averageStepLength();
if( timestep_verbose_ )
{
substepTimer.report( std::cout );
std::cout << "Suggested next step size = " << unit::convert::to( last_timestep_, unit::day ) << " (days)" << std::endl;
}
if( ! std::isfinite( last_timestep_ ) ) { // check for NaN
last_timestep_ = timestep;
}
}
SimulatorReport AdaptiveTimeStepping::
stepImpl( const SimulatorTimer& simulatorTimer,
Solver& solver, State& state, WState& well_state,
Output* outputWriter )
{
SimulatorReport report;
const double timestep = simulatorTimer.currentStepLength();
// init last time step as a fraction of the given time step
if( suggested_next_timestep_ < 0 ) {
suggested_next_timestep_ = restart_factor_ * timestep;
}
if (full_timestep_initially_) {
suggested_next_timestep_ = timestep;
}
// TODO
// take change in well state into account
// create adaptive step timer with previously used sub step size
AdaptiveSimulatorTimer substepTimer( simulatorTimer, suggested_next_timestep_, max_time_step_ );
// copy states in case solver has to be restarted (to be revised)
State last_state( state );
WState last_well_state( well_state );
// counter for solver restarts
int restarts = 0;
// sub step time loop
while( ! substepTimer.done() )
{
// get current delta t
const double dt = substepTimer.currentStepLength() ;
if( timestep_verbose_ )
{
std::ostringstream ss;
ss <<" Substep " << substepTimer.currentStepNum() << ", stepsize "
<< unit::convert::to(substepTimer.currentStepLength(), unit::day) << " days.";
OpmLog::info(ss.str());
}
SimulatorReport substepReport;
try {
substepReport = solver.step( substepTimer, state, well_state);
report += substepReport;
if( solver_verbose_ ) {
// report number of linear iterations
OpmLog::note("Overall linear iterations used: " + std::to_string(substepReport.total_linear_iterations));
}
}
catch (const Opm::NumericalProblem& e) {
detail::logException(e, solver_verbose_);
// since linearIterations is < 0 this will restart the solver
}
catch (const std::runtime_error& e) {
detail::logException(e, solver_verbose_);
// also catch linear solver not converged
}
catch (const Dune::ISTLError& e) {
detail::logException(e, solver_verbose_);
// also catch errors in ISTL AMG that occur when time step is too large
}
catch (const Dune::MatrixBlockError& e) {
detail::logException(e, solver_verbose_);
// this can be thrown by ISTL's ILU0 in block mode, yet is not an ISTLError
}
if( substepReport.converged )
{
// advance by current dt
++substepTimer;
// create object to compute the time error, simply forwards the call to the model
detail::SolutionTimeErrorSolverWrapper< Solver, State >
relativeChange( solver, last_state, state );
// compute new time step estimate
double dtEstimate =
timeStepControl_->computeTimeStepSize( dt, substepReport.total_linear_iterations, relativeChange, substepTimer.simulationTimeElapsed());
// limit the growth of the timestep size by the growth factor
dtEstimate = std::min( dtEstimate, double(max_growth_ * dt) );
// further restrict time step size growth after convergence problems
if( restarts > 0 ) {
dtEstimate = std::min( growth_factor_ * dt, dtEstimate );
// solver converged, reset restarts counter
restarts = 0;
}
if( timestep_verbose_ )
{
std::ostringstream ss;
ss << " Substep summary: ";
if (report.total_well_iterations != 0) {
ss << "well iterations = " << report.total_well_iterations << ", ";
}
ss << "newton iterations = " << report.total_newton_iterations << ", "
<< "linearizations = " << report.total_linearizations
<< " (" << report.assemble_time << " sec), "
<< "linear iterations = " << report.total_linear_iterations
<< " (" << report.linear_solve_time << " sec)";
OpmLog::info(ss.str());
}
// write data if outputWriter was provided
// if the time step is done we do not need
// to write it as this will be done by the simulator
// anyway.
if( outputWriter && !substepTimer.done() ) {
Opm::time::StopWatch perfTimer;
perfTimer.start();
bool substep = true;
const auto& physicalModel = solver.model();
outputWriter->writeTimeStep( substepTimer, state, well_state, physicalModel, substep);
report.output_write_time += perfTimer.secsSinceStart();
}
// set new time step length
substepTimer.provideTimeStepEstimate( dtEstimate );
// update states
last_state = state ;
last_well_state = well_state;
report.converged = substepTimer.done();
}
else // in case of no convergence (linearIterations < 0)
{
report.converged = false;
// increase restart counter
if( restarts >= solver_restart_max_ ) {
const auto msg = std::string("Solver failed to converge after ")
+ std::to_string(restarts) + " restarts.";
if (solver_verbose_) {
OpmLog::error(msg);
}
OPM_THROW_NOLOG(Opm::NumericalProblem, msg);
}
const double newTimeStep = restart_factor_ * dt;
// we need to revise this
substepTimer.provideTimeStepEstimate( newTimeStep );
if( solver_verbose_ ) {
std::string msg;
msg = "Solver convergence failed, restarting solver with new time step ("
+ std::to_string(unit::convert::to( newTimeStep, unit::day )) + " days).\n";
OpmLog::problem(msg);
}
// reset states
state = last_state;
well_state = last_well_state;
++restarts;
}
}
// store estimated time step for next reportStep
suggested_next_timestep_ = substepTimer.currentStepLength();
if( timestep_verbose_ )
{
std::ostringstream ss;
substepTimer.report(ss);
ss << "Suggested next step size = " << unit::convert::to( suggested_next_timestep_, unit::day ) << " (days)" << std::endl;
OpmLog::note(ss.str());
}
if( ! std::isfinite( suggested_next_timestep_ ) ) { // check for NaN
suggested_next_timestep_ = timestep;
}
return report;
}
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;
}
