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