proxsuite-nlp  0.11.0
A primal-dual augmented Lagrangian-type solver for nonlinear programming on manifolds.
 
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
Loading...
Searching...
No Matches
multibody.hpp
1
2#pragma once
3
4#include <pinocchio/multibody/model.hpp>
5#include <pinocchio/algorithm/joint-configuration.hpp>
6
7#include "proxsuite-nlp/modelling/spaces/tangent-bundle.hpp"
8
9namespace proxsuite {
10namespace nlp {
11
17template <typename _Scalar, int _Options = 0>
18struct MultibodyConfiguration : public ManifoldAbstractTpl<_Scalar, _Options> {
19public:
20 using Scalar = _Scalar;
21 static constexpr int Options = _Options;
23 using Self = MultibodyConfiguration<Scalar, Options>;
24 using ModelType = pinocchio::ModelTpl<Scalar, Options>;
26
27 MultibodyConfiguration(const ModelType &model) : model_(model) {};
28 MultibodyConfiguration(const MultibodyConfiguration &) = default;
29 MultibodyConfiguration &operator=(const MultibodyConfiguration &) = default;
30 MultibodyConfiguration(MultibodyConfiguration &&) = default;
31 MultibodyConfiguration &operator=(MultibodyConfiguration &&) = default;
32
33 const ModelType &getModel() const { return model_; }
34
35 VectorXs neutral() const { return pinocchio::neutral(model_); }
36 VectorXs rand() const { return pinocchio::randomConfiguration(model_); }
37 bool isNormalized(const ConstVectorRef &x) const {
38 return pinocchio::isNormalized(model_, x);
39 }
40
43
44 void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
45 VectorRef xout) const {
46 pinocchio::integrate(model_, x, v, xout);
47 }
48
49 void Jintegrate_impl(const ConstVectorRef &x, const ConstVectorRef &v,
50 MatrixRef Jout, int arg) const {
51 switch (arg) {
52 case 0:
53 pinocchio::dIntegrate(model_, x, v, Jout, pinocchio::ARG0);
54 break;
55 case 1:
56 pinocchio::dIntegrate(model_, x, v, Jout, pinocchio::ARG1);
57 break;
58 }
59 }
60
61 void JintegrateTransport(const ConstVectorRef &x, const ConstVectorRef &v,
62 MatrixRef Jout, int arg) const {
63 switch (arg) {
64 case 0:
65 pinocchio::dIntegrateTransport(model_, x, v, Jout, pinocchio::ARG0);
66 break;
67 case 1:
68 pinocchio::dIntegrateTransport(model_, x, v, Jout, pinocchio::ARG1);
69 break;
70 default:
71 break;
72 }
73 }
74
75 void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
76 VectorRef vout) const {
77 pinocchio::difference(model_, x0, x1, vout);
78 }
79
80 void Jdifference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
81 MatrixRef Jout, int arg) const {
82 switch (arg) {
83 case 0:
84 pinocchio::dDifference(model_, x0, x1, Jout, pinocchio::ARG0);
85 break;
86 case 1:
87 pinocchio::dDifference(model_, x0, x1, Jout, pinocchio::ARG1);
88 break;
89 }
90 }
91
92 void interpolate_impl(const ConstVectorRef &x0, const ConstVectorRef &x1,
93 const Scalar &u, VectorRef out) const {
94 pinocchio::interpolate(model_, x0, x1, u, out);
95 }
96
97 inline int nx() const { return model_.nq; }
98 inline int ndx() const { return model_.nv; }
99
101
102protected:
103 ModelType model_;
104};
105
112template <typename Scalar, int Options = 0>
113struct MultibodyPhaseSpace
114 : TangentBundleTpl<MultibodyConfiguration<Scalar, Options>> {
115 using ConfigSpace = MultibodyConfiguration<Scalar, Options>;
116 using ModelType = typename ConfigSpace::ModelType;
117
118 const ModelType &getModel() const { return this->base_.getModel(); }
119
120 MultibodyPhaseSpace(const ModelType &model)
121 : TangentBundleTpl<ConfigSpace>(ConfigSpace(model)) {}
122 MultibodyPhaseSpace(const MultibodyPhaseSpace &) = default;
123 MultibodyPhaseSpace &operator=(const MultibodyPhaseSpace &) = default;
124 MultibodyPhaseSpace(MultibodyPhaseSpace &&) = default;
125 MultibodyPhaseSpace &operator=(MultibodyPhaseSpace &&) = default;
126};
127
128} // namespace nlp
129} // namespace proxsuite
130
131#ifdef PROXSUITE_NLP_ENABLE_TEMPLATE_INSTANTIATION
132#include "proxsuite-nlp/modelling/spaces/multibody.txx"
133#endif
#define PROXSUITE_NLP_DYNAMIC_TYPEDEFS(Scalar)
Definition math.hpp:26
Main package namespace.
Definition bcl-params.hpp:5
Multibody configuration group , defined using the Pinocchio library.
Definition multibody.hpp:18
int ndx() const
Get manifold tangent space dimension.
Definition multibody.hpp:98
VectorXs neutral() const
Get the neutral element from the manifold (if this makes sense).
Definition multibody.hpp:35
void interpolate_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, const Scalar &u, VectorRef out) const
Interpolation operation.
Definition multibody.hpp:92
void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v, VectorRef xout) const
Perform the manifold integration operation.
Definition multibody.hpp:44
void JintegrateTransport(const ConstVectorRef &x, const ConstVectorRef &v, MatrixRef Jout, int arg) const
Perform the parallel transport operation.
Definition multibody.hpp:61
bool isNormalized(const ConstVectorRef &x) const
Check if the input vector x is a viable element of the manifold.
Definition multibody.hpp:37
VectorXs rand() const
Sample a random point on the manifold.
Definition multibody.hpp:36
void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, VectorRef vout) const
Implementation of the manifold retraction operation.
Definition multibody.hpp:75
int nx() const
Get manifold representation dimension.
Definition multibody.hpp:97