aligator  0.9.0
A primal-dual augmented Lagrangian-type solver for nonlinear trajectory optimization.
Loading...
Searching...
No Matches
forward-dyn.hpp
Go to the documentation of this file.
1
3#pragma once
4
7#include <optional>
8
9namespace aligator {
10
18template <typename T> struct forwardDynamics {
19
20 using VectorXs = typename math_types<T>::VectorXs;
21 using VectorRef = typename math_types<T>::VectorRef;
22 using ConstVectorRef = typename math_types<T>::ConstVectorRef;
23 using MatrixRef = typename math_types<T>::MatrixRef;
24
25 static void run(const DynamicsModelTpl<T> &model, const ConstVectorRef &x,
26 const ConstVectorRef &u, DynamicsDataTpl<T> &data,
27 VectorRef xout,
28 const std::optional<ConstVectorRef> &gap = std::nullopt,
29 const uint max_iters = 1000, const T EPS = 1e-6) {
30 using ExpModel = ExplicitDynamicsModelTpl<T>;
31 using ExpData = ExplicitDynamicsDataTpl<T>;
32
33 if (model.isExplicit()) {
34 const ExpModel &model_cast = static_cast<const ExpModel &>(model);
35 ExpData &data_cast = static_cast<ExpData &>(data);
36 run(model_cast, x, u, data_cast, xout, gap);
37 } else {
38 // create NewtonRaph algo's data
39 VectorXs dx0buf(model.ndx2);
40 dx0buf.setZero();
42 model.space_next(),
43 [&](const ConstVectorRef &xnext, VectorRef out) {
44 model.evaluate(x, u, xnext, data);
45 out = data.value_;
46 if (gap.has_value())
47 out += *gap;
48 },
49 [&](const ConstVectorRef &xnext, MatrixRef Jout) {
50 model.computeJacobians(x, u, xnext, data);
51 Jout = data.Jy_;
52 },
53 x, xout, data.value_, dx0buf, data.Jy_, EPS, max_iters);
54 }
55 }
56
57 static void run(const ExplicitDynamicsModelTpl<T> &model,
58 const ConstVectorRef &x, const ConstVectorRef &u,
60 const std::optional<ConstVectorRef> &gap = std::nullopt) {
61 model.forward(x, u, data);
62 xout = data.xnext_;
63 if (gap.has_value()) {
64 model.space_next().integrate(xout, *gap, xout);
65 }
66 }
67};
68
69} // namespace aligator
Main package namespace.
unsigned int uint
Definition logger.hpp:10
Dynamics model: describes system dynamics through an implicit relation .
Definition fwd.hpp:71
virtual bool isExplicit() const
Check if this dynamics model is implicit or explicit.
Definition dynamics.hpp:36
const int ndx2
Next state space dimension.
Definition dynamics.hpp:29
const Manifold & space_next() const
State space for the output of this dynamics model.
Definition dynamics.hpp:34
Specific data struct for explicit dynamics ExplicitDynamicsModelTpl.
Definition fwd.hpp:83
Explicit forward dynamics model .
Definition fwd.hpp:80
virtual void forward(const ConstVectorRef &x, const ConstVectorRef &u, Data &data) const =0
Evaluate the forward discrete dynamics.
static bool run(const Manifold &space, Fun &&fun, JacFun &&jac_fun, const ConstVectorRef &xinit, VectorRef xout, VectorRef f0, VectorRef dx, MatrixRef Jf0, Scalar eps=1e-6, std::size_t max_iters=1000, Options options=Options{})
Evaluates the forward map for a discrete dynamics model, implicit or explicit.
typename math_types< T >::VectorRef VectorRef
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)
static void run(const ExplicitDynamicsModelTpl< T > &model, const ConstVectorRef &x, const ConstVectorRef &u, ExplicitDynamicsDataTpl< T > &data, VectorRef xout, const std::optional< ConstVectorRef > &gap=std::nullopt)
typename math_types< T >::ConstVectorRef ConstVectorRef
typename math_types< T >::VectorXs VectorXs
typename math_types< T >::MatrixRef MatrixRef