crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
shooting.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
10#define CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
11
12#include <stdexcept>
13#include <vector>
14#include "crocoddyl/core/fwd.hpp"
15#include "crocoddyl/core/utils/exception.hpp"
16#include "crocoddyl/core/action-base.hpp"
17#include "crocoddyl/core/utils/deprecate.hpp"
18
19namespace crocoddyl {
20
30template <typename _Scalar>
32 public:
33 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34
35 typedef _Scalar Scalar;
39 typedef typename MathBase::VectorXs VectorXs;
40
48 ShootingProblemTpl(const VectorXs& x0, const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
49 boost::shared_ptr<ActionModelAbstract> terminal_model);
50
60 ShootingProblemTpl(const VectorXs& x0, const std::vector<boost::shared_ptr<ActionModelAbstract> >& running_models,
61 boost::shared_ptr<ActionModelAbstract> terminal_model,
62 const std::vector<boost::shared_ptr<ActionDataAbstract> >& running_datas,
63 boost::shared_ptr<ActionDataAbstract> terminal_data);
69
80 Scalar calc(const std::vector<VectorXs>& xs, const std::vector<VectorXs>& us);
81
94 Scalar calcDiff(const std::vector<VectorXs>& xs, const std::vector<VectorXs>& us);
95
102 void rollout(const std::vector<VectorXs>& us, std::vector<VectorXs>& xs);
103
110 std::vector<VectorXs> rollout_us(const std::vector<VectorXs>& us);
111
118 void quasiStatic(std::vector<VectorXs>& us, const std::vector<VectorXs>& xs);
119
126 std::vector<VectorXs> quasiStatic_xs(const std::vector<VectorXs>& xs);
127
136 void circularAppend(boost::shared_ptr<ActionModelAbstract> model, boost::shared_ptr<ActionDataAbstract> data);
137
146 void circularAppend(boost::shared_ptr<ActionModelAbstract> model);
147
155 void updateNode(const std::size_t i, boost::shared_ptr<ActionModelAbstract> model,
156 boost::shared_ptr<ActionDataAbstract> data);
157
164 void updateModel(const std::size_t i, boost::shared_ptr<ActionModelAbstract> model);
165
169 std::size_t get_T() const;
170
174 const VectorXs& get_x0() const;
175
179 const std::vector<boost::shared_ptr<ActionModelAbstract> >& get_runningModels() const;
180
184 const boost::shared_ptr<ActionModelAbstract>& get_terminalModel() const;
185
189 const std::vector<boost::shared_ptr<ActionDataAbstract> >& get_runningDatas() const;
190
194 const boost::shared_ptr<ActionDataAbstract>& get_terminalData() const;
195
199 void set_x0(const VectorXs& x0_in);
200
204 void set_runningModels(const std::vector<boost::shared_ptr<ActionModelAbstract> >& models);
205
209 void set_terminalModel(boost::shared_ptr<ActionModelAbstract> model);
210
216 void set_nthreads(const int nthreads);
217
221 std::size_t get_nx() const;
222
226 std::size_t get_ndx() const;
227
231 DEPRECATED("Compute yourself the maximum dimension of the control vector", std::size_t get_nu_max() const;)
232
236 std::size_t get_nthreads() const;
237
242
246 template <class Scalar>
247 friend std::ostream& operator<<(std::ostream& os, const ShootingProblemTpl<Scalar>& problem);
248
249 protected:
250 Scalar cost_;
251 std::size_t T_;
252 VectorXs x0_;
253 boost::shared_ptr<ActionModelAbstract> terminal_model_;
254 boost::shared_ptr<ActionDataAbstract> terminal_data_;
255 std::vector<boost::shared_ptr<ActionModelAbstract> > running_models_;
256 std::vector<boost::shared_ptr<ActionDataAbstract> > running_datas_;
257 std::size_t nx_;
258 std::size_t ndx_;
259 std::size_t nu_max_;
260 std::size_t nthreads_;
261 bool is_updated_;
262
263 private:
264 void allocateData();
265};
266
267} // namespace crocoddyl
268
269/* --- Details -------------------------------------------------------------- */
270/* --- Details -------------------------------------------------------------- */
271/* --- Details -------------------------------------------------------------- */
272#include "crocoddyl/core/optctrl/shooting.hxx"
273
274#endif // CROCODDYL_CORE_OPTCTRL_SHOOTING_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
This class encapsulates a shooting problem.
Definition: shooting.hpp:31
boost::shared_ptr< ActionDataAbstract > terminal_data_
Terminal action data.
Definition: shooting.hpp:254
void circularAppend(boost::shared_ptr< ActionModelAbstract > model, boost::shared_ptr< ActionDataAbstract > data)
Circular append of the model and data onto the end running node.
Scalar calcDiff(const std::vector< VectorXs > &xs, const std::vector< VectorXs > &us)
Compute the derivatives of the cost and dynamics.
void quasiStatic(std::vector< VectorXs > &us, const std::vector< VectorXs > &xs)
Compute the quasic static commands given a state trajectory.
const boost::shared_ptr< ActionDataAbstract > & get_terminalData() const
Return the terminal data.
void rollout(const std::vector< VectorXs > &us, std::vector< VectorXs > &xs)
Integrate the dynamics given a control sequence.
friend std::ostream & operator<<(std::ostream &os, const ShootingProblemTpl< Scalar > &problem)
Print information on the 'ShootingProblem'.
std::size_t get_ndx() const
Return the dimension of the tangent space of the state manifold.
ShootingProblemTpl(const VectorXs &x0, const std::vector< boost::shared_ptr< ActionModelAbstract > > &running_models, boost::shared_ptr< ActionModelAbstract > terminal_model, const std::vector< boost::shared_ptr< ActionDataAbstract > > &running_datas, boost::shared_ptr< ActionDataAbstract > terminal_data)
Initialize the shooting problem (models and datas)
std::size_t nthreads_
Number of threads launch by the multi-threading application.
Definition: shooting.hpp:260
const VectorXs & get_x0() const
Return the initial state.
ShootingProblemTpl(const VectorXs &x0, const std::vector< boost::shared_ptr< ActionModelAbstract > > &running_models, boost::shared_ptr< ActionModelAbstract > terminal_model)
Initialize the shooting problem and allocate its data.
std::size_t nx_
State dimension.
Definition: shooting.hpp:257
std::size_t get_nthreads() const
Return the maximum dimension of the control vector.
std::vector< VectorXs > quasiStatic_xs(const std::vector< VectorXs > &xs)
Compute the quasic static commands given a state trajectory.
Scalar calc(const std::vector< VectorXs > &xs, const std::vector< VectorXs > &us)
Compute the cost and the next states.
void set_terminalModel(boost::shared_ptr< ActionModelAbstract > model)
Modify the terminal model and allocate new data.
const boost::shared_ptr< ActionModelAbstract > & get_terminalModel() const
Return the terminal model.
void updateNode(const std::size_t i, boost::shared_ptr< ActionModelAbstract > model, boost::shared_ptr< ActionDataAbstract > data)
Update the model and data for a specific node.
ShootingProblemTpl(const ShootingProblemTpl< Scalar > &problem)
Initialize the shooting problem.
bool is_updated()
Return only once true is the shooting problem has been changed, otherwise false.
std::size_t T_
number of running nodes
Definition: shooting.hpp:251
std::vector< boost::shared_ptr< ActionModelAbstract > > running_models_
Running action model.
Definition: shooting.hpp:255
void updateModel(const std::size_t i, boost::shared_ptr< ActionModelAbstract > model)
Update a model and allocated new data for a specific node.
void set_x0(const VectorXs &x0_in)
Modify the initial state.
const std::vector< boost::shared_ptr< ActionDataAbstract > > & get_runningDatas() const
Return the running datas.
void set_runningModels(const std::vector< boost::shared_ptr< ActionModelAbstract > > &models)
Modify the running models and allocate new data.
Scalar cost_
Total cost.
Definition: shooting.hpp:250
std::vector< boost::shared_ptr< ActionDataAbstract > > running_datas_
Running action data.
Definition: shooting.hpp:256
void set_nthreads(const int nthreads)
Modify the number of threads using with multithreading support.
boost::shared_ptr< ActionModelAbstract > terminal_model_
Terminal action model.
Definition: shooting.hpp:253
std::size_t get_T() const
Return the number of running nodes.
const std::vector< boost::shared_ptr< ActionModelAbstract > > & get_runningModels() const
Return the running models.
void circularAppend(boost::shared_ptr< ActionModelAbstract > model)
Circular append of the model and data onto the end running node.
std::size_t get_nx() const
Return the dimension of the state tuple.
VectorXs x0_
Initial state.
Definition: shooting.hpp:252
std::size_t nu_max_
Maximum control dimension.
Definition: shooting.hpp:259
std::vector< VectorXs > rollout_us(const std::vector< VectorXs > &us)
Integrate the dynamics given a control sequence.
std::size_t ndx_
State rate dimension.
Definition: shooting.hpp:258