shooting.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifdef WITH_MULTITHREADING
10 #include <omp.h>
11 #define NUM_THREADS WITH_NTHREADS
12 #endif // WITH_MULTITHREADING
13 
14 namespace crocoddyl {
15 
16 template <typename Scalar>
17 ShootingProblemTpl<Scalar>::ShootingProblemTpl(
18  const VectorXs& x0, const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
19  boost::shared_ptr<ActionModelAbstract> terminal_model)
20  : cost_(0.), T_(running_models.size()), x0_(x0), terminal_model_(terminal_model), running_models_(running_models) {
21  if (static_cast<std::size_t>(x0.size()) != running_models_[0]->get_state()->get_nx()) {
22  throw_pretty("Invalid argument: "
23  << "x0 has wrong dimension (it should be " +
24  std::to_string(running_models_[0]->get_state()->get_nx()) + ")");
25  }
26  allocateData();
27 }
28 
29 template <typename Scalar>
30 ShootingProblemTpl<Scalar>::~ShootingProblemTpl() {}
31 
32 template <typename Scalar>
33 Scalar ShootingProblemTpl<Scalar>::calc(const std::vector<VectorXs>& xs, const std::vector<VectorXs>& us) {
34  if (xs.size() != T_ + 1) {
35  throw_pretty("Invalid argument: "
36  << "xs has wrong dimension (it should be " + std::to_string(T_ + 1) + ")");
37  }
38  if (us.size() != T_) {
39  throw_pretty("Invalid argument: "
40  << "us has wrong dimension (it should be " + std::to_string(T_) + ")");
41  }
42 
43 #ifdef WITH_MULTITHREADING
44 #pragma omp parallel for
45 #endif
46  for (std::size_t i = 0; i < T_; ++i) {
47  running_models_[i]->calc(running_datas_[i], xs[i], us[i]);
48  }
49  terminal_model_->calc(terminal_data_, xs.back());
50 
51  cost_ = 0;
52  for (std::size_t i = 0; i < T_; ++i) {
53  cost_ += running_datas_[i]->cost;
54  }
55  cost_ += terminal_data_->cost;
56  return cost_;
57 }
58 
59 template <typename Scalar>
60 Scalar ShootingProblemTpl<Scalar>::calcDiff(const std::vector<VectorXs>& xs, const std::vector<VectorXs>& us) {
61  if (xs.size() != T_ + 1) {
62  throw_pretty("Invalid argument: "
63  << "xs has wrong dimension (it should be " + std::to_string(T_ + 1) + ")");
64  }
65  if (us.size() != T_) {
66  throw_pretty("Invalid argument: "
67  << "us has wrong dimension (it should be " + std::to_string(T_) + ")");
68  }
69 
70  std::size_t i;
71 
72 #ifdef WITH_MULTITHREADING
73 #pragma omp parallel for
74 #endif
75  for (i = 0; i < T_; ++i) {
76  running_models_[i]->calcDiff(running_datas_[i], xs[i], us[i]);
77  }
78  terminal_model_->calcDiff(terminal_data_, xs.back());
79 
80  cost_ = 0;
81  for (std::size_t i = 0; i < T_; ++i) {
82  cost_ += running_datas_[i]->cost;
83  }
84  cost_ += terminal_data_->cost;
85 
86  return cost_;
87 }
88 
89 template <typename Scalar>
90 void ShootingProblemTpl<Scalar>::rollout(const std::vector<VectorXs>& us, std::vector<VectorXs>& xs) {
91  if (xs.size() != T_ + 1) {
92  throw_pretty("Invalid argument: "
93  << "xs has wrong dimension (it should be " + std::to_string(T_ + 1) + ")");
94  }
95  if (us.size() != T_) {
96  throw_pretty("Invalid argument: "
97  << "us has wrong dimension (it should be " + std::to_string(T_) + ")");
98  }
99 
100  xs[0] = x0_;
101  for (std::size_t i = 0; i < T_; ++i) {
102  const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
103  const boost::shared_ptr<ActionDataAbstract>& data = running_datas_[i];
104  const VectorXs& x = xs[i];
105  const VectorXs& u = us[i];
106 
107  model->calc(data, x, u);
108  xs[i + 1] = data->xnext;
109  }
110  terminal_model_->calc(terminal_data_, xs.back());
111 }
112 
113 template <typename Scalar>
114 std::vector<typename MathBaseTpl<Scalar>::VectorXs> ShootingProblemTpl<Scalar>::rollout_us(
115  const std::vector<VectorXs>& us) {
116  std::vector<VectorXs> xs;
117  xs.resize(T_ + 1);
118  rollout(us, xs);
119  return xs;
120 }
121 
122 template <typename Scalar>
123 void ShootingProblemTpl<Scalar>::updateModel(std::size_t i, boost::shared_ptr<ActionModelAbstract> model) {
124  if (i > T_ + 1) {
125  throw_pretty("Invalid argument: "
126  << "i is bigger than the allocated horizon (it should be lower than " + std::to_string(T_) + ")");
127  }
128  if (i == T_ + 1) {
129  set_terminalModel(model);
130  } else {
131  running_models_[i] = model;
132  running_datas_[i] = model->createData();
133  }
134 }
135 
136 template <typename Scalar>
137 const std::size_t& ShootingProblemTpl<Scalar>::get_T() const {
138  return T_;
139 }
140 
141 template <typename Scalar>
142 const typename MathBaseTpl<Scalar>::VectorXs& ShootingProblemTpl<Scalar>::get_x0() const {
143  return x0_;
144 }
145 
146 template <typename Scalar>
147 void ShootingProblemTpl<Scalar>::allocateData() {
148  for (std::size_t i = 0; i < T_; ++i) {
149  const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
150  running_datas_.push_back(model->createData());
151  }
152  terminal_data_ = terminal_model_->createData();
153 }
154 
155 template <typename Scalar>
156 const std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> > >&
157 ShootingProblemTpl<Scalar>::get_runningModels() const {
158  return running_models_;
159 }
160 
161 template <typename Scalar>
162 const boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> >& ShootingProblemTpl<Scalar>::get_terminalModel()
163  const {
164  return terminal_model_;
165 }
166 
167 template <typename Scalar>
168 const std::vector<boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> > >&
169 ShootingProblemTpl<Scalar>::get_runningDatas() const {
170  return running_datas_;
171 }
172 
173 template <typename Scalar>
174 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& ShootingProblemTpl<Scalar>::get_terminalData()
175  const {
176  return terminal_data_;
177 }
178 
179 template <typename Scalar>
180 void ShootingProblemTpl<Scalar>::set_x0(const VectorXs& x0_in) {
181  if (x0_in.size() != x0_.size()) {
182  throw_pretty("Invalid argument: "
183  << "invalid size of x0 provided.");
184  }
185  x0_ = x0_in;
186 }
187 
188 template <typename Scalar>
189 void ShootingProblemTpl<Scalar>::set_runningModels(
190  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models) {
191  T_ = models.size();
192  running_models_ = models;
193  running_datas_.clear();
194  for (std::size_t i = 0; i < T_; ++i) {
195  const boost::shared_ptr<ActionModelAbstract>& model = running_models_[i];
196  running_datas_.push_back(model->createData());
197  }
198 }
199 
200 template <typename Scalar>
201 void ShootingProblemTpl<Scalar>::set_terminalModel(boost::shared_ptr<ActionModelAbstract> model) {
202  terminal_model_ = model;
203  terminal_data_ = terminal_model_->createData();
204 }
205 
206 } // namespace crocoddyl
Definition: action-base.hxx:11