Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2021 CNRS INRIA |
3 |
|
|
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
4 |
|
|
// |
5 |
|
|
|
6 |
|
|
#ifndef __pinocchio_spatial_se3_base_hpp__ |
7 |
|
|
#define __pinocchio_spatial_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 : NumericalBase<Derived> |
31 |
|
|
{ |
32 |
|
|
PINOCCHIO_SE3_TYPEDEF_TPL(Derived); |
33 |
|
|
|
34 |
|
94750 |
Derived & derived() |
35 |
|
|
{ |
36 |
|
94750 |
return *static_cast<Derived *>(this); |
37 |
|
|
} |
38 |
|
2115337 |
const Derived & derived() const |
39 |
|
|
{ |
40 |
|
2115337 |
return *static_cast<const Derived *>(this); |
41 |
|
|
} |
42 |
|
|
|
43 |
|
|
Derived & const_cast_derived() const |
44 |
|
|
{ |
45 |
|
|
return *const_cast<Derived *>(&derived()); |
46 |
|
|
} |
47 |
|
|
|
48 |
|
941040 |
ConstAngularRef rotation() const |
49 |
|
|
{ |
50 |
|
941040 |
return derived().rotation_impl(); |
51 |
|
|
} |
52 |
|
743558 |
ConstLinearRef translation() const |
53 |
|
|
{ |
54 |
|
743558 |
return derived().translation_impl(); |
55 |
|
|
} |
56 |
|
80039 |
AngularRef rotation() |
57 |
|
|
{ |
58 |
|
80039 |
return derived().rotation_impl(); |
59 |
|
|
} |
60 |
|
13888 |
LinearRef translation() |
61 |
|
|
{ |
62 |
|
13888 |
return derived().translation_impl(); |
63 |
|
|
} |
64 |
|
21351 |
void rotation(const AngularType & R) |
65 |
|
|
{ |
66 |
|
21351 |
derived().rotation_impl(R); |
67 |
|
21351 |
} |
68 |
|
21241 |
void translation(const LinearType & t) |
69 |
|
|
{ |
70 |
|
21241 |
derived().translation_impl(t); |
71 |
|
21241 |
} |
72 |
|
|
|
73 |
|
✗ |
HomogeneousMatrixType toHomogeneousMatrix() const |
74 |
|
|
{ |
75 |
|
✗ |
return derived().toHomogeneousMatrix_impl(); |
76 |
|
|
} |
77 |
|
✗ |
operator HomogeneousMatrixType() const |
78 |
|
|
{ |
79 |
|
✗ |
return toHomogeneousMatrix(); |
80 |
|
|
} |
81 |
|
|
|
82 |
|
|
/** |
83 |
|
|
* @brief The action matrix \f$ {}^aX_b \f$ of \f$ {}^aM_b \f$. |
84 |
|
|
* |
85 |
|
|
* With \f$ {}^aM_b = \left( \begin{array}{cc} R & t \\ 0 & 1 \\ \end{array} \right) \f$, |
86 |
|
|
* \f[ |
87 |
|
|
* {}^aX_b = \left( \begin{array}{cc} R & \hat{t} R \\ 0 & R \\ \end{array} \right) |
88 |
|
|
* \f] |
89 |
|
|
* |
90 |
|
|
* \cheatsheet \f$ {}^a\nu_c = {}^aX_b {}^b\nu_c \f$ |
91 |
|
|
*/ |
92 |
|
5468 |
ActionMatrixType toActionMatrix() const |
93 |
|
|
{ |
94 |
|
5468 |
return derived().toActionMatrix_impl(); |
95 |
|
|
} |
96 |
|
✗ |
operator ActionMatrixType() const |
97 |
|
|
{ |
98 |
|
✗ |
return toActionMatrix(); |
99 |
|
|
} |
100 |
|
|
|
101 |
|
|
template<typename Matrix6Like> |
102 |
|
|
void toActionMatrix(const Eigen::MatrixBase<Matrix6Like> & action_matrix) const |
103 |
|
|
{ |
104 |
|
|
derived().toActionMatrix_impl(action_matrix); |
105 |
|
|
} |
106 |
|
|
|
107 |
|
|
/** |
108 |
|
|
* @brief The action matrix \f$ {}^bX_a \f$ of \f$ {}^aM_b \f$. |
109 |
|
|
* \sa toActionMatrix() |
110 |
|
|
*/ |
111 |
|
23 |
ActionMatrixType toActionMatrixInverse() const |
112 |
|
|
{ |
113 |
|
23 |
return derived().toActionMatrixInverse_impl(); |
114 |
|
|
} |
115 |
|
|
|
116 |
|
|
template<typename Matrix6Like> |
117 |
|
✗ |
void toActionMatrixInverse(const Eigen::MatrixBase<Matrix6Like> & action_matrix_inverse) const |
118 |
|
|
{ |
119 |
|
✗ |
derived().toActionMatrixInverse_impl(action_matrix_inverse.const_cast_derived()); |
120 |
|
|
} |
121 |
|
|
|
122 |
|
71 |
ActionMatrixType toDualActionMatrix() const |
123 |
|
|
{ |
124 |
|
71 |
return derived().toDualActionMatrix_impl(); |
125 |
|
|
} |
126 |
|
|
|
127 |
|
6 |
void disp(std::ostream & os) const |
128 |
|
|
{ |
129 |
|
6 |
static_cast<const Derived *>(this)->disp_impl(os); |
130 |
|
6 |
} |
131 |
|
|
|
132 |
|
|
template<typename Matrix6Like> |
133 |
|
|
void toDualActionMatrix(const Eigen::MatrixBase<Matrix6Like> & dual_action_matrix) const |
134 |
|
|
{ |
135 |
|
|
derived().toDualActionMatrix_impl(dual_action_matrix); |
136 |
|
|
} |
137 |
|
|
|
138 |
|
180081 |
typename SE3GroupAction<Derived>::ReturnType operator*(const Derived & m2) const |
139 |
|
|
{ |
140 |
|
180081 |
return derived().__mult__(m2); |
141 |
|
|
} |
142 |
|
|
|
143 |
|
|
/// ay = aXb.act(by) |
144 |
|
|
template<typename D> |
145 |
|
466807 |
typename SE3GroupAction<D>::ReturnType act(const D & d) const |
146 |
|
|
{ |
147 |
|
466807 |
return derived().act_impl(d); |
148 |
|
|
} |
149 |
|
|
|
150 |
|
|
/// by = aXb.actInv(ay) |
151 |
|
|
template<typename D> |
152 |
|
154226 |
typename SE3GroupAction<D>::ReturnType actInv(const D & d) const |
153 |
|
|
{ |
154 |
|
154226 |
return derived().actInv_impl(d); |
155 |
|
|
} |
156 |
|
|
|
157 |
|
98 |
bool operator==(const Derived & other) const |
158 |
|
|
{ |
159 |
|
98 |
return derived().isEqual(other); |
160 |
|
|
} |
161 |
|
|
|
162 |
|
✗ |
bool operator!=(const Derived & other) const |
163 |
|
|
{ |
164 |
|
✗ |
return !(*this == other); |
165 |
|
|
} |
166 |
|
|
|
167 |
|
336 |
bool isApprox( |
168 |
|
|
const Derived & other, |
169 |
|
|
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
170 |
|
|
{ |
171 |
|
336 |
return derived().isApprox_impl(other, prec); |
172 |
|
|
} |
173 |
|
|
|
174 |
|
6 |
friend std::ostream & operator<<(std::ostream & os, const SE3Base<Derived> & X) |
175 |
|
|
{ |
176 |
|
6 |
X.disp(os); |
177 |
|
6 |
return os; |
178 |
|
|
} |
179 |
|
|
|
180 |
|
|
/// |
181 |
|
|
/// \returns true if *this is approximately equal to the identity placement, within the |
182 |
|
|
/// precision given by prec. |
183 |
|
|
/// |
184 |
|
|
bool isIdentity( |
185 |
|
|
const typename traits<Derived>::Scalar & prec = |
186 |
|
|
Eigen::NumTraits<typename traits<Derived>::Scalar>::dummy_precision()) const |
187 |
|
|
{ |
188 |
|
|
return derived().isIdentity(prec); |
189 |
|
|
} |
190 |
|
|
|
191 |
|
|
/// |
192 |
|
|
/// \returns true if the rotational part of *this is a rotation matrix (normalized columns), |
193 |
|
|
/// within the precision given by prec. |
194 |
|
|
/// |
195 |
|
|
bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
196 |
|
|
{ |
197 |
|
|
return derived().isNormalized(prec); |
198 |
|
|
} |
199 |
|
|
|
200 |
|
|
/// |
201 |
|
|
/// \brief Normalize *this in such a way the rotation part of *this lies on SO(3). |
202 |
|
|
/// |
203 |
|
|
void normalize() |
204 |
|
|
{ |
205 |
|
|
derived().normalize(); |
206 |
|
|
} |
207 |
|
|
|
208 |
|
|
/// |
209 |
|
|
/// \returns a Normalized version of *this, in such a way the rotation part of the returned |
210 |
|
|
/// transformation lies on SO(3). |
211 |
|
|
/// |
212 |
|
|
PlainType normalized() const |
213 |
|
|
{ |
214 |
|
|
derived().normalized(); |
215 |
|
|
} |
216 |
|
|
|
217 |
|
|
}; // struct SE3Base |
218 |
|
|
|
219 |
|
|
} // namespace pinocchio |
220 |
|
|
|
221 |
|
|
#endif // ifndef __pinocchio_spatial_se3_base_hpp__ |
222 |
|
|
|