crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
solver-base.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 #include "crocoddyl/core/solver-base.hpp"
11 
12 namespace crocoddyl {
13 
14 SolverAbstract::SolverAbstract(boost::shared_ptr<ShootingProblem> problem)
15  : problem_(problem),
16  is_feasible_(false),
17  cost_(0.),
18  stop_(0.),
19  xreg_(NAN),
20  ureg_(NAN),
21  steplength_(1.),
22  dV_(0.),
23  dVexp_(0.),
24  th_acceptstep_(0.1),
25  th_stop_(1e-9),
26  iter_(0) {
27  // Allocate common data
28  const std::size_t T = problem_->get_T();
29  xs_.resize(T + 1);
30  us_.resize(T);
31  for (std::size_t t = 0; t < T; ++t) {
32  const boost::shared_ptr<ActionModelAbstract>& model = problem_->get_runningModels()[t];
33 
34  xs_[t] = model->get_state()->zero();
35  us_[t] = Eigen::VectorXd::Zero(problem_->get_nu_max());
36  }
37  xs_.back() = problem_->get_terminalModel()->get_state()->zero();
38 }
39 
40 SolverAbstract::~SolverAbstract() {}
41 
42 void SolverAbstract::setCandidate(const std::vector<Eigen::VectorXd>& xs_warm,
43  const std::vector<Eigen::VectorXd>& us_warm, bool is_feasible) {
44  const std::size_t T = problem_->get_T();
45 
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();
49  }
50  xs_.back() = problem_->get_terminalModel()->get_state()->zero();
51  } else {
52  if (xs_warm.size() != T + 1) {
53  throw_pretty("Warm start state has wrong dimension, got " << xs_warm.size() << " expecting " << (T + 1));
54  }
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) +
60  ")");
61  }
62  }
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) +
67  ")");
68  }
69  std::copy(xs_warm.begin(), xs_warm.end(), xs_.begin());
70  }
71 
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());
75  }
76  } else {
77  if (us_warm.size() != T) {
78  throw_pretty("Warm start control has wrong dimension, got " << us_warm.size() << " expecting " << T);
79  }
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) + ")");
86  }
87  }
88  std::copy(us_warm.begin(), us_warm.end(), us_.begin());
89  }
90  is_feasible_ = is_feasible;
91 }
92 
93 void SolverAbstract::setCallbacks(const std::vector<boost::shared_ptr<CallbackAbstract> >& callbacks) {
94  callbacks_ = callbacks;
95 }
96 
97 const std::vector<boost::shared_ptr<CallbackAbstract> >& SolverAbstract::getCallbacks() const { return callbacks_; }
98 
99 const boost::shared_ptr<ShootingProblem>& SolverAbstract::get_problem() const { return problem_; }
100 
101 const std::vector<Eigen::VectorXd>& SolverAbstract::get_xs() const { return xs_; }
102 
103 const std::vector<Eigen::VectorXd>& SolverAbstract::get_us() const { return us_; }
104 
106 
107 double SolverAbstract::get_cost() const { return cost_; }
108 
109 double SolverAbstract::get_stop() const { return stop_; }
110 
111 const Eigen::Vector2d& SolverAbstract::get_d() const { return d_; }
112 
113 double SolverAbstract::get_xreg() const { return xreg_; }
114 
115 double SolverAbstract::get_ureg() const { return ureg_; }
116 
117 double SolverAbstract::get_steplength() const { return steplength_; }
118 
119 double SolverAbstract::get_dV() const { return dV_; }
120 
121 double SolverAbstract::get_dVexp() const { return dVexp_; }
122 
124 
125 double SolverAbstract::get_th_stop() const { return th_stop_; }
126 
127 std::size_t SolverAbstract::get_iter() const { return iter_; }
128 
129 void SolverAbstract::set_xs(const std::vector<Eigen::VectorXd>& xs) {
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));
134  }
135 
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) + ")")
141  }
142  }
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) + ")")
146  }
147  xs_ = xs;
148 }
149 
150 void SolverAbstract::set_us(const std::vector<Eigen::VectorXd>& us) {
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));
155  }
156 
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) + ")")
162  }
163  }
164  us_ = us;
165 }
166 
167 void SolverAbstract::set_xreg(const double xreg) {
168  if (xreg < 0.) {
169  throw_pretty("Invalid argument: "
170  << "xreg value has to be positive.");
171  }
172  xreg_ = xreg;
173 }
174 
175 void SolverAbstract::set_ureg(const double ureg) {
176  if (ureg < 0.) {
177  throw_pretty("Invalid argument: "
178  << "ureg value has to be positive.");
179  }
180  ureg_ = ureg;
181 }
182 
183 void SolverAbstract::set_th_acceptstep(const double th_acceptstep) {
184  if (0. >= th_acceptstep || th_acceptstep > 1) {
185  throw_pretty("Invalid argument: "
186  << "th_acceptstep value should between 0 and 1.");
187  }
188  th_acceptstep_ = th_acceptstep;
189 }
190 
191 void SolverAbstract::set_th_stop(const double th_stop) {
192  if (th_stop <= 0.) {
193  throw_pretty("Invalid argument: "
194  << "th_stop value has to higher than 0.");
195  }
196  th_stop_ = th_stop;
197 }
198 
199 bool raiseIfNaN(const double value) {
200  if (std::isnan(value) || std::isinf(value) || value >= 1e30) {
201  return true;
202  } else {
203  return false;
204  }
205 }
206 
207 } // namespace crocoddyl
crocoddyl::SolverAbstract::th_acceptstep_
double th_acceptstep_
Threshold used for accepting step.
Definition: solver-base.hpp:273
crocoddyl::SolverAbstract::get_d
const Eigen::Vector2d & get_d() const
Return the LQ approximation of the expected improvement.
Definition: solver-base.cpp:111
crocoddyl::SolverAbstract::SolverAbstract
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverAbstract(boost::shared_ptr< ShootingProblem > problem)
Initialize the solver.
Definition: solver-base.cpp:14
crocoddyl::SolverAbstract::get_steplength
double get_steplength() const
Return the step length.
Definition: solver-base.cpp:117
crocoddyl::SolverAbstract::set_th_acceptstep
void set_th_acceptstep(const double th_acceptstep)
Modify the threshold used for accepting step.
Definition: solver-base.cpp:183
crocoddyl::SolverAbstract::is_feasible_
bool is_feasible_
Label that indicates is the iteration is feasible.
Definition: solver-base.hpp:264
crocoddyl::SolverAbstract::d_
Eigen::Vector2d d_
LQ approximation of the expected improvement.
Definition: solver-base.hpp:267
crocoddyl::SolverAbstract::get_cost
double get_cost() const
Return the total cost.
Definition: solver-base.cpp:107
crocoddyl::SolverAbstract::iter_
std::size_t iter_
Number of iteration performed by the solver.
Definition: solver-base.hpp:275
crocoddyl::SolverAbstract::set_xs
void set_xs(const std::vector< Eigen::VectorXd > &xs)
Modify the state trajectory .
Definition: solver-base.cpp:129
crocoddyl::SolverAbstract::ureg_
double ureg_
Current control regularization values.
Definition: solver-base.hpp:269
crocoddyl::SolverAbstract::stop_
double stop_
Value computed by stoppingCriteria()
Definition: solver-base.hpp:266
crocoddyl::SolverAbstract::getCallbacks
const std::vector< boost::shared_ptr< CallbackAbstract > > & getCallbacks() const
"Return the list of callback functions using for diagnostic
Definition: solver-base.cpp:97
crocoddyl::SolverAbstract::set_th_stop
void set_th_stop(const double th_stop)
Modify the tolerance for stopping the algorithm.
Definition: solver-base.cpp:191
crocoddyl::SolverAbstract::get_is_feasible
bool get_is_feasible() const
Return the feasibility status of the trajectory.
Definition: solver-base.cpp:105
crocoddyl::SolverAbstract::get_dV
double get_dV() const
Return the cost reduction.
Definition: solver-base.cpp:119
crocoddyl::SolverAbstract::steplength_
double steplength_
Current applied step-length.
Definition: solver-base.hpp:270
crocoddyl::SolverAbstract::setCandidate
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 .
Definition: solver-base.cpp:42
crocoddyl::SolverAbstract::th_stop_
double th_stop_
Tolerance for stopping the algorithm.
Definition: solver-base.hpp:274
crocoddyl::SolverAbstract::get_xreg
double get_xreg() const
Return the state regularization value.
Definition: solver-base.cpp:113
crocoddyl::SolverAbstract::problem_
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
Definition: solver-base.hpp:260
crocoddyl::SolverAbstract::get_th_acceptstep
double get_th_acceptstep() const
Return the threshold used for accepting a step.
Definition: solver-base.cpp:123
crocoddyl::SolverAbstract::callbacks_
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
Definition: solver-base.hpp:263
crocoddyl::SolverAbstract::setCallbacks
void setCallbacks(const std::vector< boost::shared_ptr< CallbackAbstract > > &callbacks)
Set a list of callback functions using for diagnostic.
Definition: solver-base.cpp:93
crocoddyl::SolverAbstract::get_xs
const std::vector< Eigen::VectorXd > & get_xs() const
Return the state trajectory .
Definition: solver-base.cpp:101
crocoddyl::SolverAbstract::xreg_
double xreg_
Current state regularization value.
Definition: solver-base.hpp:268
crocoddyl::SolverAbstract::get_ureg
double get_ureg() const
Return the control regularization value.
Definition: solver-base.cpp:115
crocoddyl::SolverAbstract::dVexp_
double dVexp_
Expected cost reduction.
Definition: solver-base.hpp:272
crocoddyl::SolverAbstract::us_
std::vector< Eigen::VectorXd > us_
Control trajectory.
Definition: solver-base.hpp:262
crocoddyl::SolverAbstract::get_dVexp
double get_dVexp() const
Return the expected cost reduction.
Definition: solver-base.cpp:121
crocoddyl::SolverAbstract::get_iter
std::size_t get_iter() const
Return the number of iterations performed by the solver.
Definition: solver-base.cpp:127
crocoddyl::SolverAbstract::cost_
double cost_
Total cost.
Definition: solver-base.hpp:265
crocoddyl::SolverAbstract::xs_
std::vector< Eigen::VectorXd > xs_
State trajectory.
Definition: solver-base.hpp:261
crocoddyl::SolverAbstract::set_xreg
void set_xreg(const double xreg)
Modify the state regularization value.
Definition: solver-base.cpp:167
crocoddyl::SolverAbstract::get_th_stop
double get_th_stop() const
Return the tolerance for stopping the algorithm.
Definition: solver-base.cpp:125
crocoddyl::SolverAbstract::get_stop
double get_stop() const
Return the value computed by stoppingCriteria()
Definition: solver-base.cpp:109
crocoddyl::SolverAbstract::get_problem
const boost::shared_ptr< ShootingProblem > & get_problem() const
Return the shooting problem.
Definition: solver-base.cpp:99
crocoddyl::SolverAbstract::set_ureg
void set_ureg(const double ureg)
Modify the control regularization value.
Definition: solver-base.cpp:175
crocoddyl::SolverAbstract::dV_
double dV_
Cost reduction obtained by tryStep()
Definition: solver-base.hpp:271
crocoddyl::SolverAbstract::get_us
const std::vector< Eigen::VectorXd > & get_us() const
Return the control trajectory .
Definition: solver-base.cpp:103
crocoddyl::SolverAbstract::set_us
void set_us(const std::vector< Eigen::VectorXd > &us)
Modify the control trajectory .
Definition: solver-base.cpp:150