aligator  0.15.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
rollout.hpp
Go to the documentation of this file.
1
3#pragma once
4
5#include "aligator/utils/forward-dyn.hpp"
6
7namespace aligator {
8
10template <typename Scalar>
11typename math_types<Scalar>::VectorOfVectors rollout(
12 const std::vector<xyz::polymorphic<DynamicsModelTpl<Scalar>>> &dyn_models,
13 const typename math_types<Scalar>::VectorXs &x0,
14 const typename math_types<Scalar>::VectorOfVectors &us,
15 typename math_types<Scalar>::VectorOfVectors &xout) {
16 using Data = DynamicsDataTpl<Scalar>;
17 const std::size_t N = us.size();
18 if (dyn_models.size() != N) {
19 ALIGATOR_RUNTIME_ERROR(
20 "Number of controls should be the same as number of dynamical models!");
21 }
22 xout.resize(N + 1);
23 xout[0] = x0;
24
25 for (std::size_t i = 0; i < N; i++) {
26 shared_ptr<Data> data = dyn_models[i]->createData();
27 const ManifoldAbstractTpl<Scalar> &space = dyn_models[i]->space();
28 xout.push_back(space.neutral());
29 forwardDynamics(*dyn_models[i], xout[i], us[i], *data, xout[i + 1]);
30 }
31 return xout;
32}
33
35template <typename Scalar>
36typename math_types<Scalar>::VectorOfVectors
37rollout(const DynamicsModelTpl<Scalar> &dyn_model,
38 const typename math_types<Scalar>::VectorXs &x0,
39 const typename math_types<Scalar>::VectorOfVectors &us) {
40 using VectorXs = typename math_types<Scalar>::VectorXs;
41
42 const std::size_t N = us.size();
43 std::vector<VectorXs> xs{x0};
44 xs.reserve(N + 1);
45 shared_ptr<DynamicsDataTpl<Scalar>> data = dyn_model.createData();
46
47 for (std::size_t i = 0; i < N; i++) {
48 const ManifoldAbstractTpl<Scalar> &space = dyn_model.space();
49 xs.push_back(space.neutral());
50 forwardDynamics(dyn_model, xs[i], us[i], *data, xs[i + 1]);
51 }
52 return xs;
53}
54
57template <typename Scalar>
58void rollout(
59 const std::vector<xyz::polymorphic<ExplicitDynamicsModelTpl<Scalar>>>
60 &dyn_models,
61 const typename math_types<Scalar>::VectorXs &x0,
62 const typename math_types<Scalar>::VectorOfVectors &us,
63 typename math_types<Scalar>::VectorOfVectors &xout) {
64 using DataType = ExplicitDynamicsDataTpl<Scalar>;
65 const std::size_t N = us.size();
66 xout.resize(N + 1);
67 xout[0] = x0;
68 if (dyn_models.size() != N) {
69 ALIGATOR_RUNTIME_ERROR("Number of controls ({:d}) should be the same as "
70 "number of dynamical models ({:d})!",
71 N, dyn_models.size());
72 }
73
74 for (std::size_t i = 0; i < N; i++) {
75 shared_ptr<DataType> data =
76 std::static_pointer_cast<DataType>(dyn_models[i]->createData());
77 dyn_models[i]->forward(xout[i], us[i], *data);
78 xout[i + 1] = data->xnext_;
79 }
80}
81
83template <typename Scalar>
84void rollout(const ExplicitDynamicsModelTpl<Scalar> &dyn_model,
85 const typename math_types<Scalar>::VectorXs &x0,
86 const typename math_types<Scalar>::VectorOfVectors &us,
87 typename math_types<Scalar>::VectorOfVectors &xout) {
88 using DataType = ExplicitDynamicsDataTpl<Scalar>;
89 const std::size_t N = us.size();
90 xout.resize(N + 1);
91 xout[0] = x0;
92
93 shared_ptr<DataType> data =
94 std::static_pointer_cast<DataType>(dyn_model.createData());
95 for (std::size_t i = 0; i < N; i++) {
96 dyn_model.forward(xout[i], us[i], *data);
97 xout[i + 1] = data->xnext_;
98 }
99}
100
102template <template <typename> class C, typename Scalar>
103typename math_types<Scalar>::VectorOfVectors
104rollout(const C<Scalar> &dms, const typename math_types<Scalar>::VectorXs &x0,
105 const typename math_types<Scalar>::VectorOfVectors &us) {
106 const std::size_t N = us.size();
107 typename math_types<Scalar>::VectorOfVectors xout;
108 xout.reserve(N + 1);
109 rollout(dms, x0, us, xout);
110 return xout;
111}
112
114template <template <typename> class C, typename Scalar>
115typename math_types<Scalar>::VectorOfVectors
116rollout(const std::vector<xyz::polymorphic<C<Scalar>>> &dms,
117 const typename math_types<Scalar>::VectorXs &x0,
118 const typename math_types<Scalar>::VectorOfVectors &us) {
119 const std::size_t N = us.size();
120 typename math_types<Scalar>::VectorOfVectors xout;
121 xout.reserve(N + 1);
122 rollout(dms, x0, us, xout);
123 return xout;
124}
125
126} // namespace aligator
ALIGATOR_RUNTIME_ERROR
#define ALIGATOR_RUNTIME_ERROR(...)
Definition exceptions.hpp:7
aligator
Main package namespace.
Definition action-model-wrap.hpp:14
aligator::forwardDynamics
void forwardDynamics(const M< T > &model, const typename math_types< T >::ConstVectorRef &x, const typename math_types< T >::ConstVectorRef &u, D &data, const typename math_types< T >::VectorRef xout, const std::optional< typename math_types< T >::ConstVectorRef > &gap=std::nullopt, ForwardDynamicsOptions< T > opts={})
Evaluates the forward map for a discrete dynamics model, implicit or explicit.
Definition forward-dyn.hpp:65
aligator::rollout
math_types< Scalar >::VectorOfVectors rollout(const std::vector< xyz::polymorphic< 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:11
aligator::DynamicsModelTpl
Dynamics model: describes system dynamics through an implicit relation .
Definition dynamics.hpp:14
aligator::DynamicsModelTpl::space
const Manifold & space() const
State space for the input.
Definition dynamics.hpp:32
aligator::DynamicsModelTpl::createData
virtual shared_ptr< Data > createData() const
aligator::ExplicitDynamicsDataTpl
Specific data struct for explicit dynamics ExplicitDynamicsModelTpl.
Definition explicit-dynamics.hpp:64
aligator::ExplicitDynamicsModelTpl
Explicit forward dynamics model .
Definition explicit-dynamics.hpp:21
aligator::ExplicitDynamicsModelTpl::forward
virtual void forward(const ConstVectorRef &x, const ConstVectorRef &u, Data &data) const =0
Evaluate the forward discrete dynamics.
aligator::ExplicitDynamicsModelTpl::createData
virtual shared_ptr< Data > createData() const
Definition explicit-dynamics.hpp:51
aligator::ManifoldAbstractTpl
Base class for manifolds, to use in cost funcs, solvers...
Definition manifold-base.hpp:11
aligator::ManifoldAbstractTpl::neutral
VectorXs neutral() const
Get the neutral element from the manifold (if this makes sense).
Definition manifold-base.hpp:31
aligator::math_types
Typedefs for math (Eigen vectors, matrices) depending on scalar type.
Definition math.hpp:122