17 #ifndef HPP_PINOCCHIO_LIEGROUP_SPACE_HH
18 # define HPP_PINOCCHIO_LIEGROUP_SPACE_HH
22 # include <pinocchio/fwd.hpp>
23 # include <boost/variant.hpp>
24 # include <hpp/util/serialization-fwd.hh>
25 # include <pinocchio/multibody/liegroup/special-euclidean.hpp>
26 # include <pinocchio/multibody/liegroup/special-orthogonal.hpp>
27 # include <pinocchio/multibody/liegroup/vector-space.hpp>
36 #ifdef HPP_PINOCCHIO_PARSED_BY_DOXYGEN
50 <Eigen::Dynamic,
false>,
96 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
173 return liegroupTypes_;
211 template <DerivativeProduct s
ide>
235 template <DerivativeProduct s
ide>
239 template <
bool ApplyOnTheLeft>
251 template <DerivativeProduct s
ide>
263 template <DerivativeProduct s
ide>
302 void init (
const LiegroupSpaceWkPtr_t weak);
306 void computeNeutral ();
307 typedef std::vector <LiegroupType> LiegroupTypes;
308 LiegroupTypes liegroupTypes_;
314 LiegroupSpaceWkPtr_t weak_;
322 os << space.
name ();
return os;
Definition: liegroup-element.hh:123
Definition: liegroup-element.hh:33
Definition: liegroup-space.hh:94
LiegroupElementRef elementRef(vectorOut_t q) const
Create a LiegroupElementRef from a configuration.
LiegroupElement element(vectorIn_t q) const
Create a LiegroupElement from a configuration.
void dDifference_dq0(vectorIn_t q0, vectorIn_t q1, matrixOut_t J0) const
static LiegroupSpacePtr_t R2()
Return as a Lie group.
LiegroupElement exp(vectorIn_t v) const
Return exponential of a tangent vector.
void dIntegrate_dq(LiegroupElementConstRef q, vectorIn_t v, matrixOut_t J) const
bool isVectorSpace() const
LiegroupSpace(const LiegroupType &type)
size_type nq(const std::size_t &rank) const
Dimension of elementary Liegroup at given rank.
static LiegroupSpacePtr_t create(const LiegroupType &type)
Create instance with one Elementary Lie group.
Definition: liegroup-space.hh:147
bool operator==(const LiegroupSpace &other) const
size_type nq() const
Dimension of the vector representation.
Definition: liegroup-space.hh:156
void interpolate(vectorIn_t q0, vectorIn_t q1, value_type u, vectorOut_t result) const
static LiegroupSpacePtr_t R2xSO2()
Return .
size_type nv(const std::size_t &rank) const
Dimension of elementary Liegroup tangent space at given rank.
LiegroupElement neutral() const
Return the neutral element as a vector.
static LiegroupSpacePtr_t empty()
Return empty Lie group.
static LiegroupSpacePtr_t createCopy(const LiegroupSpaceConstPtr_t &other)
Create copy.
Definition: liegroup-space.hh:138
static LiegroupSpacePtr_t R3()
Return as a Lie group.
std::string name() const
Return name of Lie group.
static LiegroupSpacePtr_t Rn(const size_type &n)
static LiegroupSpacePtr_t SE2()
Return .
static LiegroupSpacePtr_t create(const size_type &size)
Create instance of vector space of given size.
Definition: liegroup-space.hh:128
void Jdifference(vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const
void dIntegrate_dv(LiegroupElementConstRef q, vectorIn_t v, matrixOut_t Jv) const
LiegroupSpacePtr_t operator*=(const LiegroupSpaceConstPtr_t &other)
static LiegroupSpacePtr_t R1(bool rotation=false)
static LiegroupSpacePtr_t SE3()
Return .
static LiegroupSpacePtr_t SO2()
Return .
void dDifference_dq1(vectorIn_t q0, vectorIn_t q1, matrixOut_t J1) const
static LiegroupSpacePtr_t R3xSO3()
Return .
const std::vector< LiegroupType > & liegroupTypes() const
Get reference to vector of elementary types.
Definition: liegroup-space.hh:171
size_type nv() const
Dimension of the Lie group tangent space.
Definition: liegroup-space.hh:161
LiegroupElementConstRef elementConstRef(vectorIn_t q) const
Create a LiegroupElementRef from a configuration.
static LiegroupSpacePtr_t SO3()
Return .
LiegroupSpace(const size_type &size)
Constructor of vector space of given size.
bool operator!=(const LiegroupSpace &other) const
LiegroupSpacePtr_t vectorSpacesMerged() const
LiegroupSpace(const LiegroupSpace &other)
hpp::pinocchio::LiegroupSpacePtr_t operator*(const hpp::pinocchio::LiegroupSpaceConstPtr_t &sp1, const hpp::pinocchio::LiegroupSpaceConstPtr_t &sp2)
Cartesian product between Lie groups.
DerivativeProduct
Definition: liegroup-space.hh:69
ABoostVariant LiegroupType
Definition: liegroup-space.hh:47
@ InputTimesDerivative
Definition: liegroup-space.hh:71
@ DerivativeTimesInput
Definition: liegroup-space.hh:70
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:83
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > vector_t
Definition: fwd.hh:75
Eigen::Ref< vector_t > vectorOut_t
Definition: fwd.hh:81
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:359
shared_ptr< const LiegroupSpace > LiegroupSpaceConstPtr_t
Definition: fwd.hh:137
shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:136
matrix_t::Index size_type
Definition: fwd.hh:84
double value_type
Definition: fwd.hh:40
Eigen::Ref< const vector_t > vectorIn_t
Definition: fwd.hh:80
Utility functions.
Definition: body.hh:30
Definition: collision-object.hh:32
Definition: liegroup-space.hh:329
Definition: cartesian-product.hh:29
Definition: special-euclidean.hh:27
Definition: special-orthogonal.hh:27
Definition: vector-space.hh:50