crocoddyl  1.3.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 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  const std::size_t& nu = model->get_nu();
34 
35  xs_[t] = model->get_state()->zero();
36  us_[t] = Eigen::VectorXd::Zero(nu);
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  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());
56  }
57 
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);
62  }
63  } else {
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());
67  }
68  is_feasible_ = is_feasible;
69 }
70 
71 void SolverAbstract::setCallbacks(const std::vector<boost::shared_ptr<CallbackAbstract> >& callbacks) {
72  callbacks_ = callbacks;
73 }
74 
75 const std::vector<boost::shared_ptr<CallbackAbstract> >& SolverAbstract::getCallbacks() const { return callbacks_; }
76 
77 const boost::shared_ptr<ShootingProblem>& SolverAbstract::get_problem() const { return problem_; }
78 
79 const std::vector<Eigen::VectorXd>& SolverAbstract::get_xs() const { return xs_; }
80 
81 const std::vector<Eigen::VectorXd>& SolverAbstract::get_us() const { return us_; }
82 
83 const bool& SolverAbstract::get_is_feasible() const { return is_feasible_; }
84 
85 const double& SolverAbstract::get_cost() const { return cost_; }
86 
87 const double& SolverAbstract::get_stop() const { return stop_; }
88 
89 const Eigen::Vector2d& SolverAbstract::get_d() const { return d_; }
90 
91 const double& SolverAbstract::get_xreg() const { return xreg_; }
92 
93 const double& SolverAbstract::get_ureg() const { return ureg_; }
94 
95 const double& SolverAbstract::get_steplength() const { return steplength_; }
96 
97 const double& SolverAbstract::get_dV() const { return dV_; }
98 
99 const double& SolverAbstract::get_dVexp() const { return dVexp_; }
100 
101 const double& SolverAbstract::get_th_acceptstep() const { return th_acceptstep_; }
102 
103 const double& SolverAbstract::get_th_stop() const { return th_stop_; }
104 
105 const std::size_t& SolverAbstract::get_iter() const { return iter_; }
106 
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));
112  }
113 
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) + ")")
120  }
121  }
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) + ")")
127  }
128  xs_ = xs;
129 }
130 
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));
136  }
137 
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) + ")")
144  }
145  }
146  us_ = us;
147 }
148 
149 void SolverAbstract::set_xreg(const double& xreg) {
150  if (xreg < 0.) {
151  throw_pretty("Invalid argument: "
152  << "xreg value has to be positive.");
153  }
154  xreg_ = xreg;
155 }
156 
157 void SolverAbstract::set_ureg(const double& ureg) {
158  if (ureg < 0.) {
159  throw_pretty("Invalid argument: "
160  << "ureg value has to be positive.");
161  }
162  ureg_ = ureg;
163 }
164 
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.");
169  }
170  th_acceptstep_ = th_acceptstep;
171 }
172 
173 void SolverAbstract::set_th_stop(const double& th_stop) {
174  if (th_stop <= 0.) {
175  throw_pretty("Invalid argument: "
176  << "th_stop value has to higher than 0.");
177  }
178  th_stop_ = th_stop;
179 }
180 
181 bool raiseIfNaN(const double& value) {
182  if (std::isnan(value) || std::isinf(value) || value >= 1e30) {
183  return true;
184  } else {
185  return false;
186  }
187 }
188 
189 } // namespace crocoddyl