Line |
Branch |
Exec |
Source |
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> |
14 |
|
|
struct LieGroupGenericTpl; |
15 |
|
|
|
16 |
|
|
template<typename LieGroupCollection> |
17 |
|
|
struct traits<LieGroupGenericTpl<LieGroupCollection>> |
18 |
|
|
{ |
19 |
|
|
typedef typename LieGroupCollection::Scalar Scalar; |
20 |
|
|
enum |
21 |
|
|
{ |
22 |
|
|
Options = LieGroupCollection::Options, |
23 |
|
|
NQ = Eigen::Dynamic, |
24 |
|
|
NV = Eigen::Dynamic |
25 |
|
|
}; |
26 |
|
|
}; |
27 |
|
|
|
28 |
|
|
template<typename LieGroupCollection> |
29 |
|
|
struct LieGroupGenericTpl |
30 |
|
|
: LieGroupBase<LieGroupGenericTpl<LieGroupCollection>> |
31 |
|
|
, LieGroupCollection::LieGroupVariant |
32 |
|
|
{ |
33 |
|
|
typedef typename LieGroupCollection::LieGroupVariant Base; |
34 |
|
|
typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant; |
35 |
|
|
|
36 |
|
|
typedef typename LieGroupCollection::Scalar Scalar; |
37 |
|
|
enum |
38 |
|
|
{ |
39 |
|
|
Options = LieGroupCollection::Options |
40 |
|
|
}; |
41 |
|
|
|
42 |
|
|
template<typename LieGroupDerived> |
43 |
|
✗ |
LieGroupGenericTpl(const LieGroupBase<LieGroupDerived> & lg_base) |
44 |
|
✗ |
: Base(lg_base.derived()) |
45 |
|
|
{ |
46 |
|
|
} |
47 |
|
|
|
48 |
|
|
template<typename LieGroup> |
49 |
|
|
LieGroupGenericTpl(const LieGroupVariant & lg_variant) |
50 |
|
|
: Base(lg_variant) |
51 |
|
|
{ |
52 |
|
|
} |
53 |
|
|
|
54 |
|
✗ |
LieGroupGenericTpl(const LieGroupGenericTpl & lg_generic) = default; |
55 |
|
✗ |
LieGroupGenericTpl & operator=(const LieGroupGenericTpl & other) = default; |
56 |
|
|
|
57 |
|
✗ |
const LieGroupVariant & toVariant() const |
58 |
|
|
{ |
59 |
|
✗ |
return static_cast<const LieGroupVariant &>(*this); |
60 |
|
|
} |
61 |
|
|
|
62 |
|
|
LieGroupVariant & toVariant() |
63 |
|
|
{ |
64 |
|
|
return static_cast<LieGroupVariant &>(*this); |
65 |
|
|
} |
66 |
|
|
|
67 |
|
✗ |
bool isEqual_impl(const LieGroupGenericTpl & other) const |
68 |
|
|
{ |
69 |
|
✗ |
return boost::apply_visitor( |
70 |
|
✗ |
visitor::LieGroupEqual<Scalar, Options>(), toVariant(), other.toVariant()); |
71 |
|
|
} |
72 |
|
|
|
73 |
|
|
int nq() const |
74 |
|
|
{ |
75 |
|
|
return ::pinocchio::nq(*this); |
76 |
|
|
} |
77 |
|
|
int nv() const |
78 |
|
|
{ |
79 |
|
|
return ::pinocchio::nv(*this); |
80 |
|
|
} |
81 |
|
|
|
82 |
|
✗ |
bool operator==(const LieGroupGenericTpl & other) const |
83 |
|
|
{ |
84 |
|
✗ |
return isEqual_impl(other); |
85 |
|
|
} |
86 |
|
|
|
87 |
|
✗ |
bool operator!=(const LieGroupGenericTpl & other) const |
88 |
|
|
{ |
89 |
|
✗ |
return this->isDifferent_impl(other); |
90 |
|
|
} |
91 |
|
|
|
92 |
|
✗ |
std::string name() const |
93 |
|
|
{ |
94 |
|
✗ |
return LieGroupNameVisitor::run(*this); |
95 |
|
|
} |
96 |
|
|
}; |
97 |
|
|
|
98 |
|
|
} // namespace pinocchio |
99 |
|
|
|
100 |
|
|
#endif // ifndef __pinocchio_lie_group_generic_hpp__ |
101 |
|
|
|