29 #ifndef HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH 30 #define HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH 34 #include <pinocchio/math/quaternion.hpp> 44 template <
typename vector_type>
45 class LiegroupElementConstBase {
50 template <
typename Derived>
61 template <
typename Derived>
68 template <
typename vector_type2>
86 template <
typename vector_type2>
88 return ((*
space_ == *(other.space_)) && (
value_ == other.value_));
92 template <
typename vector_type2>
94 return ((*
space_ != *(other.space_)) || (
value_ != other.value_));
98 template <
typename Derived>
102 :
value_(const_cast<Derived&>(value.derived())),
space_(space) {}
107 template <
typename vector_type2>
109 template <
typename vector_type2>
116 template <
typename vector_type>
125 : Base(value, space, NULL) {}
130 : Base(
vector_t(space->nq()), space) {}
141 template <
typename vector_type2>
149 template <
typename vector_type2>
154 template <
typename vector_type2>
162 const vector_type&
vector()
const {
return Base::vector(); }
174 template <
typename vector_type2>
177 this->
space_ = other.space();
178 this->
value_ = other.vector();
183 template <
typename Vector>
185 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector);
186 assert(this->
space_->nq() == v.derived().size());
187 this->
value_.noalias() = v.derived();
200 template <
typename vector_type>
211 template <
typename vector_type1,
typename vector_type2>
221 template <
typename vector_type>
224 const value_type& eps = PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE);
228 template <
typename vector_type>
231 template <
typename vector_type>
234 os <<
"Lie group element in " << *(e.space()) <<
" represented by vector (" 235 << e.vector().transpose() <<
")";
243 #endif // HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH LiegroupElement operator+(const LiegroupElementConstBase< vector_type > &e, vectorIn_t v)
const vector_type & vector() const
Const vector representation.
Definition: liegroup-element.hh:76
vector_t log(const LiegroupElementConstBase< vector_type > &lge)
Compute the log as a tangent vector of a Lie group element.
LiegroupElementBase(LiegroupElementBase< vector_type2 > &other)
Casting operator from LiegroupElement to LiegroupElementRef
Definition: liegroup-element.hh:155
bool operator!=(const LiegroupElementConstBase< vector_type2 > &other)
Equality operator.
Definition: liegroup-element.hh:93
const vector_type & vector() const
Const vector representation.
Definition: liegroup-element.hh:162
bool operator==(const LiegroupElementConstBase< vector_type2 > &other)
Equality operator.
Definition: liegroup-element.hh:87
size_type size() const
Size of the vector representation.
Definition: liegroup-element.hh:79
Utility functions.
Definition: body.hh:39
LiegroupElementConstBase(const Eigen::EigenBase< Derived > &value, const LiegroupSpacePtr_t &liegroupSpace)
Definition: liegroup-element.hh:51
LiegroupElementConstBase(const Eigen::EigenBase< Derived > &value)
Definition: liegroup-element.hh:62
LiegroupElementBase & operator=(const Eigen::MatrixBase< Vector > &v)
Assignment from a vector.
Definition: liegroup-element.hh:184
LiegroupElementBase(const LiegroupSpacePtr_t &space)
Definition: liegroup-element.hh:129
LiegroupElementConstBase(const Eigen::EigenBase< Derived > &value, const LiegroupSpacePtr_t &space, void *)
Definition: liegroup-element.hh:99
matrix_t::Index size_type
Definition: fwd.hh:97
Definition: liegroup-space.hh:101
LiegroupElementBase()
Constructor of trivial element.
Definition: liegroup-element.hh:159
const LiegroupSpacePtr_t & space() const
get reference to vector of Lie groups
Definition: liegroup-element.hh:73
LiegroupElementConstBase< vector_type > Base
Definition: liegroup-element.hh:119
vector_t operator-(const LiegroupElementConstBase< vector_type1 > &e1, const LiegroupElementConstBase< vector_type2 > &e2)
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:366
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > vector_t
Definition: fwd.hh:88
vector_type value_
Definition: liegroup-element.hh:104
LiegroupSpacePtr_t space_
Definition: liegroup-element.hh:105
double value_type
Definition: fwd.hh:51
Definition: collision-object.hh:40
LiegroupElementBase(const LiegroupElementBase< vector_type2 > &other)
Definition: liegroup-element.hh:150
LiegroupElementBase & operator=(const LiegroupElementConstBase< vector_type2 > &other)
Assignment from another LiegroupElement.
Definition: liegroup-element.hh:175
LiegroupElementConstBase(const LiegroupElementConstBase< vector_type2 > &other)
Definition: liegroup-element.hh:69
Eigen::Ref< const vector_t > vectorIn_t
Definition: fwd.hh:93
LiegroupElementBase(const vector_type &value, const LiegroupSpacePtr_t &space)
Definition: liegroup-element.hh:124
void setNeutral()
Set element to neutral element.
Definition: liegroup-element.hh:168
void check() const
Definition: liegroup-element.hh:83
LiegroupElementBase(const LiegroupElementConstBase< vector_type2 > &other)
Definition: liegroup-element.hh:142
bool checkNormalized(const LiegroupElementConstBase< vector_type > &e1, const value_type &eps=PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)
LiegroupElementBase(const vector_type &value)
Definition: liegroup-element.hh:136
vector_type & vector()
Modifiable vector representation.
Definition: liegroup-element.hh:165
shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:150