hpp-pinocchio  4.10.1
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
liegroup-space.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_SPACE_HH
18 # define HPP_PINOCCHIO_LIEGROUP_SPACE_HH
19 
20 # include <vector>
21 # include <string>
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>
28 # include <hpp/pinocchio/liegroup.hh>
29 # include <hpp/pinocchio/fwd.hh>
30 
31 namespace hpp {
32  namespace pinocchio {
35 
36 #ifdef HPP_PINOCCHIO_PARSED_BY_DOXYGEN
37  typedef ABoostVariant LiegroupType;
48 #else
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> >
67 #endif
68 
72  };
73 
94  {
95  public:
96  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
99 
102  static LiegroupSpacePtr_t Rn (const size_type& n);
106  static LiegroupSpacePtr_t R1 (bool rotation=false);
108  static LiegroupSpacePtr_t R2 ();
110  static LiegroupSpacePtr_t R3 ();
112  static LiegroupSpacePtr_t SE2 ();
114  static LiegroupSpacePtr_t SE3 ();
116  static LiegroupSpacePtr_t SO2 ();
118  static LiegroupSpacePtr_t SO3 ();
120  static LiegroupSpacePtr_t R2xSO2 ();
122  static LiegroupSpacePtr_t R3xSO3 ();
124  static LiegroupSpacePtr_t empty ();
126 
128  static LiegroupSpacePtr_t create (const size_type& size)
129  {
130  LiegroupSpace* ptr (new LiegroupSpace (size));
131  LiegroupSpacePtr_t shPtr (ptr);
132  ptr->init (shPtr);
133  return shPtr;
134  }
135 
138  (const LiegroupSpaceConstPtr_t& other)
139  {
140  LiegroupSpace* ptr (new LiegroupSpace (*other));
141  LiegroupSpacePtr_t shPtr (ptr);
142  ptr->init (shPtr);
143  return shPtr;
144  }
145 
147  static LiegroupSpacePtr_t create (const LiegroupType& type)
148  {
149  LiegroupSpace* ptr (new LiegroupSpace (type));
150  LiegroupSpacePtr_t shPtr (ptr);
151  ptr->init (shPtr);
152  return shPtr;
153  }
154 
156  size_type nq () const
157  {
158  return nq_;
159  }
161  size_type nv () const
162  {
163  return nv_;
164  }
166  size_type nq (const std::size_t& rank) const;
168  size_type nv (const std::size_t& rank) const;
169 
171  const std::vector <LiegroupType>& liegroupTypes () const
172  {
173  return liegroupTypes_;
174  }
175 
177  LiegroupElement neutral () const;
178 
181 
184 
187 
189  LiegroupElement exp (vectorIn_t v) const;
190 
211  template <DerivativeProduct side>
213 
233  template <DerivativeProduct side>
235 
237  template <bool ApplyOnTheLeft>
238  void Jdifference (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const;
239 
249  template <DerivativeProduct side>
250  void dDifference_dq0 (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0) const;
251 
261  template <DerivativeProduct side>
262  void dDifference_dq1 (vectorIn_t q0, vectorIn_t q1, matrixOut_t J1) const;
263 
273  vectorOut_t result) const;
274 
276  std::string name () const;
277 
278  void mergeVectorSpaces ();
279 
281 
282  bool isVectorSpace () const;
283 
284  bool operator== (const LiegroupSpace& other) const;
285  bool operator!= (const LiegroupSpace& other) const;
286 
288 
289  protected:
290 
292  LiegroupSpace (const size_type& size);
293  LiegroupSpace (const LiegroupSpace& other);
294  LiegroupSpace (const LiegroupType& type);
295 
296  private:
298  LiegroupSpace ();
300  void init (const LiegroupSpaceWkPtr_t weak);
302  void computeSize ();
304  void computeNeutral ();
305  typedef std::vector <LiegroupType> LiegroupTypes;
306  LiegroupTypes liegroupTypes_;
308  size_type nq_, nv_;
310  vector_t neutral_;
312  LiegroupSpaceWkPtr_t weak_;
313 
314  HPP_SERIALIZABLE();
315  }; // class LiegroupSpace
317  inline std::ostream& operator<< (std::ostream& os,
318  const LiegroupSpace& space)
319  {
320  os << space.name (); return os;
321  }
322 
324  } // namespace pinocchio
325 } // namespace hpp
326 
327 namespace boost {
330 
340 } // namespace boost
341 
342 #endif // HPP_PINOCCHIO_LIEGROUP_SPACE_HH
static LiegroupSpacePtr_t R1(bool rotation=false)
void Jdifference(vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const
static LiegroupSpacePtr_t SE3()
Return .
static LiegroupSpacePtr_t R2()
Return as a Lie group.
LiegroupSpacePtr_t vectorSpacesMerged() const
static LiegroupSpacePtr_t Rn(const size_type &n)
Definition: serialization.hh:25
static LiegroupSpacePtr_t SO3()
Return .
Utility functions.
Definition: body.hh:30
static LiegroupSpacePtr_t R3()
Return as a Lie group.
Eigen::Ref< vector_t > vectorOut_t
Definition: fwd.hh:81
LiegroupElement neutral() const
Return the neutral element as a vector.
LiegroupElementConstRef elementConstRef(vectorIn_t q) const
Create a LiegroupElementRef from a configuration.
static LiegroupSpacePtr_t SE2()
Return .
ABoostVariant LiegroupType
Definition: liegroup-space.hh:47
bool operator!=(const LiegroupSpace &other) const
void dIntegrate_dv(LiegroupElementConstRef q, vectorIn_t v, matrixOut_t Jv) const
static LiegroupSpacePtr_t empty()
Return empty Lie group.
matrix_t::Index size_type
Definition: fwd.hh:84
size_type nv() const
Dimension of the Lie group tangent space.
Definition: liegroup-space.hh:161
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:83
Definition: liegroup-space.hh:93
size_type nq() const
Dimension of the vector representation.
Definition: liegroup-space.hh:156
Definition: liegroup-space.hh:71
bool operator==(const LiegroupSpace &other) const
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:353
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > vector_t
Definition: fwd.hh:75
static LiegroupSpacePtr_t createCopy(const LiegroupSpaceConstPtr_t &other)
Create copy.
Definition: liegroup-space.hh:138
DerivativeProduct
Definition: liegroup-space.hh:69
boost::shared_ptr< const LiegroupSpace > LiegroupSpaceConstPtr_t
Definition: fwd.hh:137
static LiegroupSpacePtr_t create(const LiegroupType &type)
Create instance with one Elementary Lie group.
Definition: liegroup-space.hh:147
LiegroupElement element(vectorIn_t q) const
Create a LiegroupElement from a configuration.
static LiegroupSpacePtr_t R2xSO2()
Return .
LiegroupElementRef elementRef(vectorOut_t q) const
Create a LiegroupElementRef from a configuration.
double value_type
Definition: fwd.hh:40
Definition: collision-object.hh:31
static LiegroupSpacePtr_t R3xSO3()
Return .
LiegroupSpacePtr_t operator*=(const LiegroupSpaceConstPtr_t &other)
Eigen::Ref< const vector_t > vectorIn_t
Definition: fwd.hh:80
LiegroupElement exp(vectorIn_t v) const
Return exponential of a tangent vector.
void dIntegrate_dq(LiegroupElementConstRef q, vectorIn_t v, matrixOut_t Jq) const
void dDifference_dq0(vectorIn_t q0, vectorIn_t q1, matrixOut_t J0) const
static LiegroupSpacePtr_t SO2()
Return .
boost::shared_ptr< LiegroupSpace > LiegroupSpacePtr_t
Definition: fwd.hh:136
Definition: liegroup-space.hh:70
void dDifference_dq1(vectorIn_t q0, vectorIn_t q1, matrixOut_t J1) const
void interpolate(vectorIn_t q0, vectorIn_t q1, value_type u, vectorOut_t result) const
std::string name() const
Return name of Lie group.
hpp::pinocchio::LiegroupSpacePtr_t operator*(const hpp::pinocchio::LiegroupSpaceConstPtr_t &sp1, const hpp::pinocchio::LiegroupSpaceConstPtr_t &sp2)
Cartesian product between Lie groups.
static LiegroupSpacePtr_t create(const size_type &size)
Create instance of vector space of given size.
Definition: liegroup-space.hh:128
const std::vector< LiegroupType > & liegroupTypes() const
Get reference to vector of elementary types.
Definition: liegroup-space.hh:171