aligator  0.16.0
A versatile and efficient C++ library for real-time constrained 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
11template <typename T> struct ForwardDynamicsOptions {
12 uint max_iters = 1000u;
13 T eps = 1e-6;
14};
15
16namespace detail {
17template <typename T> struct ForwardDynamics {
18
19 using VectorXs = typename math_types<T>::VectorXs;
20 using VectorRef = typename math_types<T>::VectorRef;
21 using ConstVectorRef = typename math_types<T>::ConstVectorRef;
22 using MatrixRef = typename math_types<T>::MatrixRef;
23
24 static void run(const DynamicsModelTpl<T> &model, const ConstVectorRef &x,
25 const ConstVectorRef &u, DynamicsDataTpl<T> &data,
26 VectorRef xout, const std::optional<ConstVectorRef> &gap,
28 // create NewtonRaph algo's data
29 VectorXs dx0buf(model.ndx2);
30 dx0buf.setZero();
32 model.space_next(),
33 [&](const ConstVectorRef &xnext, VectorRef out) {
34 model.evaluate(x, u, xnext, data);
35 out = data.value_;
36 if (gap.has_value())
37 out += *gap;
38 },
39 [&](const ConstVectorRef &xnext, MatrixRef Jout) {
40 model.computeJacobians(x, u, xnext, data);
41 Jout = data.Jy_;
42 },
43 x, xout, data.value_, dx0buf, data.Jy_, opts.eps, opts.max_iters);
44 }
45
46 static void run(const ExplicitDynamicsModelTpl<T> &model,
47 const ConstVectorRef &x, const ConstVectorRef &u,
48 ExplicitDynamicsDataTpl<T> &data, VectorRef xout,
49 const std::optional<ConstVectorRef> &gap,
51 model.forward(x, u, data);
52 xout = data.xnext_;
53 if (gap.has_value())
54 model.space_next().integrate(xout, *gap, xout);
55 }
56};
57} // namespace detail
58
63template <typename T, template <typename> class M,
64 typename D = typename M<T>::Data>
65void forwardDynamics(const M<T> &model,
66 const typename math_types<T>::ConstVectorRef &x,
67 const typename math_types<T>::ConstVectorRef &u, D &data,
68 const typename math_types<T>::VectorRef xout,
69 const std::optional<typename math_types<T>::ConstVectorRef>
70 &gap = std::nullopt,
71 ForwardDynamicsOptions<T> opts = {}) {
72 detail::ForwardDynamics<T>::run(model, x, u, data, xout, gap, opts);
73}
74
75} // namespace aligator
Main package namespace.
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.
unsigned int uint
Definition logger.hpp:9
MatrixRef Jy_
Jacobian with respect to .
Definition dynamics.hpp:97
VectorXs value_
Function value.
Definition dynamics.hpp:88
Dynamics model: describes system dynamics through an implicit relation .
Definition dynamics.hpp:14
const Manifold & space_next() const
State space for the output of this dynamics model.
Definition dynamics.hpp:34
const int ndx2
Next state space dimension.
Definition dynamics.hpp:29
Specific data struct for explicit dynamics ExplicitDynamicsModelTpl.
Explicit forward dynamics model .
void integrate(const ConstVectorRef &x, const ConstVectorRef &v, VectorRef out) const
Manifold integration operation .
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{})
Typedefs for math (Eigen vectors, matrices) depending on scalar type.
Definition math.hpp:122