crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
solver-base.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh, University of Oxford
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifdef CROCODDYL_WITH_MULTITHREADING
10 #include <omp.h>
11 #endif // CROCODDYL_WITH_MULTITHREADING
12 
13 #include "crocoddyl/core/utils/exception.hpp"
14 #include "crocoddyl/core/solver-base.hpp"
15 
16 namespace crocoddyl {
17 
18 SolverAbstract::SolverAbstract(boost::shared_ptr<ShootingProblem> problem)
19  : problem_(problem),
20  is_feasible_(false),
21  was_feasible_(false),
22  cost_(0.),
23  stop_(0.),
24  xreg_(NAN),
25  ureg_(NAN),
26  steplength_(1.),
27  dV_(0.),
28  dVexp_(0.),
29  th_acceptstep_(0.1),
30  th_stop_(1e-9),
31  iter_(0),
32  th_gaptol_(1e-16),
33  ffeas_(NAN),
34  inffeas_(true),
35  tmp_feas_(0.) {
36  // Allocate common data
37  const std::size_t ndx = problem_->get_ndx();
38  const std::size_t T = problem_->get_T();
39  xs_.resize(T + 1);
40  us_.resize(T);
41  fs_.resize(T + 1);
42  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
43  for (std::size_t t = 0; t < T; ++t) {
44  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
45  const std::size_t nu = model->get_nu();
46  xs_[t] = model->get_state()->zero();
47  us_[t] = Eigen::VectorXd::Zero(nu);
48  fs_[t] = Eigen::VectorXd::Zero(ndx);
49  }
50  xs_.back() = problem_->get_terminalModel()->get_state()->zero();
51  fs_.back() = Eigen::VectorXd::Zero(ndx);
52 }
53 
54 SolverAbstract::~SolverAbstract() {}
55 
57  const std::size_t T = problem_->get_T();
58  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
59  for (std::size_t t = 0; t < T; ++t) {
60  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
61  const std::size_t nu = model->get_nu();
62  us_[t].conservativeResize(nu);
63  }
64 }
65 
67  tmp_feas_ = 0.;
68  if (!is_feasible_) {
69  const std::size_t T = problem_->get_T();
70  const Eigen::VectorXd& x0 = problem_->get_x0();
71  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
72  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
73 
74  models[0]->get_state()->diff(xs_[0], x0, fs_[0]);
75 #ifdef CROCODDYL_WITH_MULTITHREADING
76 #pragma omp parallel for num_threads(problem_->get_nthreads())
77 #endif
78  for (std::size_t t = 0; t < T; ++t) {
79  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
80  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
81  m->get_state()->diff(xs_[t + 1], d->xnext, fs_[t + 1]);
82  }
83 
84  if (inffeas_) {
85  tmp_feas_ = std::max(tmp_feas_, fs_[0].lpNorm<Eigen::Infinity>());
86  for (std::size_t t = 0; t < T; ++t) {
87  tmp_feas_ = std::max(tmp_feas_, fs_[t + 1].lpNorm<Eigen::Infinity>());
88  }
89  } else {
90  tmp_feas_ = fs_[0].lpNorm<1>();
91  for (std::size_t t = 0; t < T; ++t) {
92  tmp_feas_ += fs_[t + 1].lpNorm<1>();
93  }
94  }
95  } else if (!was_feasible_) { // closing the gaps
96  for (std::vector<Eigen::VectorXd>::iterator it = fs_.begin(); it != fs_.end(); ++it) {
97  it->setZero();
98  }
99  }
100  return tmp_feas_;
101 }
102 
103 void SolverAbstract::setCandidate(const std::vector<Eigen::VectorXd>& xs_warm,
104  const std::vector<Eigen::VectorXd>& us_warm, bool is_feasible) {
105  const std::size_t T = problem_->get_T();
106 
107  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
108  if (xs_warm.size() == 0) {
109  for (std::size_t t = 0; t < T; ++t) {
110  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
111  xs_[t] = model->get_state()->zero();
112  }
113  xs_.back() = problem_->get_terminalModel()->get_state()->zero();
114  } else {
115  if (xs_warm.size() != T + 1) {
116  throw_pretty("Warm start state vector has wrong dimension, got " << xs_warm.size() << " expecting " << (T + 1));
117  }
118  for (std::size_t t = 0; t < T; ++t) {
119  const std::size_t nx = models[t]->get_state()->get_nx();
120  if (static_cast<std::size_t>(xs_warm[t].size()) != nx) {
121  throw_pretty("Invalid argument: "
122  << "xs_init[" + std::to_string(t) + "] has wrong dimension (" << xs_warm[t].size()
123  << " provided - it should be equal to " + std::to_string(nx) + "). ActionModel: " << *models[t]);
124  }
125  }
126  const std::size_t nx = problem_->get_terminalModel()->get_state()->get_nx();
127  if (static_cast<std::size_t>(xs_warm[T].size()) != nx) {
128  throw_pretty("Invalid argument: "
129  << "xs_init[" + std::to_string(T) + "] (terminal state) has wrong dimension (" << xs_warm[T].size()
130  << " provided - it should be equal to " + std::to_string(nx) + "). ActionModel: "
131  << *problem_->get_terminalModel());
132  }
133  std::copy(xs_warm.begin(), xs_warm.end(), xs_.begin());
134  }
135 
136  if (us_warm.size() == 0) {
137  for (std::size_t t = 0; t < T; ++t) {
138  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
139  const std::size_t nu = model->get_nu();
140  us_[t] = Eigen::VectorXd::Zero(nu);
141  }
142  } else {
143  if (us_warm.size() != T) {
144  throw_pretty("Warm start control has wrong dimension, got " << us_warm.size() << " expecting " << T);
145  }
146  for (std::size_t t = 0; t < T; ++t) {
147  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
148  const std::size_t nu = model->get_nu();
149  if (static_cast<std::size_t>(us_warm[t].size()) != nu) {
150  throw_pretty("Invalid argument: "
151  << "us_init[" + std::to_string(t) + "] has wrong dimension (" << us_warm[t].size()
152  << " provided - it should be equal to " + std::to_string(nu) + "). ActionModel: " << *model);
153  }
154  }
155  std::copy(us_warm.begin(), us_warm.end(), us_.begin());
156  }
157  is_feasible_ = is_feasible;
158 }
159 
160 void SolverAbstract::setCallbacks(const std::vector<boost::shared_ptr<CallbackAbstract> >& callbacks) {
161  callbacks_ = callbacks;
162 }
163 
164 const std::vector<boost::shared_ptr<CallbackAbstract> >& SolverAbstract::getCallbacks() const { return callbacks_; }
165 
166 const boost::shared_ptr<ShootingProblem>& SolverAbstract::get_problem() const { return problem_; }
167 
168 const std::vector<Eigen::VectorXd>& SolverAbstract::get_xs() const { return xs_; }
169 
170 const std::vector<Eigen::VectorXd>& SolverAbstract::get_us() const { return us_; }
171 
172 const std::vector<Eigen::VectorXd>& SolverAbstract::get_fs() const { return fs_; }
173 
175 
176 double SolverAbstract::get_cost() const { return cost_; }
177 
178 double SolverAbstract::get_stop() const { return stop_; }
179 
180 const Eigen::Vector2d& SolverAbstract::get_d() const { return d_; }
181 
182 double SolverAbstract::get_xreg() const { return xreg_; }
183 
184 double SolverAbstract::get_ureg() const { return ureg_; }
185 
186 double SolverAbstract::get_steplength() const { return steplength_; }
187 
188 double SolverAbstract::get_dV() const { return dV_; }
189 
190 double SolverAbstract::get_dVexp() const { return dVexp_; }
191 
193 
194 double SolverAbstract::get_th_stop() const { return th_stop_; }
195 
196 std::size_t SolverAbstract::get_iter() const { return iter_; }
197 
198 double SolverAbstract::get_th_gaptol() const { return th_gaptol_; }
199 
200 double SolverAbstract::get_ffeas() const { return ffeas_; }
201 
202 bool SolverAbstract::get_inffeas() const { return inffeas_; }
203 
204 void SolverAbstract::set_xs(const std::vector<Eigen::VectorXd>& xs) {
205  const std::size_t T = problem_->get_T();
206  if (xs.size() != T + 1) {
207  throw_pretty("Invalid argument: "
208  << "xs list has to be of length " + std::to_string(T + 1));
209  }
210 
211  const std::size_t nx = problem_->get_nx();
212  for (std::size_t t = 0; t < T; ++t) {
213  if (static_cast<std::size_t>(xs[t].size()) != nx) {
214  throw_pretty("Invalid argument: "
215  << "xs[" + std::to_string(t) + "] has wrong dimension (" << xs[t].size()
216  << " provided - it should be " + std::to_string(nx) + ")")
217  }
218  }
219  if (static_cast<std::size_t>(xs[T].size()) != nx) {
220  throw_pretty("Invalid argument: "
221  << "xs[" + std::to_string(T) + "] (terminal state) has wrong dimension (" << xs[T].size()
222  << " provided - it should be " + std::to_string(nx) + ")")
223  }
224  xs_ = xs;
225 }
226 
227 void SolverAbstract::set_us(const std::vector<Eigen::VectorXd>& us) {
228  const std::size_t T = problem_->get_T();
229  if (us.size() != T) {
230  throw_pretty("Invalid argument: "
231  << "us list has to be of length " + std::to_string(T));
232  }
233 
234  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
235  for (std::size_t t = 0; t < T; ++t) {
236  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
237  const std::size_t nu = model->get_nu();
238  if (static_cast<std::size_t>(us[t].size()) != nu) {
239  throw_pretty("Invalid argument: "
240  << "us[" + std::to_string(t) + "] has wrong dimension (" << us[t].size()
241  << " provided - it should be " + std::to_string(nu) + ")")
242  }
243  }
244  us_ = us;
245 }
246 
247 void SolverAbstract::set_xreg(const double xreg) {
248  if (xreg < 0.) {
249  throw_pretty("Invalid argument: "
250  << "xreg value has to be positive.");
251  }
252  xreg_ = xreg;
253 }
254 
255 void SolverAbstract::set_ureg(const double ureg) {
256  if (ureg < 0.) {
257  throw_pretty("Invalid argument: "
258  << "ureg value has to be positive.");
259  }
260  ureg_ = ureg;
261 }
262 
263 void SolverAbstract::set_th_acceptstep(const double th_acceptstep) {
264  if (0. >= th_acceptstep || th_acceptstep > 1) {
265  throw_pretty("Invalid argument: "
266  << "th_acceptstep value should between 0 and 1.");
267  }
268  th_acceptstep_ = th_acceptstep;
269 }
270 
271 void SolverAbstract::set_th_stop(const double th_stop) {
272  if (th_stop <= 0.) {
273  throw_pretty("Invalid argument: "
274  << "th_stop value has to higher than 0.");
275  }
276  th_stop_ = th_stop;
277 }
278 
279 void SolverAbstract::set_th_gaptol(const double th_gaptol) {
280  if (0. > th_gaptol) {
281  throw_pretty("Invalid argument: "
282  << "th_gaptol value has to be positive.");
283  }
284  th_gaptol_ = th_gaptol;
285 }
286 
287 void SolverAbstract::set_inffeas(const bool inffeas) { inffeas_ = inffeas; }
288 
289 bool raiseIfNaN(const double value) {
290  if (std::isnan(value) || std::isinf(value) || value >= 1e30) {
291  return true;
292  } else {
293  return false;
294  }
295 }
296 
297 } // namespace crocoddyl
bool is_feasible_
Label that indicates is the iteration is feasible.
void set_ureg(const double ureg)
Modify the control regularization value.
virtual void resizeData()
Resizing the solver data.
Definition: solver-base.cpp:56
double get_th_acceptstep() const
Return the threshold used for accepting a step.
const std::vector< Eigen::VectorXd > & get_us() const
Return the control trajectory .
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.
std::size_t get_iter() const
Return the number of iterations performed by the solver.
double get_stop() const
Return the value computed by stoppingCriteria()
double get_ffeas() const
Return the feasibility of the dynamic constraints of the current guess.
void set_th_acceptstep(const double th_acceptstep)
Modify the threshold used for accepting step.
double get_th_stop() const
Return the tolerance for stopping the algorithm.
double get_dVexp() const
Return the expected cost reduction .
double get_xreg() const
Return the state regularization value.
void setCallbacks(const std::vector< boost::shared_ptr< CallbackAbstract > > &callbacks)
Set a list of callback functions using for the solver diagnostic.
double th_gaptol_
Threshold limit to check non-zero gaps.
bool was_feasible_
Label that indicates in the previous iterate was feasible.
double computeDynamicFeasibility()
Compute the dynamic feasibility for the current guess .
Definition: solver-base.cpp:66
double xreg_
Current state regularization value.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
double get_cost() const
Return the total cost.
double th_acceptstep_
Threshold used for accepting step.
std::vector< Eigen::VectorXd > us_
Control trajectory.
double dV_
Cost reduction obtained by tryStep()
const std::vector< Eigen::VectorXd > & get_fs() const
Return the gaps .
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverAbstract(boost::shared_ptr< ShootingProblem > problem)
Initialize the solver.
Definition: solver-base.cpp:18
Eigen::Vector2d d_
LQ approximation of the expected improvement.
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
double dVexp_
Expected cost reduction.
void set_th_stop(const double th_stop)
Modify the tolerance for stopping the algorithm.
void set_th_gaptol(const double th_gaptol)
Modify the threshold for accepting a gap as non-zero.
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 trajectories .
void set_inffeas(const bool inffeas)
Modify the current norm used for computed the feasibility.
std::vector< Eigen::VectorXd > xs_
State trajectory.
bool get_inffeas() const
Return the norm used for the computing the feasibility (true for , false for )
const std::vector< boost::shared_ptr< CallbackAbstract > > & getCallbacks() const
Return the list of callback functions using for diagnostic.
double cost_
Total cost.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
bool get_is_feasible() const
Return the feasibility status of the trajectory.
double th_stop_
Tolerance for stopping the algorithm.
double get_dV() const
Return the cost reduction .
void set_xs(const std::vector< Eigen::VectorXd > &xs)
Modify the state trajectory .
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.
double ffeas_
Feasibility of the dynamic constraints.
double get_th_gaptol() const
Return the threshold for accepting a gap as non-zero.
std::size_t iter_
Number of iteration performed by the solver.
double tmp_feas_
Temporal variables used for computed the feasibility.
double get_ureg() const
Return the control regularization value.
double stop_
Value computed by stoppingCriteria()
void set_xreg(const double xreg)
Modify the state regularization value.
double get_steplength() const
Return the step length .