9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/core/solver-base.hpp" 28 const std::size_t& T =
problem_->get_T();
31 for (std::size_t t = 0; t < T; ++t) {
32 const boost::shared_ptr<ActionModelAbstract>& model =
problem_->get_runningModels()[t];
34 xs_[t] = model->get_state()->zero();
35 us_[t] = Eigen::VectorXd::Zero(
problem_->get_nu_max());
37 xs_.back() =
problem_->get_terminalModel()->get_state()->zero();
40 SolverAbstract::~SolverAbstract() {}
43 const std::vector<Eigen::VectorXd>& us_warm,
const bool& is_feasible) {
44 const std::size_t& T =
problem_->get_T();
46 if (xs_warm.size() == 0) {
47 for (std::size_t t = 0; t < T; ++t) {
48 xs_[t] =
problem_->get_runningModels()[t]->get_state()->zero();
50 xs_.back() =
problem_->get_terminalModel()->get_state()->zero();
52 if (xs_warm.size() != T + 1) {
53 throw_pretty(
"Warm start state has wrong dimension, got " << xs_warm.size() <<
" expecting " << (T + 1));
55 for (std::size_t t = 0; t < T; ++t) {
56 const std::size_t& nx =
problem_->get_runningModels()[t]->get_state()->get_nx();
57 if (static_cast<std::size_t>(xs_warm[t].size()) != nx) {
58 throw_pretty(
"Invalid argument: " 59 <<
"xs_init[" + std::to_string(t) +
"] has wrong dimension (it should be " + std::to_string(nx) +
63 const std::size_t& nx =
problem_->get_terminalModel()->get_state()->get_nx();
64 if (static_cast<std::size_t>(xs_warm[T].size()) != nx) {
65 throw_pretty(
"Invalid argument: " 66 <<
"xs_init[" + std::to_string(T) +
"] has wrong dimension (it should be " + std::to_string(nx) +
69 std::copy(xs_warm.begin(), xs_warm.end(),
xs_.begin());
72 if (us_warm.size() == 0) {
73 for (std::size_t t = 0; t < T; ++t) {
74 us_[t] = Eigen::VectorXd::Zero(
problem_->get_nu_max());
77 if (us_warm.size() != T) {
78 throw_pretty(
"Warm start control has wrong dimension, got " << us_warm.size() <<
" expecting " << T);
80 const std::size_t& nu =
problem_->get_nu_max();
81 for (std::size_t t = 0; t < T; ++t) {
82 if (static_cast<std::size_t>(us_warm[t].size()) > nu) {
83 throw_pretty(
"Invalid argument: " 84 <<
"us_init[" + std::to_string(t) +
"] has wrong dimension (it should be lower than " +
85 std::to_string(nu) +
")");
88 std::copy(us_warm.begin(), us_warm.end(),
us_.begin());
130 const std::size_t& T =
problem_->get_T();
131 if (xs.size() != T + 1) {
132 throw_pretty(
"Invalid argument: " 133 <<
"xs list has to be " + std::to_string(T + 1));
136 const std::size_t& nx =
problem_->get_nx();
137 for (std::size_t t = 0; t < T; ++t) {
138 if (static_cast<std::size_t>(xs[t].size()) != nx) {
139 throw_pretty(
"Invalid argument: " 140 <<
"xs[" + std::to_string(t) +
"] has wrong dimension (it should be " + std::to_string(nx) +
")")
143 if (static_cast<std::size_t>(xs[T].size()) != nx) {
144 throw_pretty(
"Invalid argument: " 145 <<
"xs[" + std::to_string(T) +
"] has wrong dimension (it should be " + std::to_string(nx) +
")")
151 const std::size_t& T =
problem_->get_T();
152 if (us.size() != T) {
153 throw_pretty(
"Invalid argument: " 154 <<
"us list has to be " + std::to_string(T));
157 const std::size_t& nu =
problem_->get_nu_max();
158 for (std::size_t t = 0; t < T; ++t) {
159 if (static_cast<std::size_t>(us[t].size()) != nu) {
160 throw_pretty(
"Invalid argument: " 161 <<
"us[" + std::to_string(t) +
"] has wrong dimension (it should be " + std::to_string(nu) +
")")
169 throw_pretty(
"Invalid argument: " 170 <<
"xreg value has to be positive.");
177 throw_pretty(
"Invalid argument: " 178 <<
"ureg value has to be positive.");
184 if (0. >= th_acceptstep || th_acceptstep > 1) {
185 throw_pretty(
"Invalid argument: " 186 <<
"th_acceptstep value should between 0 and 1.");
193 throw_pretty(
"Invalid argument: " 194 <<
"th_stop value has to higher than 0.");
199 bool raiseIfNaN(
const double& value) {
200 if (std::isnan(value) || std::isinf(value) || value >= 1e30) {
const double & get_dV() const
Return the cost reduction.
bool is_feasible_
Label that indicates is the iteration is feasible.
void set_th_stop(const double &th_stop)
Modify the tolerance for stopping the algorithm.
const std::vector< Eigen::VectorXd > & get_us() const
Return the control trajectory .
const double & get_steplength() const
Return the step length.
const std::vector< Eigen::VectorXd > & get_xs() const
Return the state trajectory .
const boost::shared_ptr< ShootingProblem > & get_problem() const
Return the shooting problem.
double ureg_
Current control regularization values.
double steplength_
Current applied step-length.
void set_th_acceptstep(const double &th_acceptstep)
Modify the threshold used for accepting step.
void setCallbacks(const std::vector< boost::shared_ptr< CallbackAbstract > > &callbacks)
Set a list of callback functions using for diagnostic.
void setCandidate(const std::vector< Eigen::VectorXd > &xs_warm=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &us_warm=DEFAULT_VECTOR, const bool &is_feasible=false)
Set the solver candidate warm-point values .
double xreg_
Current state regularization value.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
double th_acceptstep_
Threshold used for accepting step.
const std::size_t & get_iter() const
Return the number of iterations performed by the solver.
std::vector< Eigen::VectorXd > us_
Control trajectory.
double dV_
Cost reduction obtained by tryStep()
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverAbstract(boost::shared_ptr< ShootingProblem > problem)
Initialize the solver.
Eigen::Vector2d d_
LQ approximation of the expected improvement.
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
double dVexp_
Expected cost reduction.
std::vector< Eigen::VectorXd > xs_
State trajectory.
void set_xreg(const double &xreg)
Modify the state regularization value.
const double & get_cost() const
Return the total cost.
const double & get_stop() const
Return the value computed by stoppingCriteria()
void set_ureg(const double &ureg)
Modify the control regularization value.
const std::vector< boost::shared_ptr< CallbackAbstract > > & getCallbacks() const
"Return the list of callback functions using for diagnostic
const double & get_th_stop() const
Return the tolerance for stopping the algorithm.
double th_stop_
Tolerance for stopping the algorithm.
void set_xs(const std::vector< Eigen::VectorXd > &xs)
Modify the state trajectory .
const double & get_dVexp() const
Return the expected cost reduction.
const double & get_ureg() const
Return the control regularization value.
void set_us(const std::vector< Eigen::VectorXd > &us)
Modify the control trajectory .
const Eigen::Vector2d & get_d() const
Return the LQ approximation of the expected improvement.
std::size_t iter_
Number of iteration performed by the solver.
const double & get_xreg() const
Return the state regularization value.
const double & get_th_acceptstep() const
Return the threshold used for accepting a step.
double stop_
Value computed by stoppingCriteria()
const bool & get_is_feasible() const
Return the feasibility status of the trajectory.