hpp-pinocchio
4.12.0
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
|
Go to the documentation of this file.
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
49 typedef boost::variant <liegroup::VectorSpaceOperation
50 <Eigen::Dynamic,
false>,
51 liegroup::VectorSpaceOperation <1, true>,
52 liegroup::VectorSpaceOperation <1, false>,
53 liegroup::VectorSpaceOperation <2, false>,
54 liegroup::VectorSpaceOperation <3, false>,
55 liegroup::VectorSpaceOperation <3, true>,
56 liegroup::CartesianProductOperation<
57 liegroup::VectorSpaceOperation<3, false>,
58 liegroup::SpecialOrthogonalOperation<3> >,
59 liegroup::CartesianProductOperation<
60 liegroup::VectorSpaceOperation<2, false>,
61 liegroup::SpecialOrthogonalOperation<2> >,
62 liegroup::SpecialOrthogonalOperation <2>,
63 liegroup::SpecialOrthogonalOperation <3>,
64 liegroup::SpecialEuclideanOperation <2>,
65 liegroup::SpecialEuclideanOperation <3> >
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>
278 std::string
name ()
const;
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;
344 #endif // HPP_PINOCCHIO_LIEGROUP_SPACE_HH
shared_ptr< const LiegroupSpace > LiegroupSpaceConstPtr_t
Definition: fwd.hh:137
LiegroupElementConstRef elementConstRef(vectorIn_t q) const
Create a LiegroupElementRef from a configuration.
static LiegroupSpacePtr_t R2xSO2()
Return .
static LiegroupSpacePtr_t R3()
Return as a Lie group.
static LiegroupSpacePtr_t empty()
Return empty Lie group.
double value_type
Definition: fwd.hh:40
void dIntegrate_dq(LiegroupElementConstRef q, vectorIn_t v, matrixOut_t J) const
LiegroupElement exp(vectorIn_t v) const
Return exponential of a tangent vector.
void interpolate(vectorIn_t q0, vectorIn_t q1, value_type u, vectorOut_t result) const
std::string name() const
Return name of Lie group.
Definition: liegroup-space.hh:93
ABoostVariant LiegroupType
Definition: liegroup-space.hh:47
shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:136
const std::vector< LiegroupType > & liegroupTypes() const
Get reference to vector of elementary types.
Definition: liegroup-space.hh:171
void Jdifference(vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const
DerivativeProduct
Definition: liegroup-space.hh:69
void dDifference_dq0(vectorIn_t q0, vectorIn_t q1, matrixOut_t J0) const
matrix_t::Index size_type
Definition: fwd.hh:84
static LiegroupSpacePtr_t create(const LiegroupType &type)
Create instance with one Elementary Lie group.
Definition: liegroup-space.hh:147
Utility functions.
Definition: body.hh:30
LiegroupElement element(vectorIn_t q) const
Create a LiegroupElement from a configuration.
size_type nq() const
Dimension of the vector representation.
Definition: liegroup-space.hh:156
Eigen::Ref< vector_t > vectorOut_t
Definition: fwd.hh:81
LiegroupElement neutral() const
Return the neutral element as a vector.
Eigen::Ref< const vector_t > vectorIn_t
Definition: fwd.hh:80
bool isVectorSpace() const
@ DerivativeTimesInput
Definition: liegroup-space.hh:70
static LiegroupSpacePtr_t SE2()
Return .
LiegroupSpacePtr_t operator*=(const LiegroupSpaceConstPtr_t &other)
static LiegroupSpacePtr_t Rn(const size_type &n)
bool operator!=(const LiegroupSpace &other) const
static LiegroupSpacePtr_t R3xSO3()
Return .
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > vector_t
Definition: fwd.hh:75
Definition: liegroup-space.hh:329
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:83
bool operator==(const LiegroupSpace &other) const
static LiegroupSpacePtr_t SO3()
Return .
void dIntegrate_dv(LiegroupElementConstRef q, vectorIn_t v, matrixOut_t Jv) const
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:359
@ InputTimesDerivative
Definition: liegroup-space.hh:71
static LiegroupSpacePtr_t createCopy(const LiegroupSpaceConstPtr_t &other)
Create copy.
Definition: liegroup-space.hh:138
hpp::pinocchio::LiegroupSpacePtr_t operator*(const hpp::pinocchio::LiegroupSpaceConstPtr_t &sp1, const hpp::pinocchio::LiegroupSpaceConstPtr_t &sp2)
Cartesian product between Lie groups.
LiegroupElementRef elementRef(vectorOut_t q) const
Create a LiegroupElementRef from a configuration.
static LiegroupSpacePtr_t R1(bool rotation=false)
void dDifference_dq1(vectorIn_t q0, vectorIn_t q1, matrixOut_t J1) const
LiegroupSpacePtr_t vectorSpacesMerged() const
size_type nv() const
Dimension of the Lie group tangent space.
Definition: liegroup-space.hh:161
static LiegroupSpacePtr_t create(const size_type &size)
Create instance of vector space of given size.
Definition: liegroup-space.hh:128
static LiegroupSpacePtr_t R2()
Return as a Lie group.
static LiegroupSpacePtr_t SE3()
Return .
static LiegroupSpacePtr_t SO2()
Return .
Definition: collision-object.hh:31