9 #ifdef WITH_MULTITHREADING 11 #define NUM_THREADS WITH_NTHREADS 12 #endif // WITH_MULTITHREADING 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()) +
")");
29 template <
typename Scalar>
30 ShootingProblemTpl<Scalar>::~ShootingProblemTpl() {}
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) +
")");
38 if (us.size() != T_) {
39 throw_pretty(
"Invalid argument: " 40 <<
"us has wrong dimension (it should be " + std::to_string(T_) +
")");
43 #ifdef WITH_MULTITHREADING 44 #pragma omp parallel for 46 for (std::size_t i = 0; i < T_; ++i) {
47 running_models_[i]->calc(running_datas_[i], xs[i], us[i]);
49 terminal_model_->calc(terminal_data_, xs.back());
52 for (std::size_t i = 0; i < T_; ++i) {
53 cost_ += running_datas_[i]->cost;
55 cost_ += terminal_data_->cost;
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) +
")");
65 if (us.size() != T_) {
66 throw_pretty(
"Invalid argument: " 67 <<
"us has wrong dimension (it should be " + std::to_string(T_) +
")");
72 #ifdef WITH_MULTITHREADING 73 #pragma omp parallel for 75 for (i = 0; i < T_; ++i) {
76 running_models_[i]->calcDiff(running_datas_[i], xs[i], us[i]);
78 terminal_model_->calcDiff(terminal_data_, xs.back());
81 for (std::size_t i = 0; i < T_; ++i) {
82 cost_ += running_datas_[i]->cost;
84 cost_ += terminal_data_->cost;
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) +
")");
95 if (us.size() != T_) {
96 throw_pretty(
"Invalid argument: " 97 <<
"us has wrong dimension (it should be " + std::to_string(T_) +
")");
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];
107 model->calc(data, x, u);
108 xs[i + 1] = data->xnext;
110 terminal_model_->calc(terminal_data_, xs.back());
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;
122 template <
typename Scalar>
123 void ShootingProblemTpl<Scalar>::updateModel(std::size_t i, boost::shared_ptr<ActionModelAbstract> model) {
125 throw_pretty(
"Invalid argument: " 126 <<
"i is bigger than the allocated horizon (it should be lower than " + std::to_string(T_) +
")");
129 set_terminalModel(model);
131 running_models_[i] = model;
132 running_datas_[i] = model->createData();
136 template <
typename Scalar>
137 const std::size_t& ShootingProblemTpl<Scalar>::get_T()
const {
141 template <
typename Scalar>
142 const typename MathBaseTpl<Scalar>::VectorXs& ShootingProblemTpl<Scalar>::get_x0()
const {
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());
152 terminal_data_ = terminal_model_->createData();
155 template <
typename Scalar>
156 const std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> > >&
157 ShootingProblemTpl<Scalar>::get_runningModels()
const {
158 return running_models_;
161 template <
typename Scalar>
162 const boost::shared_ptr<crocoddyl::ActionModelAbstractTpl<Scalar> >& ShootingProblemTpl<Scalar>::get_terminalModel()
164 return terminal_model_;
167 template <
typename Scalar>
168 const std::vector<boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> > >&
169 ShootingProblemTpl<Scalar>::get_runningDatas()
const {
170 return running_datas_;
173 template <
typename Scalar>
174 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& ShootingProblemTpl<Scalar>::get_terminalData()
176 return terminal_data_;
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.");
188 template <
typename Scalar>
189 void ShootingProblemTpl<Scalar>::set_runningModels(
190 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models) {
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());
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();
Definition: action-base.hxx:11