aligator  0.6.1
A primal-dual augmented Lagrangian-type solver for nonlinear trajectory optimization.
Loading...
Searching...
No Matches
rollout.hpp
Go to the documentation of this file.
1#pragma once
2
3#include "aligator/utils/forward-dyn.hpp"
4
5namespace aligator {
6
8template <typename Scalar>
9typename math_types<Scalar>::VectorOfVectors
10rollout(const std::vector<shared_ptr<DynamicsModelTpl<Scalar>>> &dyn_models,
11 const typename math_types<Scalar>::VectorXs &x0,
12 const typename math_types<Scalar>::VectorOfVectors &us,
13 typename math_types<Scalar>::VectorOfVectors &xout) {
14 using Data = DynamicsDataTpl<Scalar>;
15 const std::size_t N = us.size();
16 if (dyn_models.size() != N) {
17 ALIGATOR_RUNTIME_ERROR(
18 "Number of controls should be the same as number of dynamical models!");
19 }
20 xout.resize(N + 1);
21 xout[0] = x0;
22
23 for (std::size_t i = 0; i < N; i++) {
24 shared_ptr<Data> data = dyn_models[i]->createData();
25 const ManifoldAbstractTpl<Scalar> &space = dyn_models[i]->space();
26 xout.push_back(space.neutral());
27 forwardDynamics<Scalar>::run(*dyn_models[i], xout[i], us[i], *data,
28 xout[i + 1]);
29 }
30 return xout;
31}
32
34template <typename Scalar>
35typename math_types<Scalar>::VectorOfVectors
36rollout(const DynamicsModelTpl<Scalar> &dyn_model,
37 const typename math_types<Scalar>::VectorXs &x0,
38 const typename math_types<Scalar>::VectorOfVectors &us) {
39 using VectorXs = typename math_types<Scalar>::VectorXs;
40
41 const std::size_t N = us.size();
42 std::vector<VectorXs> xs{x0};
43 xs.reserve(N + 1);
44 shared_ptr<DynamicsDataTpl<Scalar>> data = dyn_model.createData();
45
46 for (std::size_t i = 0; i < N; i++) {
47 const ManifoldAbstractTpl<Scalar> &space = dyn_model.space();
48 xs.push_back(space.neutral());
49 forwardDynamics<Scalar>::run(dyn_model, xs[i], us[i], *data, xs[i + 1]);
50 }
51 return xs;
52}
53
56template <typename Scalar>
57void rollout(
58 const std::vector<shared_ptr<ExplicitDynamicsModelTpl<Scalar>>> &dyn_models,
59 const typename math_types<Scalar>::VectorXs &x0,
60 const typename math_types<Scalar>::VectorOfVectors &us,
61 typename math_types<Scalar>::VectorOfVectors &xout) {
62 using DataType = ExplicitDynamicsDataTpl<Scalar>;
63 const std::size_t N = us.size();
64 xout.resize(N + 1);
65 xout[0] = x0;
66 if (dyn_models.size() != N) {
67 ALIGATOR_RUNTIME_ERROR(
68 fmt::format("Number of controls ({}) should be the same as number of "
69 "dynamical models ({})!",
70 N, dyn_models.size()));
71 }
72
73 for (std::size_t i = 0; i < N; i++) {
74 shared_ptr<DataType> data =
75 std::static_pointer_cast<DataType>(dyn_models[i]->createData());
76 dyn_models[i]->forward(xout[i], us[i], *data);
77 xout[i + 1] = data->xnext_;
78 }
79}
80
82template <typename Scalar>
83void rollout(const ExplicitDynamicsModelTpl<Scalar> &dyn_model,
84 const typename math_types<Scalar>::VectorXs &x0,
85 const typename math_types<Scalar>::VectorOfVectors &us,
86 typename math_types<Scalar>::VectorOfVectors &xout) {
87 using DataType = ExplicitDynamicsDataTpl<Scalar>;
88 const std::size_t N = us.size();
89 xout.resize(N + 1);
90 xout[0] = x0;
91
92 shared_ptr<DataType> data =
93 std::static_pointer_cast<DataType>(dyn_model.createData());
94 for (std::size_t i = 0; i < N; i++) {
95 dyn_model.forward(xout[i], us[i], *data);
96 xout[i + 1] = data->xnext_;
97 }
98}
99
101template <template <typename> class C, typename Scalar>
102typename math_types<Scalar>::VectorOfVectors
103rollout(const C<Scalar> &dms, const typename math_types<Scalar>::VectorXs &x0,
104 const typename math_types<Scalar>::VectorOfVectors &us) {
105 const std::size_t N = us.size();
106 typename math_types<Scalar>::VectorOfVectors xout;
107 xout.reserve(N + 1);
108 rollout(dms, x0, us, xout);
109 return xout;
110}
111
113template <template <typename> class C, typename Scalar>
114typename math_types<Scalar>::VectorOfVectors
115rollout(const std::vector<shared_ptr<C<Scalar>>> &dms,
116 const typename math_types<Scalar>::VectorXs &x0,
117 const typename math_types<Scalar>::VectorOfVectors &us) {
118 const std::size_t N = us.size();
119 typename math_types<Scalar>::VectorOfVectors xout;
120 xout.reserve(N + 1);
121 rollout(dms, x0, us, xout);
122 return xout;
123}
124
125} // namespace aligator
ALIGATOR_RUNTIME_ERROR
#define ALIGATOR_RUNTIME_ERROR(msg)
Definition exceptions.hpp:6
aligator
Main package namespace.
Definition action-model-wrap.hpp:12
aligator::rollout
math_types< Scalar >::VectorOfVectors rollout(const std::vector< shared_ptr< DynamicsModelTpl< Scalar > > > &dyn_models, const typename math_types< Scalar >::VectorXs &x0, const typename math_types< Scalar >::VectorOfVectors &us, typename math_types< Scalar >::VectorOfVectors &xout)
Perform a rollout of the supplied dynamical models.
Definition rollout.hpp:10
aligator::forwardDynamics::run
static void run(const DynamicsModelTpl< T > &model, const ConstVectorRef &x, const ConstVectorRef &u, DynamicsDataTpl< T > &data, VectorRef xout, const std::optional< ConstVectorRef > &gap=std::nullopt, const uint max_iters=1000, const T EPS=1e-6)
Definition forward-dyn.hpp:25