hpp-pinocchio  4.13.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 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28 
29 #ifndef HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH
30 #define HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH
31 
34 
35 namespace hpp {
36 namespace pinocchio {
39 
43 template <typename vector_type>
44 class LiegroupElementConstBase {
45  public:
49  template <typename Derived>
50  LiegroupElementConstBase(const Eigen::EigenBase<Derived>& value,
51  const LiegroupSpacePtr_t& liegroupSpace)
52  : value_(value.derived()), space_(liegroupSpace) {
53  check();
54  }
55 
60  template <typename Derived>
61  explicit LiegroupElementConstBase(const Eigen::EigenBase<Derived>& value)
62  : value_(value.derived()),
63  space_(LiegroupSpace::create(value.derived().size())) {}
64 
67  template <typename vector_type2>
69  : value_(other.vector()), space_(other.space()) {}
70 
72  const LiegroupSpacePtr_t& space() const { return space_; }
73 
75  const vector_type& vector() const { return value_; }
76 
78  size_type size() const { return value_.size(); }
79 
82  void check() const { assert(value_.size() == space_->nq()); }
83 
85  template <typename vector_type2>
87  return ((*space_ == *(other.space_)) && (value_ == other.value_));
88  }
89 
91  template <typename vector_type2>
93  return ((*space_ != *(other.space_)) || (value_ != other.value_));
94  }
95 
96  protected:
97  template <typename Derived>
98  LiegroupElementConstBase(const Eigen::EigenBase<Derived>& value,
100  void* /*to enable this constructor*/)
101  : value_(const_cast<Derived&>(value.derived())), space_(space) {}
102 
103  vector_type value_;
105 
106  template <typename vector_type2>
108  template <typename vector_type2>
109  friend class LiegroupElementBase;
110 };
111 
115 template <typename vector_type>
116 class LiegroupElementBase : public LiegroupElementConstBase<vector_type> {
117  public:
119 
123  LiegroupElementBase(const vector_type& value, const LiegroupSpacePtr_t& space)
124  : Base(value, space, NULL) {}
125 
129  : Base(vector_t(space->nq()), space) {}
130 
135  explicit LiegroupElementBase(const vector_type& value)
136  : Base(value, LiegroupSpace::create(value.size()), NULL) {}
137 
140  template <typename vector_type2>
142  : Base(other.value_, other.space()) {}
143 
148  template <typename vector_type2>
150  : Base(other.value_, other.space()) {}
151 
153  template <typename vector_type2>
155  : Base(other.value_, other.space(), NULL) {}
156 
158  LiegroupElementBase() : Base(vector_t(), LiegroupSpace::empty()) {}
159 
161  const vector_type& vector() const { return Base::vector(); }
162 
164  vector_type& vector() { return this->value_; }
165 
167  void setNeutral() { this->value_ = this->space_->neutral().vector(); }
168 
170  LiegroupElementBase& operator+=(vectorIn_t v);
171 
173  template <typename vector_type2>
176  this->space_ = other.space();
177  this->value_ = other.vector();
178  return *this;
179  }
180 
182  template <typename Vector>
183  LiegroupElementBase& operator=(const Eigen::MatrixBase<Vector>& v) {
184  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector);
185  assert(this->space_->nq() == v.derived().size());
186  this->value_.noalias() = v.derived();
187  return *this;
188  }
189 };
190 
199 template <typename vector_type>
201  vectorIn_t v);
202 
210 template <typename vector_type1, typename vector_type2>
214 
216 template <typename vector_type>
218 
219 template <typename vector_type>
220 inline std::ostream& operator<<(
221  std::ostream& os, const LiegroupElementConstBase<vector_type>& e) {
222  os << "Lie group element in " << *(e.space()) << " represented by vector ("
223  << e.vector().transpose() << ")";
224  return os;
225 }
226 } // namespace pinocchio
227 } // namespace hpp
228 
229 HPP_SERIALIZABLE_SPLIT_FREE(hpp::pinocchio::LiegroupElement)
230 
231 #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:75
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:154
bool operator!=(const LiegroupElementConstBase< vector_type2 > &other)
Equality operator.
Definition: liegroup-element.hh:92
const vector_type & vector() const
Const vector representation.
Definition: liegroup-element.hh:161
bool operator==(const LiegroupElementConstBase< vector_type2 > &other)
Equality operator.
Definition: liegroup-element.hh:86
size_type size() const
Size of the vector representation.
Definition: liegroup-element.hh:78
Utility functions.
Definition: body.hh:39
LiegroupElementConstBase(const Eigen::EigenBase< Derived > &value, const LiegroupSpacePtr_t &liegroupSpace)
Definition: liegroup-element.hh:50
LiegroupElementConstBase(const Eigen::EigenBase< Derived > &value)
Definition: liegroup-element.hh:61
LiegroupElementBase & operator=(const Eigen::MatrixBase< Vector > &v)
Assignment from a vector.
Definition: liegroup-element.hh:183
LiegroupElementBase(const LiegroupSpacePtr_t &space)
Definition: liegroup-element.hh:128
LiegroupElementConstBase(const Eigen::EigenBase< Derived > &value, const LiegroupSpacePtr_t &space, void *)
Definition: liegroup-element.hh:98
matrix_t::Index size_type
Definition: fwd.hh:96
Definition: liegroup-space.hh:101
LiegroupElementBase()
Constructor of trivial element.
Definition: liegroup-element.hh:158
const LiegroupSpacePtr_t & space() const
get reference to vector of Lie groups
Definition: liegroup-element.hh:72
LiegroupElementConstBase< vector_type > Base
Definition: liegroup-element.hh:118
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:364
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > vector_t
Definition: fwd.hh:87
vector_type value_
Definition: liegroup-element.hh:103
LiegroupSpacePtr_t space_
Definition: liegroup-element.hh:104
Definition: collision-object.hh:40
LiegroupElementBase(const LiegroupElementBase< vector_type2 > &other)
Definition: liegroup-element.hh:149
LiegroupElementBase & operator=(const LiegroupElementConstBase< vector_type2 > &other)
Assignment from another LiegroupElement.
Definition: liegroup-element.hh:174
LiegroupElementConstBase(const LiegroupElementConstBase< vector_type2 > &other)
Definition: liegroup-element.hh:68
Eigen::Ref< const vector_t > vectorIn_t
Definition: fwd.hh:92
LiegroupElementBase(const vector_type &value, const LiegroupSpacePtr_t &space)
Definition: liegroup-element.hh:123
void setNeutral()
Set element to neutral element.
Definition: liegroup-element.hh:167
void check() const
Definition: liegroup-element.hh:82
LiegroupElementBase(const LiegroupElementConstBase< vector_type2 > &other)
Definition: liegroup-element.hh:141
LiegroupElementBase(const vector_type &value)
Definition: liegroup-element.hh:135
vector_type & vector()
Modifiable vector representation.
Definition: liegroup-element.hh:164
shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:150