crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
solver-base.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2019, LAAS-CNRS
5 // Copyright (C) 2020, University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #include "crocoddyl/core/utils/exception.hpp"
11 #include "crocoddyl/core/solver-base.hpp"
12 
13 namespace crocoddyl {
14 
15 SolverAbstract::SolverAbstract(boost::shared_ptr<ShootingProblem> problem)
16  : problem_(problem),
17  is_feasible_(false),
18  cost_(0.),
19  stop_(0.),
20  xreg_(NAN),
21  ureg_(NAN),
22  steplength_(1.),
23  dV_(0.),
24  dVexp_(0.),
25  th_acceptstep_(0.1),
26  th_stop_(1e-9),
27  iter_(0) {
28  // Allocate common data
29  const std::size_t& T = problem_->get_T();
30  xs_.resize(T + 1);
31  us_.resize(T);
32  for (std::size_t t = 0; t < T; ++t) {
33  const boost::shared_ptr<ActionModelAbstract>& model = problem_->get_runningModels()[t];
34 
35  xs_[t] = model->get_state()->zero();
36  us_[t] = Eigen::VectorXd::Zero(problem_->get_nu_max());
37  }
38  xs_.back() = problem_->get_terminalModel()->get_state()->zero();
39 }
40 
41 SolverAbstract::~SolverAbstract() {}
42 
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();
46 
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();
50  }
51  xs_.back() = problem_->get_terminalModel()->get_state()->zero();
52  } else {
53  if (xs_warm.size() != T + 1) {
54  throw_pretty("Warm start state has wrong dimension, got " << xs_warm.size() << " expecting " << (T + 1));
55  }
56  for (std::size_t t = 0; t < T; ++t) {
57  const std::size_t& nx = problem_->get_runningModels()[t]->get_state()->get_nx();
58  if (static_cast<std::size_t>(xs_warm[t].size()) != nx) {
59  throw_pretty("Invalid argument: "
60  << "xs_init[" + std::to_string(t) + "] has wrong dimension (it should be " + std::to_string(nx) +
61  ")");
62  }
63  }
64  const std::size_t& nx = problem_->get_terminalModel()->get_state()->get_nx();
65  if (static_cast<std::size_t>(xs_warm[T].size()) != nx) {
66  throw_pretty("Invalid argument: "
67  << "xs_init[" + std::to_string(T) + "] has wrong dimension (it should be " + std::to_string(nx) +
68  ")");
69  }
70  std::copy(xs_warm.begin(), xs_warm.end(), xs_.begin());
71  }
72 
73  if (us_warm.size() == 0) {
74  for (std::size_t t = 0; t < T; ++t) {
75  us_[t] = Eigen::VectorXd::Zero(problem_->get_nu_max());
76  }
77  } else {
78  if (us_warm.size() != T) {
79  throw_pretty("Warm start control has wrong dimension, got " << us_warm.size() << " expecting " << T);
80  }
81  const std::size_t& nu = problem_->get_nu_max();
82  for (std::size_t t = 0; t < T; ++t) {
83  if (static_cast<std::size_t>(us_warm[t].size()) > nu) {
84  throw_pretty("Invalid argument: "
85  << "us_init[" + std::to_string(t) + "] has wrong dimension (it should be lower than " + std::to_string(nu) +
86  ")");
87  }
88  }
89  std::copy(us_warm.begin(), us_warm.end(), us_.begin());
90  }
91  is_feasible_ = is_feasible;
92 }
93 
94 void SolverAbstract::setCallbacks(const std::vector<boost::shared_ptr<CallbackAbstract> >& callbacks) {
95  callbacks_ = callbacks;
96 }
97 
98 const std::vector<boost::shared_ptr<CallbackAbstract> >& SolverAbstract::getCallbacks() const { return callbacks_; }
99 
100 const boost::shared_ptr<ShootingProblem>& SolverAbstract::get_problem() const { return problem_; }
101 
102 const std::vector<Eigen::VectorXd>& SolverAbstract::get_xs() const { return xs_; }
103 
104 const std::vector<Eigen::VectorXd>& SolverAbstract::get_us() const { return us_; }
105 
106 const bool& SolverAbstract::get_is_feasible() const { return is_feasible_; }
107 
108 const double& SolverAbstract::get_cost() const { return cost_; }
109 
110 const double& SolverAbstract::get_stop() const { return stop_; }
111 
112 const Eigen::Vector2d& SolverAbstract::get_d() const { return d_; }
113 
114 const double& SolverAbstract::get_xreg() const { return xreg_; }
115 
116 const double& SolverAbstract::get_ureg() const { return ureg_; }
117 
118 const double& SolverAbstract::get_steplength() const { return steplength_; }
119 
120 const double& SolverAbstract::get_dV() const { return dV_; }
121 
122 const double& SolverAbstract::get_dVexp() const { return dVexp_; }
123 
124 const double& SolverAbstract::get_th_acceptstep() const { return th_acceptstep_; }
125 
126 const double& SolverAbstract::get_th_stop() const { return th_stop_; }
127 
128 const std::size_t& SolverAbstract::get_iter() const { return iter_; }
129 
130 void SolverAbstract::set_xs(const std::vector<Eigen::VectorXd>& xs) {
131  const std::size_t& T = problem_->get_T();
132  if (xs.size() != T + 1) {
133  throw_pretty("Invalid argument: "
134  << "xs list has to be " + std::to_string(T + 1));
135  }
136 
137  const std::size_t& nx = problem_->get_nx();
138  for (std::size_t t = 0; t < T; ++t) {
139  if (static_cast<std::size_t>(xs[t].size()) != nx) {
140  throw_pretty("Invalid argument: "
141  << "xs[" + std::to_string(t) + "] has wrong dimension (it should be " + std::to_string(nx) + ")")
142  }
143  }
144  if (static_cast<std::size_t>(xs[T].size()) != nx) {
145  throw_pretty("Invalid argument: "
146  << "xs[" + std::to_string(T) + "] has wrong dimension (it should be " + std::to_string(nx) + ")")
147  }
148  xs_ = xs;
149 }
150 
151 void SolverAbstract::set_us(const std::vector<Eigen::VectorXd>& us) {
152  const std::size_t& T = problem_->get_T();
153  if (us.size() != T) {
154  throw_pretty("Invalid argument: "
155  << "us list has to be " + std::to_string(T));
156  }
157 
158  const std::size_t& nu = problem_->get_nu_max();
159  for (std::size_t t = 0; t < T; ++t) {
160  if (static_cast<std::size_t>(us[t].size()) != nu) {
161  throw_pretty("Invalid argument: "
162  << "us[" + std::to_string(t) + "] has wrong dimension (it should be " + std::to_string(nu) + ")")
163  }
164  }
165  us_ = us;
166 }
167 
168 void SolverAbstract::set_xreg(const double& xreg) {
169  if (xreg < 0.) {
170  throw_pretty("Invalid argument: "
171  << "xreg value has to be positive.");
172  }
173  xreg_ = xreg;
174 }
175 
176 void SolverAbstract::set_ureg(const double& ureg) {
177  if (ureg < 0.) {
178  throw_pretty("Invalid argument: "
179  << "ureg value has to be positive.");
180  }
181  ureg_ = ureg;
182 }
183 
184 void SolverAbstract::set_th_acceptstep(const double& th_acceptstep) {
185  if (0. >= th_acceptstep || th_acceptstep > 1) {
186  throw_pretty("Invalid argument: "
187  << "th_acceptstep value should between 0 and 1.");
188  }
189  th_acceptstep_ = th_acceptstep;
190 }
191 
192 void SolverAbstract::set_th_stop(const double& th_stop) {
193  if (th_stop <= 0.) {
194  throw_pretty("Invalid argument: "
195  << "th_stop value has to higher than 0.");
196  }
197  th_stop_ = th_stop;
198 }
199 
200 bool raiseIfNaN(const double& value) {
201  if (std::isnan(value) || std::isinf(value) || value >= 1e30) {
202  return true;
203  } else {
204  return false;
205  }
206 }
207 
208 } // namespace crocoddyl
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.
Definition: solver-base.cpp:94
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:43
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.
Definition: solver-base.cpp:15
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
Definition: solver-base.cpp:98
double cost_
Total cost.
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.