9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/core/solver-base.hpp" 14 SolverAbstract::SolverAbstract(boost::shared_ptr<ShootingProblem> problem)
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];
33 const std::size_t& nu = model->get_nu();
35 xs_[t] = model->get_state()->zero();
36 us_[t] = Eigen::VectorXd::Zero(nu);
38 xs_.back() = problem_->get_terminalModel()->get_state()->zero();
41 SolverAbstract::~SolverAbstract() {}
43 void SolverAbstract::setCandidate(
const std::vector<Eigen::VectorXd>& xs_warm,
44 const std::vector<Eigen::VectorXd>& us_warm,
const bool& is_feasible) {
45 const std::size_t& T = problem_->get_T();
47 if (xs_warm.size() == 0) {
48 for (std::size_t t = 0; t < T; ++t) {
49 xs_[t] = problem_->get_runningModels()[t]->get_state()->zero();
51 xs_.back() = problem_->get_terminalModel()->get_state()->zero();
53 assert_pretty(xs_warm.size() == T + 1,
54 "Warm start state has wrong dimension, got " << xs_warm.size() <<
" expecting " << (T + 1));
55 std::copy(xs_warm.begin(), xs_warm.end(), xs_.begin());
58 if (us_warm.size() == 0) {
59 for (std::size_t t = 0; t < T; ++t) {
60 const std::size_t& nu = problem_->get_runningModels()[t]->get_nu();
61 us_[t] = Eigen::VectorXd::Zero(nu);
64 assert_pretty(us_warm.size() == T,
65 "Warm start control has wrong dimension, got " << us_warm.size() <<
" expecting " << T);
66 std::copy(us_warm.begin(), us_warm.end(), us_.begin());
68 is_feasible_ = is_feasible;
71 void SolverAbstract::setCallbacks(
const std::vector<boost::shared_ptr<CallbackAbstract> >& callbacks) {
72 callbacks_ = callbacks;
75 const std::vector<boost::shared_ptr<CallbackAbstract> >& SolverAbstract::getCallbacks()
const {
return callbacks_; }
77 const boost::shared_ptr<ShootingProblem>& SolverAbstract::get_problem()
const {
return problem_; }
79 const std::vector<Eigen::VectorXd>& SolverAbstract::get_xs()
const {
return xs_; }
81 const std::vector<Eigen::VectorXd>& SolverAbstract::get_us()
const {
return us_; }
83 const bool& SolverAbstract::get_is_feasible()
const {
return is_feasible_; }
85 const double& SolverAbstract::get_cost()
const {
return cost_; }
87 const double& SolverAbstract::get_stop()
const {
return stop_; }
89 const Eigen::Vector2d& SolverAbstract::get_d()
const {
return d_; }
91 const double& SolverAbstract::get_xreg()
const {
return xreg_; }
93 const double& SolverAbstract::get_ureg()
const {
return ureg_; }
95 const double& SolverAbstract::get_steplength()
const {
return steplength_; }
97 const double& SolverAbstract::get_dV()
const {
return dV_; }
99 const double& SolverAbstract::get_dVexp()
const {
return dVexp_; }
101 const double& SolverAbstract::get_th_acceptstep()
const {
return th_acceptstep_; }
103 const double& SolverAbstract::get_th_stop()
const {
return th_stop_; }
105 const std::size_t& SolverAbstract::get_iter()
const {
return iter_; }
107 void SolverAbstract::set_xs(
const std::vector<Eigen::VectorXd>& xs) {
108 const std::size_t& T = problem_->get_T();
109 if (xs.size() != T + 1) {
110 throw_pretty(
"Invalid argument: " 111 <<
"xs list has to be " + std::to_string(T + 1));
114 for (std::size_t t = 0; t < T; ++t) {
115 const boost::shared_ptr<ActionModelAbstract>& model = problem_->get_runningModels()[t];
116 const std::size_t& nx = model->get_state()->get_nx();
117 if (static_cast<std::size_t>(xs[t].size()) != nx) {
118 throw_pretty(
"Invalid argument: " 119 <<
"xs[" + std::to_string(t) +
"] has wrong dimension (it should be " + std::to_string(nx) +
")")
122 const boost::shared_ptr<ActionModelAbstract>& model = problem_->get_terminalModel();
123 const std::size_t& nx = model->get_state()->get_nx();
124 if (static_cast<std::size_t>(xs[T].size()) != nx) {
125 throw_pretty(
"Invalid argument: " 126 <<
"xs[" + std::to_string(T) +
"] has wrong dimension (it should be " + std::to_string(nx) +
")")
131 void SolverAbstract::set_us(
const std::vector<Eigen::VectorXd>& us) {
132 const std::size_t& T = problem_->get_T();
133 if (us.size() != T) {
134 throw_pretty(
"Invalid argument: " 135 <<
"us list has to be " + std::to_string(T));
138 for (std::size_t t = 0; t < T; ++t) {
139 const boost::shared_ptr<ActionModelAbstract>& model = problem_->get_runningModels()[t];
140 const std::size_t& nu = model->get_nu();
141 if (static_cast<std::size_t>(us[t].size()) != nu && nu != 0) {
142 throw_pretty(
"Invalid argument: " 143 <<
"us[" + std::to_string(t) +
"] has wrong dimension (it should be " + std::to_string(nu) +
")")
149 void SolverAbstract::set_xreg(
const double& xreg) {
151 throw_pretty(
"Invalid argument: " 152 <<
"xreg value has to be positive.");
157 void SolverAbstract::set_ureg(
const double& ureg) {
159 throw_pretty(
"Invalid argument: " 160 <<
"ureg value has to be positive.");
165 void SolverAbstract::set_th_acceptstep(
const double& th_acceptstep) {
166 if (0. >= th_acceptstep || th_acceptstep > 1) {
167 throw_pretty(
"Invalid argument: " 168 <<
"th_acceptstep value should between 0 and 1.");
170 th_acceptstep_ = th_acceptstep;
173 void SolverAbstract::set_th_stop(
const double& th_stop) {
175 throw_pretty(
"Invalid argument: " 176 <<
"th_stop value has to higher than 0.");
181 bool raiseIfNaN(
const double& value) {
182 if (std::isnan(value) || std::isinf(value) || value >= 1e30) {