Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2016-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__ |
6 |
|
|
#define __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/multibody/liegroup/fwd.hpp" |
9 |
|
|
|
10 |
|
|
#include <limits> |
11 |
|
|
|
12 |
|
|
namespace pinocchio |
13 |
|
|
{ |
14 |
|
|
#ifdef PINOCCHIO_WITH_CXX11_SUPPORT |
15 |
|
|
constexpr int SELF = 0; |
16 |
|
|
#else |
17 |
|
|
enum |
18 |
|
|
{ |
19 |
|
|
SELF = 0 |
20 |
|
|
}; |
21 |
|
|
#endif |
22 |
|
|
|
23 |
|
|
#define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived, TYPENAME) \ |
24 |
|
|
typedef LieGroupBase<Derived> Base; \ |
25 |
|
|
typedef TYPENAME Base::Index Index; \ |
26 |
|
|
typedef TYPENAME traits<Derived>::Scalar Scalar; \ |
27 |
|
|
enum \ |
28 |
|
|
{ \ |
29 |
|
|
Options = traits<Derived>::Options, \ |
30 |
|
|
NQ = Base::NQ, \ |
31 |
|
|
NV = Base::NV \ |
32 |
|
|
}; \ |
33 |
|
|
typedef TYPENAME Base::ConfigVector_t ConfigVector_t; \ |
34 |
|
|
typedef TYPENAME Base::TangentVector_t TangentVector_t; \ |
35 |
|
|
typedef TYPENAME Base::JacobianMatrix_t JacobianMatrix_t |
36 |
|
|
|
37 |
|
|
#define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE(Derived) \ |
38 |
|
|
PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived, PINOCCHIO_MACRO_EMPTY_ARG) |
39 |
|
|
|
40 |
|
|
#define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived) \ |
41 |
|
|
PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived, typename) |
42 |
|
|
|
43 |
|
|
template<typename Derived> |
44 |
|
|
struct LieGroupBase |
45 |
|
|
{ |
46 |
|
|
typedef Derived LieGroupDerived; |
47 |
|
|
typedef int Index; |
48 |
|
|
typedef typename traits<LieGroupDerived>::Scalar Scalar; |
49 |
|
|
enum |
50 |
|
|
{ |
51 |
|
|
Options = traits<LieGroupDerived>::Options, |
52 |
|
|
NQ = traits<LieGroupDerived>::NQ, |
53 |
|
|
NV = traits<LieGroupDerived>::NV |
54 |
|
|
}; |
55 |
|
|
|
56 |
|
|
typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; |
57 |
|
|
typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; |
58 |
|
|
typedef Eigen::Matrix<Scalar, NV, NV, Options> JacobianMatrix_t; |
59 |
|
|
|
60 |
|
|
/// \name API with return value as argument |
61 |
|
|
/// \{ |
62 |
|
|
|
63 |
|
|
/** |
64 |
|
|
* @brief Integrate a joint's configuration with a tangent vector during one unit time |
65 |
|
|
* duration |
66 |
|
|
* |
67 |
|
|
* @param[in] q the initial configuration. |
68 |
|
|
* @param[in] v the tangent velocity. |
69 |
|
|
* |
70 |
|
|
* @param[out] qout the configuration integrated. |
71 |
|
|
*/ |
72 |
|
|
template<class ConfigIn_t, class Tangent_t, class ConfigOut_t> |
73 |
|
|
void integrate( |
74 |
|
|
const Eigen::MatrixBase<ConfigIn_t> & q, |
75 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
76 |
|
|
const Eigen::MatrixBase<ConfigOut_t> & qout) const; |
77 |
|
|
|
78 |
|
|
/** |
79 |
|
|
* @brief Computes the Jacobian of the integrate operator around zero. |
80 |
|
|
* |
81 |
|
|
* @details This function computes the Jacobian of the configuration vector variation |
82 |
|
|
* (component-vise) with respect to a small variation in the tangent. |
83 |
|
|
* |
84 |
|
|
* @param[in] q configuration vector. |
85 |
|
|
* |
86 |
|
|
* @param[out] J the Jacobian of the log of the Integrate operation w.r.t. the configuration |
87 |
|
|
* vector q. |
88 |
|
|
* |
89 |
|
|
* @remarks This function might be useful for instance when using google-ceres to compute the |
90 |
|
|
* Jacobian of the integrate operation. |
91 |
|
|
*/ |
92 |
|
|
template<class Config_t, class Jacobian_t> |
93 |
|
|
void integrateCoeffWiseJacobian( |
94 |
|
|
const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) const; |
95 |
|
|
|
96 |
|
|
/** |
97 |
|
|
* @brief Computes the Jacobian of a small variation of the configuration vector or the |
98 |
|
|
* tangent vector into tangent space at identity. |
99 |
|
|
* |
100 |
|
|
* @details This Jacobian corresponds to the Jacobian of \f$ (\mathbf{q} \oplus \delta |
101 |
|
|
* \mathbf{q}) \oplus \mathbf{v} \f$ with \f$ \delta \mathbf{q} \rightarrow 0 \f$ if arg == ARG0 |
102 |
|
|
* or \f$ \delta \mathbf{v} \rightarrow 0 \f$ if arg == ARG1. |
103 |
|
|
* |
104 |
|
|
* @param[in] q configuration vector. |
105 |
|
|
* @param[in] v tangent vector. |
106 |
|
|
* @param[in] op assignment operator (SETTO, ADDTO or RMTO). |
107 |
|
|
* @tparam arg ARG0 (resp. ARG1) to get the Jacobian with respect to q (resp. v). |
108 |
|
|
* |
109 |
|
|
* @param[out] J the Jacobian of the Integrate operation w.r.t. the argument arg. |
110 |
|
|
*/ |
111 |
|
|
template<ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t> |
112 |
|
✗ |
void dIntegrate( |
113 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
114 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
115 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J, |
116 |
|
|
AssignmentOperatorType op = SETTO) const |
117 |
|
|
{ |
118 |
|
|
PINOCCHIO_STATIC_ASSERT(arg == ARG0 || arg == ARG1, arg_SHOULD_BE_ARG0_OR_ARG1); |
119 |
|
✗ |
return dIntegrate( |
120 |
|
✗ |
q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J), arg, op); |
121 |
|
|
} |
122 |
|
|
|
123 |
|
|
/** |
124 |
|
|
* @brief Computes the Jacobian of a small variation of the configuration vector or the |
125 |
|
|
* tangent vector into tangent space at identity. |
126 |
|
|
* |
127 |
|
|
* @details This Jacobian corresponds to the Jacobian of \f$ (\mathbf{q} \oplus \delta |
128 |
|
|
* \mathbf{q}) \oplus \mathbf{v} \f$ with \f$ \delta \mathbf{q} \rightarrow 0 \f$ if arg == ARG0 |
129 |
|
|
* or \f$ \delta \mathbf{v} \rightarrow 0 \f$ if arg == ARG1. |
130 |
|
|
* |
131 |
|
|
* @param[in] q configuration vector. |
132 |
|
|
* @param[in] v tangent vector. |
133 |
|
|
* @param[in] arg ARG0 (resp. ARG1) to get the Jacobian with respect to q (resp. v). |
134 |
|
|
* @param[in] op assignment operator (SETTO, ADDTO and RMTO). |
135 |
|
|
* |
136 |
|
|
* @param[out] J the Jacobian of the Integrate operation w.r.t. the argument arg. |
137 |
|
|
*/ |
138 |
|
|
template<class Config_t, class Tangent_t, class JacobianOut_t> |
139 |
|
|
void dIntegrate( |
140 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
141 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
142 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J, |
143 |
|
|
const ArgumentPosition arg, |
144 |
|
|
const AssignmentOperatorType op = SETTO) const; |
145 |
|
|
|
146 |
|
|
/** |
147 |
|
|
* @brief Computes the Jacobian of a small variation of the configuration vector into |
148 |
|
|
* tangent space at identity. |
149 |
|
|
* |
150 |
|
|
* @details This Jacobian corresponds to the Jacobian of \f$ (\mathbf{q} \oplus \delta |
151 |
|
|
* \mathbf{q}) \oplus \mathbf{v} \f$ with \f$ \delta \mathbf{q} \rightarrow 0 \f$. |
152 |
|
|
* |
153 |
|
|
* @param[in] q configuration vector. |
154 |
|
|
* @param[in] v tangent vector. |
155 |
|
|
* @param[in] op assignment operator (SETTO, ADDTO or RMTO). |
156 |
|
|
* |
157 |
|
|
* @param[out] J the Jacobian of the Integrate operation w.r.t. the configuration vector q. |
158 |
|
|
*/ |
159 |
|
|
template<class Config_t, class Tangent_t, class JacobianOut_t> |
160 |
|
|
void dIntegrate_dq( |
161 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
162 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
163 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J, |
164 |
|
|
const AssignmentOperatorType op = SETTO) const; |
165 |
|
|
|
166 |
|
|
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
167 |
|
|
void dIntegrate_dq( |
168 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
169 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
170 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
171 |
|
|
int self, |
172 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
173 |
|
|
const AssignmentOperatorType op = SETTO) const; |
174 |
|
|
|
175 |
|
|
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
176 |
|
|
void dIntegrate_dq( |
177 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
178 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
179 |
|
|
int self, |
180 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
181 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
182 |
|
|
const AssignmentOperatorType op = SETTO) const; |
183 |
|
|
|
184 |
|
|
/** |
185 |
|
|
* @brief Computes the Jacobian of a small variation of the tangent vector into tangent |
186 |
|
|
* space at identity. |
187 |
|
|
* |
188 |
|
|
* @details This Jacobian corresponds to the Jacobian of \f$ \mathbf{q} \oplus (\mathbf{v} + |
189 |
|
|
* \delta \mathbf{v}) \f$ with \f$ \delta \mathbf{v} \rightarrow 0 \f$. |
190 |
|
|
* |
191 |
|
|
* @param[in] q configuration vector. |
192 |
|
|
* @param[in] v tangent vector. |
193 |
|
|
* @param[in] op assignment operator (SETTO, ADDTO or RMTO). |
194 |
|
|
* |
195 |
|
|
* @param[out] J the Jacobian of the Integrate operation w.r.t. the tangent vector v. |
196 |
|
|
*/ |
197 |
|
|
template<class Config_t, class Tangent_t, class JacobianOut_t> |
198 |
|
|
void dIntegrate_dv( |
199 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
200 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
201 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J, |
202 |
|
|
const AssignmentOperatorType op = SETTO) const; |
203 |
|
|
|
204 |
|
|
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
205 |
|
|
void dIntegrate_dv( |
206 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
207 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
208 |
|
|
int self, |
209 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
210 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
211 |
|
|
const AssignmentOperatorType op = SETTO) const; |
212 |
|
|
|
213 |
|
|
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
214 |
|
|
void dIntegrate_dv( |
215 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
216 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
217 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
218 |
|
|
int self, |
219 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
220 |
|
|
const AssignmentOperatorType op = SETTO) const; |
221 |
|
|
|
222 |
|
|
/** |
223 |
|
|
* |
224 |
|
|
* @brief Transport a matrix from the terminal to the initial tangent space of the integrate |
225 |
|
|
* operation, with respect to the configuration or the velocity arguments. |
226 |
|
|
* |
227 |
|
|
* @details This function performs the parallel transportation of an input matrix whose columns |
228 |
|
|
* are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, to the |
229 |
|
|
* tangent space at \f$ q \f$. In other words, this functions transforms a tangent vector |
230 |
|
|
* expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that |
231 |
|
|
* the change of configuration between \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of |
232 |
|
|
* this tangent vector. A typical example of parallel transportation is the action operated by a |
233 |
|
|
* rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in |
234 |
|
|
* \text{se}(3)\f$. In the context of configuration spaces assimilated to vector spaces, this |
235 |
|
|
* operation corresponds to Identity. For Lie groups, its corresponds to the canonical vector |
236 |
|
|
* field transportation. |
237 |
|
|
* |
238 |
|
|
* @param[in] q configuration vector. |
239 |
|
|
* @param[in] v tangent vector |
240 |
|
|
* @param[in] Jin the input matrix |
241 |
|
|
* @param[in] arg argument with respect to which the differentiation is performed (ARG0 |
242 |
|
|
* corresponding to q, and ARG1 to v) |
243 |
|
|
* |
244 |
|
|
* @param[out] Jout Transported matrix |
245 |
|
|
* |
246 |
|
|
*/ |
247 |
|
|
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
248 |
|
|
void dIntegrateTransport( |
249 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
250 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
251 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
252 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
253 |
|
|
const ArgumentPosition arg) const; |
254 |
|
|
|
255 |
|
|
/** |
256 |
|
|
* |
257 |
|
|
* @brief Transport a matrix from the terminal to the initial tangent space of the integrate |
258 |
|
|
* operation, with respect to the configuration argument. |
259 |
|
|
* |
260 |
|
|
* @details This function performs the parallel transportation of an input matrix whose columns |
261 |
|
|
* are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, to the |
262 |
|
|
* tangent space at \f$ q \f$. In other words, this functions transforms a tangent vector |
263 |
|
|
* expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that |
264 |
|
|
* the change of configuration between \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of |
265 |
|
|
* this tangent vector. A typical example of parallel transportation is the action operated by a |
266 |
|
|
* rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in |
267 |
|
|
* \text{se}(3)\f$. In the context of configuration spaces assimilated to vector spaces, this |
268 |
|
|
* operation corresponds to Identity. For Lie groups, its corresponds to the canonical vector |
269 |
|
|
* field transportation. |
270 |
|
|
* |
271 |
|
|
* @param[in] q configuration vector. |
272 |
|
|
* @param[in] v tangent vector |
273 |
|
|
* @param[in] Jin the input matrix |
274 |
|
|
* |
275 |
|
|
* @param[out] Jout Transported matrix |
276 |
|
|
*/ |
277 |
|
|
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
278 |
|
|
void dIntegrateTransport_dq( |
279 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
280 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
281 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
282 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout) const; |
283 |
|
|
/** |
284 |
|
|
* |
285 |
|
|
* @brief Transport a matrix from the terminal to the initial tangent space of the integrate |
286 |
|
|
* operation, with respect to the velocity argument. |
287 |
|
|
* |
288 |
|
|
* @details This function performs the parallel transportation of an input matrix whose columns |
289 |
|
|
* are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, to the |
290 |
|
|
* tangent space at \f$ q \f$. In other words, this functions transforms a tangent vector |
291 |
|
|
* expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that |
292 |
|
|
* the change of configuration between \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of |
293 |
|
|
* this tangent vector. A typical example of parallel transportation is the action operated by a |
294 |
|
|
* rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in |
295 |
|
|
* \text{se}(3)\f$. In the context of configuration spaces assimilated to vector spaces, this |
296 |
|
|
* operation corresponds to Identity. For Lie groups, its corresponds to the canonical vector |
297 |
|
|
* field transportation. |
298 |
|
|
* |
299 |
|
|
* @param[in] q configuration vector. |
300 |
|
|
* @param[in] v tangent vector |
301 |
|
|
* @param[in] Jin the input matrix |
302 |
|
|
* |
303 |
|
|
* @param[out] Jout Transported matrix |
304 |
|
|
*/ |
305 |
|
|
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
306 |
|
|
void dIntegrateTransport_dv( |
307 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
308 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
309 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
310 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout) const; |
311 |
|
|
|
312 |
|
|
/** |
313 |
|
|
* |
314 |
|
|
* @brief Transport in place a matrix from the terminal to the initial tangent space of the |
315 |
|
|
* integrate operation, with respect to the configuration or the velocity arguments. |
316 |
|
|
* |
317 |
|
|
* @details This function performs the parallel transportation of an input matrix whose columns |
318 |
|
|
* are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, to the |
319 |
|
|
* tangent space at \f$ q \f$. In other words, this functions transforms a tangent vector |
320 |
|
|
* expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that |
321 |
|
|
* the change of configuration between \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of |
322 |
|
|
* this tangent vector. A typical example of parallel transportation is the action operated by a |
323 |
|
|
* rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in |
324 |
|
|
* \text{se}(3)\f$. In the context of configuration spaces assimilated to vector spaces, this |
325 |
|
|
* operation corresponds to Identity. For Lie groups, its corresponds to the canonical vector |
326 |
|
|
* field transportation. |
327 |
|
|
* |
328 |
|
|
* @param[in] q configuration vector. |
329 |
|
|
* @param[in] v tangent vector |
330 |
|
|
* @param[in,out] J the inplace matrix |
331 |
|
|
* @param[in] arg argument with respect to which the differentiation is performed (ARG0 |
332 |
|
|
* corresponding to q, and ARG1 to v) |
333 |
|
|
*/ |
334 |
|
|
template<class Config_t, class Tangent_t, class Jacobian_t> |
335 |
|
|
void dIntegrateTransport( |
336 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
337 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
338 |
|
|
const Eigen::MatrixBase<Jacobian_t> & J, |
339 |
|
|
const ArgumentPosition arg) const; |
340 |
|
|
|
341 |
|
|
/** |
342 |
|
|
* |
343 |
|
|
* @brief Transport in place a matrix from the terminal to the initial tangent space of the |
344 |
|
|
* integrate operation, with respect to the configuration argument. |
345 |
|
|
* |
346 |
|
|
* @details This function performs the parallel transportation of an input matrix whose columns |
347 |
|
|
* are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, to the |
348 |
|
|
* tangent space at \f$ q \f$. In other words, this functions transforms a tangent vector |
349 |
|
|
* expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that |
350 |
|
|
* the change of configuration between \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of |
351 |
|
|
* this tangent vector. A typical example of parallel transportation is the action operated by a |
352 |
|
|
* rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in |
353 |
|
|
* \text{se}(3)\f$. In the context of configuration spaces assimilated to vector spaces, this |
354 |
|
|
* operation corresponds to Identity. For Lie groups, its corresponds to the canonical vector |
355 |
|
|
* field transportation. |
356 |
|
|
* |
357 |
|
|
* @param[in] q configuration vector. |
358 |
|
|
* @param[in] v tangent vector |
359 |
|
|
* @param[in,out] Jin the inplace matrix |
360 |
|
|
* |
361 |
|
|
*/ |
362 |
|
|
template<class Config_t, class Tangent_t, class Jacobian_t> |
363 |
|
|
void dIntegrateTransport_dq( |
364 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
365 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
366 |
|
|
const Eigen::MatrixBase<Jacobian_t> & J) const; |
367 |
|
|
/** |
368 |
|
|
* |
369 |
|
|
* @brief Transport in place a matrix from the terminal to the initial tangent space of the |
370 |
|
|
* integrate operation, with respect to the velocity argument. |
371 |
|
|
* |
372 |
|
|
* @details This function performs the parallel transportation of an input matrix whose columns |
373 |
|
|
* are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, to the |
374 |
|
|
* tangent space at \f$ q \f$. In other words, this functions transforms a tangent vector |
375 |
|
|
* expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that |
376 |
|
|
* the change of configuration between \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of |
377 |
|
|
* this tangent vector. A typical example of parallel transportation is the action operated by a |
378 |
|
|
* rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in |
379 |
|
|
* \text{se}(3)\f$. In the context of configuration spaces assimilated to vector spaces, this |
380 |
|
|
* operation corresponds to Identity. For Lie groups, its corresponds to the canonical vector |
381 |
|
|
* field transportation. |
382 |
|
|
* |
383 |
|
|
* @param[in] q configuration vector. |
384 |
|
|
* @param[in] v tangent vector |
385 |
|
|
* @param[in,out] J the inplace matrix |
386 |
|
|
* |
387 |
|
|
*/ |
388 |
|
|
template<class Config_t, class Tangent_t, class Jacobian_t> |
389 |
|
|
void dIntegrateTransport_dv( |
390 |
|
|
const Eigen::MatrixBase<Config_t> & q, |
391 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
392 |
|
|
const Eigen::MatrixBase<Jacobian_t> & J) const; |
393 |
|
|
|
394 |
|
|
/** |
395 |
|
|
* @brief Interpolation between two joint's configurations |
396 |
|
|
* |
397 |
|
|
* @param[in] q0 the initial configuration to interpolate. |
398 |
|
|
* @param[in] q1 the final configuration to interpolate. |
399 |
|
|
* @param[in] u in [0;1] the absicca along the interpolation. |
400 |
|
|
* |
401 |
|
|
* @param[out] qout the interpolated configuration (q0 if u = 0, q1 if u = 1) |
402 |
|
|
*/ |
403 |
|
|
template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
404 |
|
|
void interpolate( |
405 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
406 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
407 |
|
|
const Scalar & u, |
408 |
|
|
const Eigen::MatrixBase<ConfigOut_t> & qout) const; |
409 |
|
|
|
410 |
|
|
/** |
411 |
|
|
* @brief Normalize the joint configuration given as input. |
412 |
|
|
* For instance, the quaternion must be unitary. |
413 |
|
|
* |
414 |
|
|
* @note If the input vector is too small (i.e., qout.norm()==0), then it is left |
415 |
|
|
* unchanged. It is therefore possible that after this method is called `isNormalized(qout)` is |
416 |
|
|
* still false. |
417 |
|
|
* |
418 |
|
|
* @param[in,out] qout the normalized joint configuration. |
419 |
|
|
*/ |
420 |
|
|
template<class Config_t> |
421 |
|
|
void normalize(const Eigen::MatrixBase<Config_t> & qout) const; |
422 |
|
|
|
423 |
|
|
/** |
424 |
|
|
* @brief Check whether the input joint configuration is normalized. |
425 |
|
|
* For instance, the quaternion must be unitary. |
426 |
|
|
* |
427 |
|
|
* @param[in] qin The joint configuration to check. |
428 |
|
|
* |
429 |
|
|
* @return true if the input vector is normalized, false otherwise. |
430 |
|
|
*/ |
431 |
|
|
template<class Config_t> |
432 |
|
|
bool isNormalized( |
433 |
|
|
const Eigen::MatrixBase<Config_t> & qin, |
434 |
|
|
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const; |
435 |
|
|
|
436 |
|
|
/** |
437 |
|
|
* @brief Generate a random joint configuration, normalizing quaternions when necessary. |
438 |
|
|
* |
439 |
|
|
* \warning Do not take into account the joint limits. To shoot a configuration uniformingly |
440 |
|
|
* depending on joint limits, see randomConfiguration. |
441 |
|
|
* |
442 |
|
|
* @param[out] qout the random joint configuration. |
443 |
|
|
*/ |
444 |
|
|
template<class Config_t> |
445 |
|
|
void random(const Eigen::MatrixBase<Config_t> & qout) const; |
446 |
|
|
|
447 |
|
|
/** |
448 |
|
|
* @brief Generate a configuration vector uniformly sampled among |
449 |
|
|
* provided limits. |
450 |
|
|
* |
451 |
|
|
* @param[in] lower_pos_limit the lower joint limit vector. |
452 |
|
|
* @param[in] upper_pos_limit the upper joint limit vector. |
453 |
|
|
* |
454 |
|
|
* @param[out] qout the random joint configuration in the two bounds. |
455 |
|
|
*/ |
456 |
|
|
template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
457 |
|
|
void randomConfiguration( |
458 |
|
|
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit, |
459 |
|
|
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit, |
460 |
|
|
const Eigen::MatrixBase<ConfigOut_t> & qout) const; |
461 |
|
|
|
462 |
|
|
/** |
463 |
|
|
* @brief Computes the tangent vector that must be integrated during one unit time to go |
464 |
|
|
* from q0 to q1. |
465 |
|
|
* |
466 |
|
|
* @param[in] q0 the initial configuration vector. |
467 |
|
|
* @param[in] q1 the terminal configuration vector. |
468 |
|
|
* |
469 |
|
|
* @param[out] v the corresponding velocity. |
470 |
|
|
* |
471 |
|
|
* @note Both inputs must be well-formed configuration vectors. The output of this |
472 |
|
|
* function is unspecified if inputs contain NaNs or extremal values for the underlying scalar |
473 |
|
|
* type. |
474 |
|
|
* |
475 |
|
|
* \cheatsheet \f$ q_1 \ominus q_0 = - \left( q_0 \ominus q_1 \right) \f$ |
476 |
|
|
*/ |
477 |
|
|
template<class ConfigL_t, class ConfigR_t, class Tangent_t> |
478 |
|
|
void difference( |
479 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
480 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
481 |
|
|
const Eigen::MatrixBase<Tangent_t> & v) const; |
482 |
|
|
|
483 |
|
|
/** |
484 |
|
|
* |
485 |
|
|
* @brief Computes the Jacobian of the difference operation with respect to q0 or q1. |
486 |
|
|
* |
487 |
|
|
* @tparam arg ARG0 (resp. ARG1) to get the Jacobian with respect to q0 (resp. q1). |
488 |
|
|
* |
489 |
|
|
* @param[in] q0 the initial configuration vector. |
490 |
|
|
* @param[in] q1 the terminal configuration vector. |
491 |
|
|
* |
492 |
|
|
* @param[out] J the Jacobian of the difference operation. |
493 |
|
|
* |
494 |
|
|
* \warning because \f$ q_1 \ominus q_0 = - \left( q_0 \ominus q_1 \right) \f$, |
495 |
|
|
* you may be tempted to write |
496 |
|
|
* \f$ \frac{\partial\ominus}{\partial q_1} = - \frac{\partial\ominus}{\partial q_0} \f$. |
497 |
|
|
* This is **false** in the general case because |
498 |
|
|
* \f$ \frac{\partial\ominus}{\partial q_i} \f$ expects an input velocity relative to frame i. |
499 |
|
|
* See SpecialEuclideanOperationTpl<3,_Scalar,_Options>::dDifference_impl. |
500 |
|
|
* |
501 |
|
|
* \cheatsheet \f$ \frac{\partial\ominus}{\partial q_1} \frac{\partial\oplus}{\partial v} = I |
502 |
|
|
* \f$ |
503 |
|
|
*/ |
504 |
|
|
template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> |
505 |
|
|
void dDifference( |
506 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
507 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
508 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J) const; |
509 |
|
|
|
510 |
|
|
/** |
511 |
|
|
* |
512 |
|
|
* @brief Computes the Jacobian of the difference operation with respect to q0 or q1. |
513 |
|
|
* |
514 |
|
|
* @param[in] q0 the initial configuration vector. |
515 |
|
|
* @param[in] q1 the terminal configuration vector. |
516 |
|
|
* @param[in] arg ARG0 (resp. ARG1) to get the Jacobian with respect to q0 (resp. q1). |
517 |
|
|
* |
518 |
|
|
* @param[out] J the Jacobian of the difference operation. |
519 |
|
|
* |
520 |
|
|
*/ |
521 |
|
|
template<class ConfigL_t, class ConfigR_t, class JacobianOut_t> |
522 |
|
|
void dDifference( |
523 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
524 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
525 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J, |
526 |
|
|
const ArgumentPosition arg) const; |
527 |
|
|
|
528 |
|
|
template< |
529 |
|
|
ArgumentPosition arg, |
530 |
|
|
class ConfigL_t, |
531 |
|
|
class ConfigR_t, |
532 |
|
|
class JacobianIn_t, |
533 |
|
|
class JacobianOut_t> |
534 |
|
|
void dDifference( |
535 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
536 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
537 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
538 |
|
|
int self, |
539 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
540 |
|
|
const AssignmentOperatorType op = SETTO) const; |
541 |
|
|
|
542 |
|
|
template< |
543 |
|
|
ArgumentPosition arg, |
544 |
|
|
class ConfigL_t, |
545 |
|
|
class ConfigR_t, |
546 |
|
|
class JacobianIn_t, |
547 |
|
|
class JacobianOut_t> |
548 |
|
|
void dDifference( |
549 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
550 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
551 |
|
|
int self, |
552 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
553 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
554 |
|
|
const AssignmentOperatorType op = SETTO) const; |
555 |
|
|
|
556 |
|
|
/** |
557 |
|
|
* @brief Squared distance between two joint configurations. |
558 |
|
|
* |
559 |
|
|
* @param[in] q0 the initial configuration vector. |
560 |
|
|
* @param[in] q1 the terminal configuration vector. |
561 |
|
|
* |
562 |
|
|
* @param[out] d the corresponding distance betwenn q0 and q1. |
563 |
|
|
*/ |
564 |
|
|
template<class ConfigL_t, class ConfigR_t> |
565 |
|
|
Scalar squaredDistance( |
566 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const; |
567 |
|
|
|
568 |
|
|
/** |
569 |
|
|
* @brief Distance between two configurations of the joint |
570 |
|
|
* |
571 |
|
|
* @param[in] q0 the initial configuration vector. |
572 |
|
|
* @param[in] q1 the terminal configuration vector. |
573 |
|
|
* |
574 |
|
|
* @return The corresponding distance. |
575 |
|
|
*/ |
576 |
|
|
template<class ConfigL_t, class ConfigR_t> |
577 |
|
|
Scalar distance( |
578 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const; |
579 |
|
|
|
580 |
|
|
/** |
581 |
|
|
* @brief Check if two configurations are equivalent within the given precision. |
582 |
|
|
* |
583 |
|
|
* @param[in] q0 Configuration 0 |
584 |
|
|
* @param[in] q1 Configuration 1 |
585 |
|
|
* |
586 |
|
|
* \cheatsheet \f$ q_1 \equiv q_0 \oplus \left( q_1 \ominus q_0 \right) \f$ (\f$\equiv\f$ means |
587 |
|
|
* equivalent, not equal). |
588 |
|
|
*/ |
589 |
|
|
template<class ConfigL_t, class ConfigR_t> |
590 |
|
|
bool isSameConfiguration( |
591 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
592 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
593 |
|
|
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const; |
594 |
|
|
|
595 |
|
✗ |
bool operator==(const LieGroupBase & other) const |
596 |
|
|
{ |
597 |
|
✗ |
return derived().isEqual_impl(other.derived()); |
598 |
|
|
} |
599 |
|
|
|
600 |
|
|
bool operator!=(const LieGroupBase & other) const |
601 |
|
|
{ |
602 |
|
|
return derived().isDifferent_impl(other.derived()); |
603 |
|
|
} |
604 |
|
|
/// \} |
605 |
|
|
|
606 |
|
|
/// \name API that allocates memory |
607 |
|
|
/// \{ |
608 |
|
|
|
609 |
|
|
template<class Config_t, class Tangent_t> |
610 |
|
|
ConfigVector_t |
611 |
|
|
integrate(const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Tangent_t> & v) const; |
612 |
|
|
|
613 |
|
|
template<class ConfigL_t, class ConfigR_t> |
614 |
|
|
ConfigVector_t interpolate( |
615 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
616 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
617 |
|
|
const Scalar & u) const; |
618 |
|
|
|
619 |
|
|
ConfigVector_t random() const; |
620 |
|
|
|
621 |
|
|
template<class ConfigL_t, class ConfigR_t> |
622 |
|
|
ConfigVector_t randomConfiguration( |
623 |
|
|
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit, |
624 |
|
|
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const; |
625 |
|
|
|
626 |
|
|
template<class ConfigL_t, class ConfigR_t> |
627 |
|
|
TangentVector_t difference( |
628 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const; |
629 |
|
|
/// \} |
630 |
|
|
|
631 |
|
|
/// \name Default implementations |
632 |
|
|
/// \{ |
633 |
|
|
|
634 |
|
|
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
635 |
|
|
void dIntegrate_product_impl( |
636 |
|
|
const Config_t & q, |
637 |
|
|
const Tangent_t & v, |
638 |
|
|
const JacobianIn_t & Jin, |
639 |
|
|
JacobianOut_t & Jout, |
640 |
|
|
bool dIntegrateOnTheLeft, |
641 |
|
|
const ArgumentPosition arg, |
642 |
|
|
const AssignmentOperatorType op) const; |
643 |
|
|
|
644 |
|
|
template< |
645 |
|
|
ArgumentPosition arg, |
646 |
|
|
class ConfigL_t, |
647 |
|
|
class ConfigR_t, |
648 |
|
|
class JacobianIn_t, |
649 |
|
|
class JacobianOut_t> |
650 |
|
|
void dDifference_product_impl( |
651 |
|
|
const ConfigL_t & q0, |
652 |
|
|
const ConfigR_t & q1, |
653 |
|
|
const JacobianIn_t & Jin, |
654 |
|
|
JacobianOut_t & Jout, |
655 |
|
|
bool dDifferenceOnTheLeft, |
656 |
|
|
const AssignmentOperatorType op) const; |
657 |
|
|
|
658 |
|
|
template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
659 |
|
|
void interpolate_impl( |
660 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
661 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
662 |
|
|
const Scalar & u, |
663 |
|
|
const Eigen::MatrixBase<ConfigOut_t> & qout) const; |
664 |
|
|
|
665 |
|
|
template<class Config_t> |
666 |
|
|
void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const; |
667 |
|
|
|
668 |
|
|
template<class Config_t> |
669 |
|
|
bool isNormalized_impl( |
670 |
|
|
const Eigen::MatrixBase<Config_t> & qin, |
671 |
|
|
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const; |
672 |
|
|
|
673 |
|
|
template<class ConfigL_t, class ConfigR_t> |
674 |
|
|
Scalar squaredDistance_impl( |
675 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const; |
676 |
|
|
|
677 |
|
|
template<class ConfigL_t, class ConfigR_t> |
678 |
|
|
bool isSameConfiguration_impl( |
679 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
680 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
681 |
|
|
const Scalar & prec) const; |
682 |
|
|
|
683 |
|
|
/// \brief Default equality check. |
684 |
|
|
/// By default, two LieGroupBase of same type are considered equal. |
685 |
|
✗ |
bool isEqual_impl(const LieGroupBase & /*other*/) const |
686 |
|
|
{ |
687 |
|
✗ |
return true; |
688 |
|
|
} |
689 |
|
✗ |
bool isDifferent_impl(const LieGroupBase & other) const |
690 |
|
|
{ |
691 |
|
✗ |
return !derived().isEqual_impl(other.derived()); |
692 |
|
|
} |
693 |
|
|
|
694 |
|
|
/// Get dimension of Lie Group vector representation |
695 |
|
|
/// |
696 |
|
|
/// For instance, for SO(3), the dimension of the vector representation is |
697 |
|
|
/// 4 (quaternion) while the dimension of the tangent space is 3. |
698 |
|
|
Index nq() const; |
699 |
|
|
/// Get dimension of Lie Group tangent space |
700 |
|
|
Index nv() const; |
701 |
|
|
/// Get neutral element as a vector |
702 |
|
|
ConfigVector_t neutral() const; |
703 |
|
|
|
704 |
|
|
/// Get name of instance |
705 |
|
|
std::string name() const; |
706 |
|
|
|
707 |
|
|
Derived & derived() |
708 |
|
|
{ |
709 |
|
|
return static_cast<Derived &>(*this); |
710 |
|
|
} |
711 |
|
|
|
712 |
|
256410 |
const Derived & derived() const |
713 |
|
|
{ |
714 |
|
256410 |
return static_cast<const Derived &>(*this); |
715 |
|
|
} |
716 |
|
|
/// \} |
717 |
|
|
|
718 |
|
|
protected: |
719 |
|
|
/// Default constructor. |
720 |
|
|
/// |
721 |
|
|
/// Prevent the construction of derived class. |
722 |
|
258242 |
LieGroupBase() |
723 |
|
|
{ |
724 |
|
|
} |
725 |
|
|
|
726 |
|
|
/// Copy constructor |
727 |
|
|
/// |
728 |
|
|
/// Prevent the copy of derived class. |
729 |
|
✗ |
LieGroupBase(const LieGroupBase & /*clone*/) |
730 |
|
|
{ |
731 |
|
|
} |
732 |
|
|
|
733 |
|
|
/// Copy assignment operator |
734 |
|
|
/// |
735 |
|
|
/// Prevent the copy of derived class. |
736 |
|
✗ |
LieGroupBase & operator=(const LieGroupBase & /*other*/) |
737 |
|
|
{ |
738 |
|
✗ |
return *this; |
739 |
|
|
} |
740 |
|
|
|
741 |
|
|
// C++11 |
742 |
|
|
// LieGroupBase(const LieGroupBase &) = delete; |
743 |
|
|
// LieGroupBase& operator=(const LieGroupBase & /*x*/) = delete; |
744 |
|
|
}; // struct LieGroupBase |
745 |
|
|
|
746 |
|
|
} // namespace pinocchio |
747 |
|
|
|
748 |
|
|
#include "pinocchio/multibody/liegroup/liegroup-base.hxx" |
749 |
|
|
|
750 |
|
|
#endif // ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__ |
751 |
|
|
|