aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
multibody-free-fwd.hpp
Go to the documentation of this file.
1
2#pragma once
3
5
6#ifdef ALIGATOR_WITH_PINOCCHIO
8#include <pinocchio/multibody/data.hpp>
9
10namespace aligator {
11namespace dynamics {
12template <typename Scalar> struct MultibodyFreeFwdDataTpl;
13
24template <typename _Scalar>
26 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27 using Scalar = _Scalar;
34 using PolyManifold = xyz::polymorphic<Manifold>;
35
36 using Base::nu_;
37
40
41 const Manifold &space() const { return space_; }
42 int ntau() const { return space_.getModel().nv; }
43
44 MultibodyFreeFwdDynamicsTpl(const Manifold &state, const MatrixXs &actuation);
46
52 bool isUnderactuated() const {
53 long nv = space().getModel().nv;
54 return act_matrix_rank < nv;
55 }
56
57 Eigen::Index getActuationMatrixRank() const { return act_matrix_rank; }
58
59 virtual void forward(const ConstVectorRef &x, const ConstVectorRef &u,
60 BaseData &data) const;
61 virtual void dForward(const ConstVectorRef &x, const ConstVectorRef &u,
62 BaseData &data) const;
63
64 shared_ptr<ContDataAbstract> createData() const;
65
66private:
67 Eigen::FullPivLU<MatrixXs> lu_decomp_;
68 Eigen::Index act_matrix_rank;
69};
70
71template <typename Scalar>
83
84} // namespace dynamics
85} // namespace aligator
86
87#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
88#include "aligator/modelling/dynamics/multibody-free-fwd.txx"
89#endif
90#endif
Namespace for modelling system dynamics.
Main package namespace.
Defines a class representing ODEs.
The tangent bundle of a multibody configuration group.
const ModelType & getModel() const
Data struct for ContinuousDynamicsAbstractTpl.
MultibodyFreeFwdDataTpl(const MultibodyFreeFwdDynamicsTpl< Scalar > *cont_dyn)
ContinuousDynamicsDataTpl< Scalar > Base
typename math_types< Scalar >::MatrixXs MatrixXs
typename math_types< Scalar >::VectorXs VectorXs
Free-space multibody forward dynamics, using Pinocchio.
virtual void forward(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const
Evaluate the ODE vector field: this returns the value of .
ContinuousDynamicsDataTpl< Scalar > ContDataAbstract
virtual void dForward(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const
Evaluate the vector field Jacobians.
ContinuousDynamicsDataTpl< Scalar > BaseData
MultibodyFreeFwdDynamicsTpl(const Manifold &state, const MatrixXs &actuation)
shared_ptr< ContDataAbstract > createData() const
Create a data holder instance.
bool isUnderactuated() const
Determine whether the system is underactuated.
Base class for ODE dynamics .
Typedefs for math (Eigen vectors, matrices) depending on scalar type.
Definition math.hpp:122