20 using Scalar = _Scalar;
21 static constexpr int Options = _Options;
23 using Self = MultibodyConfiguration<Scalar, Options>;
24 using ModelType = pinocchio::ModelTpl<Scalar, Options>;
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;
33 const ModelType &getModel()
const {
return model_; }
35 VectorXs
neutral()
const {
return pinocchio::neutral(model_); }
36 VectorXs
rand()
const {
return pinocchio::randomConfiguration(model_); }
38 return pinocchio::isNormalized(model_, x);
45 VectorRef xout)
const {
46 pinocchio::integrate(model_, x, v, xout);
49 void Jintegrate_impl(
const ConstVectorRef &x,
const ConstVectorRef &v,
50 MatrixRef Jout,
int arg)
const {
53 pinocchio::dIntegrate(model_, x, v, Jout, pinocchio::ARG0);
56 pinocchio::dIntegrate(model_, x, v, Jout, pinocchio::ARG1);
62 MatrixRef Jout,
int arg)
const {
65 pinocchio::dIntegrateTransport(model_, x, v, Jout, pinocchio::ARG0);
68 pinocchio::dIntegrateTransport(model_, x, v, Jout, pinocchio::ARG1);
76 VectorRef vout)
const {
77 pinocchio::difference(model_, x0, x1, vout);
80 void Jdifference_impl(
const ConstVectorRef &x0,
const ConstVectorRef &x1,
81 MatrixRef Jout,
int arg)
const {
84 pinocchio::dDifference(model_, x0, x1, Jout, pinocchio::ARG0);
87 pinocchio::dDifference(model_, x0, x1, Jout, pinocchio::ARG1);
93 const Scalar &u, VectorRef out)
const {
94 pinocchio::interpolate(model_, x0, x1, u, out);
97 inline int nx()
const {
return model_.nq; }
98 inline int ndx()
const {
return model_.nv; }
113struct MultibodyPhaseSpace
116 using ModelType =
typename ConfigSpace::ModelType;
118 const ModelType &getModel()
const {
return this->base_.getModel(); }
120 MultibodyPhaseSpace(
const ModelType &model)
122 MultibodyPhaseSpace(
const MultibodyPhaseSpace &) =
default;
123 MultibodyPhaseSpace &operator=(
const MultibodyPhaseSpace &) =
default;
124 MultibodyPhaseSpace(MultibodyPhaseSpace &&) =
default;
125 MultibodyPhaseSpace &operator=(MultibodyPhaseSpace &&) =
default;
Multibody configuration group , defined using the Pinocchio library.
int ndx() const
Get manifold tangent space dimension.
VectorXs neutral() const
Get the neutral element from the manifold (if this makes sense).
void interpolate_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, const Scalar &u, VectorRef out) const
Interpolation operation.
void integrate_impl(const ConstVectorRef &x, const ConstVectorRef &v, VectorRef xout) const
Perform the manifold integration operation.
void JintegrateTransport(const ConstVectorRef &x, const ConstVectorRef &v, MatrixRef Jout, int arg) const
Perform the parallel transport operation.
bool isNormalized(const ConstVectorRef &x) const
Check if the input vector x is a viable element of the manifold.
VectorXs rand() const
Sample a random point on the manifold.
void difference_impl(const ConstVectorRef &x0, const ConstVectorRef &x1, VectorRef vout) const
Implementation of the manifold retraction operation.
int nx() const
Get manifold representation dimension.