1 |
|
|
// |
2 |
|
|
// Copyright (c) 2018 CNRS |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_lie_group_generic_hpp__ |
6 |
|
|
#define __pinocchio_lie_group_generic_hpp__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/multibody/liegroup/liegroup-base.hpp" |
9 |
|
|
#include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hpp" |
10 |
|
|
|
11 |
|
|
namespace pinocchio |
12 |
|
|
{ |
13 |
|
|
template<typename LieGroupCollection> struct LieGroupGenericTpl; |
14 |
|
|
|
15 |
|
|
template<typename LieGroupCollection> |
16 |
|
|
struct traits< LieGroupGenericTpl<LieGroupCollection> > |
17 |
|
|
{ |
18 |
|
|
typedef typename LieGroupCollection::Scalar Scalar; |
19 |
|
|
enum { |
20 |
|
|
Options = LieGroupCollection::Options, |
21 |
|
|
NQ = Eigen::Dynamic, |
22 |
|
|
NV = Eigen::Dynamic |
23 |
|
|
}; |
24 |
|
|
}; |
25 |
|
|
|
26 |
|
|
template<typename LieGroupCollection> |
27 |
|
|
struct LieGroupGenericTpl |
28 |
|
|
: LieGroupBase< LieGroupGenericTpl<LieGroupCollection> >, LieGroupCollection::LieGroupVariant |
29 |
|
|
{ |
30 |
|
|
typedef typename LieGroupCollection::LieGroupVariant Base; |
31 |
|
|
typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant; |
32 |
|
|
|
33 |
|
|
typedef typename LieGroupCollection::Scalar Scalar; |
34 |
|
|
enum { Options = LieGroupCollection::Options }; |
35 |
|
|
|
36 |
|
|
template<typename LieGroupDerived> |
37 |
|
60 |
LieGroupGenericTpl(const LieGroupBase<LieGroupDerived> & lg_base) |
38 |
|
60 |
: Base(lg_base.derived()) |
39 |
|
60 |
{} |
40 |
|
|
|
41 |
|
|
template<typename LieGroup> |
42 |
|
|
LieGroupGenericTpl(const LieGroupVariant & lg_variant) |
43 |
|
|
: Base(lg_variant) |
44 |
|
|
{} |
45 |
|
|
|
46 |
|
22 |
LieGroupGenericTpl(const LieGroupGenericTpl & lg_generic) |
47 |
|
22 |
: Base(lg_generic) |
48 |
|
22 |
{} |
49 |
|
|
|
50 |
|
|
LieGroupGenericTpl & operator=(const LieGroupGenericTpl & other) |
51 |
|
|
{ |
52 |
|
|
static_cast<Base&>(*this) = other.toVariant(); |
53 |
|
|
return *this; |
54 |
|
|
} |
55 |
|
|
|
56 |
|
24 |
const LieGroupVariant & toVariant() const |
57 |
|
24 |
{ return static_cast<const LieGroupVariant &>(*this); } |
58 |
|
|
|
59 |
|
|
LieGroupVariant & toVariant() |
60 |
|
|
{ return static_cast<LieGroupVariant &>(*this); } |
61 |
|
|
|
62 |
|
12 |
bool isEqual_impl (const LieGroupGenericTpl& other) const |
63 |
|
|
{ |
64 |
✓✗ |
12 |
return boost::apply_visitor(visitor::LieGroupEqual<Scalar, Options>(), toVariant(), other.toVariant()); |
65 |
|
|
} |
66 |
|
|
|
67 |
|
|
int nq() const { return ::pinocchio::nq(*this); } |
68 |
|
|
int nv() const { return ::pinocchio::nv(*this); } |
69 |
|
|
|
70 |
|
1 |
bool operator== (const LieGroupGenericTpl& other) const |
71 |
|
|
{ |
72 |
|
1 |
return isEqual_impl(other); |
73 |
|
|
} |
74 |
|
|
|
75 |
|
1 |
bool operator!= (const LieGroupGenericTpl& other) const |
76 |
|
|
{ |
77 |
|
1 |
return this->isDifferent_impl(other); |
78 |
|
|
} |
79 |
|
|
|
80 |
|
|
std::string name() const |
81 |
|
|
{ |
82 |
|
|
return LieGroupNameVisitor::run(*this); |
83 |
|
|
} |
84 |
|
|
}; |
85 |
|
|
|
86 |
|
|
} |
87 |
|
|
|
88 |
|
|
#endif // ifndef __pinocchio_lie_group_generic_hpp__ |
89 |
|
|
|