hpp-pinocchio  4.11.0
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
liegroup-element.hh
Go to the documentation of this file.
1 // Copyright (c) 2017, CNRS
2 // Authors: Florent Lamiraux
3 //
4 // This file is part of hpp-pinocchio.
5 // hpp-pinocchio is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-pinocchio is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH
18 # define HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH
19 
22 
23 namespace hpp {
24  namespace pinocchio {
27 
31  template <typename vector_type>
32  class LiegroupElementConstBase
33  {
34  public:
38  template <typename Derived>
39  LiegroupElementConstBase (const Eigen::EigenBase<Derived>& value,
40  const LiegroupSpacePtr_t& liegroupSpace) :
41  value_ (value.derived()), space_ (liegroupSpace)
42  {
43  check();
44  }
45 
50  template <typename Derived>
51  explicit LiegroupElementConstBase (const Eigen::EigenBase<Derived>& value) :
52  value_ (value.derived()),
53  space_ (LiegroupSpace::create (value.derived().size ())) {}
54 
57  template <typename vector_type2>
59  value_ (other.vector()),
60  space_ (other.space ())
61  {}
62 
64  const LiegroupSpacePtr_t& space () const
65  {
66  return space_;
67  }
68 
70  const vector_type& vector () const
71  {
72  return value_;
73  }
74 
76  size_type size () const
77  {
78  return value_.size ();
79  }
80 
83  void check () const
84  {
85  assert (value_.size () == space_->nq ());
86  }
87 
89  template <typename vector_type2>
91  {
92  return ((*space_ == *(other.space_)) && (value_ == other.value_));
93  }
94 
96  template <typename vector_type2>
98  {
99  return ((*space_ != *(other.space_)) || (value_ != other.value_));
100  }
101 
102  protected:
103  template <typename Derived>
104  LiegroupElementConstBase (const Eigen::EigenBase<Derived>& value,
105  const LiegroupSpacePtr_t& space,
106  void* /*to enable this constructor*/):
107  value_ (const_cast<Derived&>(value.derived())),
108  space_ (space)
109  {}
110 
111  vector_type value_;
113 
114  template <typename vector_type2> friend class LiegroupElementConstBase;
115  template <typename vector_type2> friend class LiegroupElementBase;
116  };
117 
121  template <typename vector_type>
122  class LiegroupElementBase : public LiegroupElementConstBase<vector_type>
123  {
124  public:
126 
130  LiegroupElementBase (const vector_type& value,
131  const LiegroupSpacePtr_t& space) :
132  Base (value, space, NULL) {}
133 
137  Base (vector_t (space->nq ()), space) {}
138 
143  explicit LiegroupElementBase (const vector_type& value) :
144  Base (value, LiegroupSpace::create (value.size ()), NULL) {}
145 
148  template <typename vector_type2>
150  Base (other.value_, other.space()) {}
151 
156  template <typename vector_type2>
158  Base (other.value_, other.space()) {}
159 
161  template <typename vector_type2>
163  Base (other.value_, other.space(), NULL) {}
164 
166  LiegroupElementBase () : Base (vector_t(), LiegroupSpace::empty ()) {}
167 
169  const vector_type& vector () const
170  {
171  return Base::vector();
172  }
173 
175  vector_type& vector ()
176  {
177  return this->value_;
178  }
179 
181  void setNeutral ()
182  {
183  this->value_ = this->space_->neutral ().vector ();
184  }
185 
187  LiegroupElementBase& operator+= (vectorIn_t v);
188 
190  template <typename vector_type2>
192  {
193  this->space_ = other.space();
194  this->value_ = other.vector();
195  return *this;
196  }
197 
199  template <typename Vector>
200  LiegroupElementBase& operator= (const Eigen::MatrixBase<Vector>& v)
201  {
202  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector);
203  assert (this->space_->nq() == v.derived().size());
204  this->value_.noalias() = v.derived();
205  return *this;
206  }
207  };
208 
217  template <typename vector_type>
219 
227  template <typename vector_type1, typename vector_type2>
230 
232  template <typename vector_type>
234 
235  template <typename vector_type>
236  inline std::ostream& operator<< (std::ostream& os, const LiegroupElementConstBase<vector_type>& e)
237  {
238  os << "Lie group element in " << *(e.space ())
239  << " represented by vector (" << e. vector ().transpose () << ")";
240  return os;
241  }
242  } // namespace pinocchio
243 } // namespace hpp
244 
245 HPP_SERIALIZABLE_SPLIT_FREE(hpp::pinocchio::LiegroupElement)
246 
247 #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:70
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:162
bool operator!=(const LiegroupElementConstBase< vector_type2 > &other)
Equality operator.
Definition: liegroup-element.hh:97
const vector_type & vector() const
Const vector representation.
Definition: liegroup-element.hh:169
bool operator==(const LiegroupElementConstBase< vector_type2 > &other)
Equality operator.
Definition: liegroup-element.hh:90
size_type size() const
Size of the vector representation.
Definition: liegroup-element.hh:76
Utility functions.
Definition: body.hh:30
LiegroupElementConstBase(const Eigen::EigenBase< Derived > &value, const LiegroupSpacePtr_t &liegroupSpace)
Definition: liegroup-element.hh:39
LiegroupElementConstBase(const Eigen::EigenBase< Derived > &value)
Definition: liegroup-element.hh:51
LiegroupElementBase(const LiegroupSpacePtr_t &space)
Definition: liegroup-element.hh:136
LiegroupElementConstBase(const Eigen::EigenBase< Derived > &value, const LiegroupSpacePtr_t &space, void *)
Definition: liegroup-element.hh:104
matrix_t::Index size_type
Definition: fwd.hh:84
Definition: liegroup-space.hh:93
LiegroupElementBase()
Constructor of trivial element.
Definition: liegroup-element.hh:166
const LiegroupSpacePtr_t & space() const
get reference to vector of Lie groups
Definition: liegroup-element.hh:64
LiegroupElementConstBase< vector_type > Base
Definition: liegroup-element.hh:125
vector_t operator-(const LiegroupElementConstBase< vector_type1 > &e1, const LiegroupElementConstBase< vector_type2 > &e2)
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > vector_t
Definition: fwd.hh:75
vector_type value_
Definition: liegroup-element.hh:111
LiegroupSpacePtr_t space_
Definition: liegroup-element.hh:112
Definition: collision-object.hh:31
LiegroupElementBase(const LiegroupElementBase< vector_type2 > &other)
Definition: liegroup-element.hh:157
LiegroupElementConstBase(const LiegroupElementConstBase< vector_type2 > &other)
Definition: liegroup-element.hh:58
Eigen::Ref< const vector_t > vectorIn_t
Definition: fwd.hh:80
LiegroupElementBase(const vector_type &value, const LiegroupSpacePtr_t &space)
Definition: liegroup-element.hh:130
void setNeutral()
Set element to neutral element.
Definition: liegroup-element.hh:181
void check() const
Definition: liegroup-element.hh:83
LiegroupElementBase(const LiegroupElementConstBase< vector_type2 > &other)
Definition: liegroup-element.hh:149
LiegroupElementBase(const vector_type &value)
Definition: liegroup-element.hh:143
vector_type & vector()
Modifiable vector representation.
Definition: liegroup-element.hh:175
shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:136