1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2020 CNRS INRIA |
3 |
|
|
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
4 |
|
|
// |
5 |
|
|
|
6 |
|
|
#ifndef __pinocchio_se3_base_hpp__ |
7 |
|
|
#define __pinocchio_se3_base_hpp__ |
8 |
|
|
|
9 |
|
|
namespace pinocchio |
10 |
|
|
{ |
11 |
|
|
/** \brief Base class for rigid transformation. |
12 |
|
|
* |
13 |
|
|
* The rigid transform aMb can be seen in two ways: |
14 |
|
|
* |
15 |
|
|
* - given a point p expressed in frame B by its coordinate vector \f$ ^bp \f$, \f$ ^aM_b \f$ |
16 |
|
|
* computes its coordinates in frame A by \f$ ^ap = {}^aM_b {}^bp \f$. |
17 |
|
|
* - \f$ ^aM_b \f$ displaces a solid S centered at frame A into the solid centered in |
18 |
|
|
* B. In particular, the origin of A is displaced at the origin of B: |
19 |
|
|
* \f$^aM_b {}^aA = {}^aB \f$. |
20 |
|
|
|
21 |
|
|
* The rigid displacement is stored as a rotation matrix and translation vector by: |
22 |
|
|
* \f$ ^aM_b x = {}^aR_b x + {}^aAB \f$ |
23 |
|
|
* where \f$^aAB\f$ is the vector from origin A to origin B expressed in coordinates A. |
24 |
|
|
* |
25 |
|
|
* \cheatsheet \f$ {}^aM_c = {}^aM_b {}^bM_c \f$ |
26 |
|
|
* |
27 |
|
|
* \ingroup pinocchio_spatial |
28 |
|
|
*/ |
29 |
|
|
template<class Derived> |
30 |
|
|
struct SE3Base |
31 |
|
|
{ |
32 |
|
|
PINOCCHIO_SE3_TYPEDEF_TPL(Derived); |
33 |
|
|
|
34 |
|
717611 |
Derived & derived() { return *static_cast<Derived*>(this); } |
35 |
|
8872374 |
const Derived& derived() const { return *static_cast<const Derived*>(this); } |
36 |
|
|
|
37 |
|
3770010 |
ConstAngularRef rotation() const { return derived().rotation_impl(); } |
38 |
|
3296472 |
ConstLinearRef translation() const { return derived().translation_impl(); } |
39 |
|
568481 |
AngularRef rotation() { return derived().rotation_impl(); } |
40 |
|
97551 |
LinearRef translation() { return derived().translation_impl(); } |
41 |
|
29023 |
void rotation(const AngularType & R) { derived().rotation_impl(R); } |
42 |
|
22556 |
void translation(const LinearType & t) { derived().translation_impl(t); } |
43 |
|
|
|
44 |
|
5 |
HomogeneousMatrixType toHomogeneousMatrix() const |
45 |
|
|
{ |
46 |
|
5 |
return derived().toHomogeneousMatrix_impl(); |
47 |
|
|
} |
48 |
|
4 |
operator HomogeneousMatrixType() const { return toHomogeneousMatrix(); } |
49 |
|
|
|
50 |
|
|
/** |
51 |
|
|
* @brief The action matrix \f$ {}^aX_b \f$ of \f$ {}^aM_b \f$. |
52 |
|
|
* |
53 |
|
|
* With \f$ {}^aM_b = \left( \begin{array}{cc} R & t \\ 0 & 1 \\ \end{array} \right) \f$, |
54 |
|
|
* \f[ |
55 |
|
|
* {}^aX_b = \left( \begin{array}{cc} R & \hat{t} R \\ 0 & R \\ \end{array} \right) |
56 |
|
|
* \f] |
57 |
|
|
* |
58 |
|
|
* \cheatsheet \f$ {}^a\nu_c = {}^aX_b {}^b\nu_c \f$ |
59 |
|
|
*/ |
60 |
|
3853 |
ActionMatrixType toActionMatrix() const |
61 |
|
|
{ |
62 |
|
3853 |
return derived().toActionMatrix_impl(); |
63 |
|
|
} |
64 |
|
10 |
operator ActionMatrixType() const { return toActionMatrix(); } |
65 |
|
|
|
66 |
|
|
/** |
67 |
|
|
* @brief The action matrix \f$ {}^bX_a \f$ of \f$ {}^aM_b \f$. |
68 |
|
|
* \sa toActionMatrix() |
69 |
|
|
*/ |
70 |
|
15 |
ActionMatrixType toActionMatrixInverse() const |
71 |
|
|
{ |
72 |
|
15 |
return derived().toActionMatrixInverse_impl(); |
73 |
|
|
} |
74 |
|
|
|
75 |
|
88 |
ActionMatrixType toDualActionMatrix() const |
76 |
|
88 |
{ return derived().toDualActionMatrix_impl(); } |
77 |
|
|
|
78 |
|
4 |
void disp(std::ostream & os) const |
79 |
|
|
{ |
80 |
|
4 |
static_cast<const Derived*>(this)->disp_impl(os); |
81 |
|
4 |
} |
82 |
|
|
|
83 |
|
|
typename SE3GroupAction<Derived>::ReturnType |
84 |
|
1413723 |
operator*(const Derived & m2) const |
85 |
|
1413723 |
{ return derived().__mult__(m2); } |
86 |
|
|
|
87 |
|
|
/// ay = aXb.act(by) |
88 |
|
|
template<typename D> |
89 |
|
|
typename SE3GroupAction<D>::ReturnType |
90 |
|
287243 |
act(const D & d) const |
91 |
|
|
{ |
92 |
|
287243 |
return derived().act_impl(d); |
93 |
|
|
} |
94 |
|
|
|
95 |
|
|
/// by = aXb.actInv(ay) |
96 |
|
|
template<typename D> typename SE3GroupAction<D>::ReturnType |
97 |
|
172179 |
actInv(const D & d) const |
98 |
|
|
{ |
99 |
|
172179 |
return derived().actInv_impl(d); |
100 |
|
|
} |
101 |
|
|
|
102 |
|
13365 |
bool operator==(const Derived & other) const |
103 |
|
13365 |
{ return derived().isEqual(other); } |
104 |
|
|
|
105 |
|
|
bool operator!=(const Derived & other) const |
106 |
|
|
{ return !(*this == other); } |
107 |
|
|
|
108 |
|
55775 |
bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
109 |
|
|
{ |
110 |
|
55775 |
return derived().isApprox_impl(other, prec); |
111 |
|
|
} |
112 |
|
|
|
113 |
|
4 |
friend std::ostream & operator <<(std::ostream & os,const SE3Base<Derived> & X) |
114 |
|
|
{ |
115 |
|
4 |
X.disp(os); |
116 |
|
4 |
return os; |
117 |
|
|
} |
118 |
|
|
|
119 |
|
|
/// |
120 |
|
|
/// \returns true if *this is approximately equal to the identity placement, within the precision given by prec. |
121 |
|
|
/// |
122 |
|
|
bool isIdentity(const typename traits<Derived>::Scalar & prec = Eigen::NumTraits<typename traits<Derived>::Scalar>::dummy_precision()) const |
123 |
|
|
{ |
124 |
|
|
return derived().isIdentity(prec); |
125 |
|
|
} |
126 |
|
|
|
127 |
|
|
/// |
128 |
|
|
/// \returns true if the rotational part of *this is a rotation matrix (normalized columns), within the precision given by prec. |
129 |
|
|
/// |
130 |
|
|
bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
131 |
|
|
{ |
132 |
|
|
return derived().isNormalized(prec); |
133 |
|
|
} |
134 |
|
|
|
135 |
|
|
/// |
136 |
|
|
/// \brief Normalize *this in such a way the rotation part of *this lies on SO(3). |
137 |
|
|
/// |
138 |
|
|
void normalize() |
139 |
|
|
{ |
140 |
|
|
derived().normalize(); |
141 |
|
|
} |
142 |
|
|
|
143 |
|
|
/// |
144 |
|
|
/// \returns a Normalized version of *this, in such a way the rotation part of the returned transformation lies on SO(3). |
145 |
|
|
/// |
146 |
|
|
PlainType normalized() const |
147 |
|
|
{ |
148 |
|
|
derived().normalized(); |
149 |
|
|
} |
150 |
|
|
|
151 |
|
|
}; // struct SE3Base |
152 |
|
|
|
153 |
|
|
} // namespace pinocchio |
154 |
|
|
|
155 |
|
|
#endif // ifndef __pinocchio_se3_base_hpp__ |