1 |
|
|
// |
2 |
|
|
// Copyright (c) 2016-2021 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_algorithm_joint_configuration_hpp__ |
6 |
|
|
#define __pinocchio_algorithm_joint_configuration_hpp__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/multibody/model.hpp" |
9 |
|
|
#include "pinocchio/multibody/liegroup/liegroup.hpp" |
10 |
|
|
|
11 |
|
|
namespace pinocchio |
12 |
|
|
{ |
13 |
|
|
|
14 |
|
|
/// \name API with return value as argument |
15 |
|
|
/// \{ |
16 |
|
|
|
17 |
|
|
/** |
18 |
|
|
* |
19 |
|
|
* @brief Integrate a configuration vector for the specified model for a tangent vector during one unit time |
20 |
|
|
* |
21 |
|
|
* @details This function corresponds to the exponential map of the joint configuration Lie Group. |
22 |
|
|
* Its output can be interpreted as the "sum" from the Lie algebra to the joint configuration space \f$ q \oplus v \f$. |
23 |
|
|
* |
24 |
|
|
* @param[in] model Model of the kinematic tree on which the integration is performed. |
25 |
|
|
* @param[in] q Initial configuration (size model.nq) |
26 |
|
|
* @param[in] v Joint velocity (size model.nv) |
27 |
|
|
* |
28 |
|
|
* @param[out] qout The integrated configuration (size model.nq) |
29 |
|
|
* |
30 |
|
|
*/ |
31 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType> |
32 |
|
|
void |
33 |
|
|
integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
34 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
35 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v, |
36 |
|
|
const Eigen::MatrixBase<ReturnType> & qout); |
37 |
|
|
|
38 |
|
|
/** |
39 |
|
|
* |
40 |
|
|
* @brief Integrate a configuration vector for the specified model for a tangent vector during one unit time |
41 |
|
|
* |
42 |
|
|
* @details This function corresponds to the exponential map of the joint configuration Lie Group. |
43 |
|
|
* Its output can be interpreted as the "sum" from the Lie algebra to the joint configuration space \f$ q \oplus v \f$. |
44 |
|
|
* |
45 |
|
|
* @param[in] model Model of the kinematic tree on which the integration is performed. |
46 |
|
|
* @param[in] q Initial configuration (size model.nq) |
47 |
|
|
* @param[in] v Joint velocity (size model.nv) |
48 |
|
|
* |
49 |
|
|
* @param[out] qout The integrated configuration (size model.nq) |
50 |
|
|
* |
51 |
|
|
*/ |
52 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType> |
53 |
|
|
void |
54 |
|
|
integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
55 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
56 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v, |
57 |
|
|
const Eigen::MatrixBase<ReturnType> & qout) |
58 |
|
|
{ |
59 |
|
|
integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,ReturnType>(model, q.derived(), v.derived(), qout.derived()); |
60 |
|
|
} |
61 |
|
|
|
62 |
|
|
/** |
63 |
|
|
* |
64 |
|
|
* @brief Interpolate two configurations for a given model |
65 |
|
|
* |
66 |
|
|
* @param[in] model Model of the kinematic tree on which the interpolation is performed. |
67 |
|
|
* @param[in] q0 Initial configuration vector (size model.nq) |
68 |
|
|
* @param[in] q1 Final configuration vector (size model.nq) |
69 |
|
|
* @param[in] u u in [0;1] position along the interpolation. |
70 |
|
|
* |
71 |
|
|
* @param[out] qout The interpolated configuration (q0 if u = 0, q1 if u = 1) |
72 |
|
|
* |
73 |
|
|
*/ |
74 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
75 |
|
|
void |
76 |
|
|
interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
77 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
78 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
79 |
|
|
const Scalar & u, |
80 |
|
|
const Eigen::MatrixBase<ReturnType> & qout); |
81 |
|
|
|
82 |
|
|
/** |
83 |
|
|
* |
84 |
|
|
* @brief Interpolate two configurations for a given model |
85 |
|
|
* |
86 |
|
|
* @param[in] model Model of the kinematic tree on which the interpolation is performed. |
87 |
|
|
* @param[in] q0 Initial configuration vector (size model.nq) |
88 |
|
|
* @param[in] q1 Final configuration vector (size model.nq) |
89 |
|
|
* @param[in] u u in [0;1] position along the interpolation. |
90 |
|
|
* |
91 |
|
|
* @param[out] qout The interpolated configuration (q0 if u = 0, q1 if u = 1) |
92 |
|
|
* |
93 |
|
|
*/ |
94 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
95 |
|
|
void |
96 |
|
|
interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
97 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
98 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
99 |
|
|
const Scalar & u, |
100 |
|
|
const Eigen::MatrixBase<ReturnType> & qout) |
101 |
|
|
{ |
102 |
|
|
interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout)); |
103 |
|
|
} |
104 |
|
|
|
105 |
|
|
/** |
106 |
|
|
* |
107 |
|
|
* @brief Compute the tangent vector that must be integrated during one unit time to go from q0 to q1 |
108 |
|
|
* |
109 |
|
|
* @details This function corresponds to the log map of the joint configuration Lie Group. |
110 |
|
|
* Its output can be interpreted as a difference from the joint configuration space to the Lie algebra \f$ q_1 \ominus q_0 \f$. |
111 |
|
|
* |
112 |
|
|
* @param[in] model Model of the system on which the difference operation is performed. |
113 |
|
|
* @param[in] q0 Initial configuration (size model.nq) |
114 |
|
|
* @param[in] q1 Desired configuration (size model.nq) |
115 |
|
|
* |
116 |
|
|
* @param[out] dvout The corresponding velocity (size model.nv) |
117 |
|
|
* |
118 |
|
|
*/ |
119 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
120 |
|
|
void |
121 |
|
|
difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
122 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
123 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
124 |
|
|
const Eigen::MatrixBase<ReturnType> & dvout); |
125 |
|
|
|
126 |
|
|
/** |
127 |
|
|
* |
128 |
|
|
* @brief Compute the tangent vector that must be integrated during one unit time to go from q0 to q1 |
129 |
|
|
* |
130 |
|
|
* @details This function corresponds to the log map of the joint configuration Lie Group. |
131 |
|
|
* Its output can be interpreted as a difference from the joint configuration space to the Lie algebra \f$ q_1 \ominus q_0 \f$. |
132 |
|
|
* |
133 |
|
|
* @param[in] model Model of the system on which the difference operation is performed. |
134 |
|
|
* @param[in] q0 Initial configuration (size model.nq) |
135 |
|
|
* @param[in] q1 Desired configuration (size model.nq) |
136 |
|
|
* |
137 |
|
|
* @param[out] dvout The corresponding velocity (size model.nv). |
138 |
|
|
* |
139 |
|
|
*/ |
140 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
141 |
|
|
void |
142 |
|
|
difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
143 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
144 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
145 |
|
|
const Eigen::MatrixBase<ReturnType> & dvout) |
146 |
|
|
{ |
147 |
|
|
difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,dvout)); |
148 |
|
|
} |
149 |
|
|
|
150 |
|
|
/** |
151 |
|
|
* |
152 |
|
|
* @brief Squared distance between two configuration vectors |
153 |
|
|
* |
154 |
|
|
* @param[in] model Model of the system on which the squared distance operation is performed. |
155 |
|
|
* @param[in] q0 Configuration 0 (size model.nq) |
156 |
|
|
* @param[in] q1 Configuration 1 (size model.nq) |
157 |
|
|
* @param[out] out The corresponding squared distances for each joint (size model.njoints-1 = number of joints). |
158 |
|
|
* |
159 |
|
|
*/ |
160 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
161 |
|
|
void |
162 |
|
|
squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
163 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
164 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
165 |
|
|
const Eigen::MatrixBase<ReturnType> & out); |
166 |
|
|
|
167 |
|
|
/** |
168 |
|
|
* |
169 |
|
|
* @brief Squared distance between two configuration vectors |
170 |
|
|
* |
171 |
|
|
* @param[in] model Model of the system on which the squared distance operation is performed. |
172 |
|
|
* @param[in] q0 Configuration 0 (size model.nq) |
173 |
|
|
* @param[in] q1 Configuration 1 (size model.nq) |
174 |
|
|
* @param[out] out The corresponding squared distances for each joint (size model.njoints-1 = number of joints). |
175 |
|
|
* |
176 |
|
|
*/ |
177 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
178 |
|
|
void |
179 |
|
|
squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
180 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
181 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
182 |
|
|
const Eigen::MatrixBase<ReturnType> & out) |
183 |
|
|
{ |
184 |
|
|
squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,out)); |
185 |
|
|
} |
186 |
|
|
|
187 |
|
|
/** |
188 |
|
|
* |
189 |
|
|
* @brief Generate a configuration vector uniformly sampled among provided limits. |
190 |
|
|
* |
191 |
|
|
* @remarks Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded. |
192 |
|
|
* |
193 |
|
|
* @warning If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample. |
194 |
|
|
* |
195 |
|
|
* @param[in] model Model of the system on which the random configuration operation is performed. |
196 |
|
|
* @param[in] lowerLimits Joints lower limits (size model.nq). |
197 |
|
|
* @param[in] upperLimits Joints upper limits (size model.nq). |
198 |
|
|
* @param[out] qout The resulting configuration vector (size model.nq). |
199 |
|
|
* |
200 |
|
|
*/ |
201 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
202 |
|
|
void |
203 |
|
|
randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
204 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits, |
205 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits, |
206 |
|
|
const Eigen::MatrixBase<ReturnType> & qout); |
207 |
|
|
|
208 |
|
|
/** |
209 |
|
|
* |
210 |
|
|
* @brief Generate a configuration vector uniformly sampled among provided limits. |
211 |
|
|
* |
212 |
|
|
* @remarks Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded. |
213 |
|
|
* |
214 |
|
|
* @warning If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample |
215 |
|
|
* |
216 |
|
|
* @param[in] model Model of the system on which the random configuration operation is performed. |
217 |
|
|
* @param[in] lowerLimits Joints lower limits (size model.nq). |
218 |
|
|
* @param[in] upperLimits Joints upper limits (size model.nq). |
219 |
|
|
* @param[out] qout The resulting configuration vector (size model.nq). |
220 |
|
|
* |
221 |
|
|
*/ |
222 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
223 |
|
|
void |
224 |
|
|
randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
225 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits, |
226 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits, |
227 |
|
|
const Eigen::MatrixBase<ReturnType> & qout) |
228 |
|
|
{ |
229 |
|
|
randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, lowerLimits.derived(), upperLimits.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout)); |
230 |
|
|
} |
231 |
|
|
|
232 |
|
|
/** |
233 |
|
|
* |
234 |
|
|
* @brief Return the neutral configuration element related to the model configuration space. |
235 |
|
|
* |
236 |
|
|
* @param[in] model Model of the kinematic tree on which the neutral element is computed |
237 |
|
|
* |
238 |
|
|
* @param[out] qout The neutral configuration element (size model.nq). |
239 |
|
|
* |
240 |
|
|
*/ |
241 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType> |
242 |
|
|
void |
243 |
|
|
neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
244 |
|
|
const Eigen::MatrixBase<ReturnType> & qout); |
245 |
|
|
|
246 |
|
|
/** |
247 |
|
|
* |
248 |
|
|
* @brief Return the neutral configuration element related to the model configuration space. |
249 |
|
|
* |
250 |
|
|
* @param[in] model Model of the kinematic tree on which the neutral element is computed. |
251 |
|
|
* |
252 |
|
|
* @param[out] qout The neutral configuration element (size model.nq). |
253 |
|
|
* |
254 |
|
|
*/ |
255 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType> |
256 |
|
|
void |
257 |
|
8 |
neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
258 |
|
|
const Eigen::MatrixBase<ReturnType> & qout) |
259 |
|
|
{ |
260 |
|
8 |
neutral<LieGroupMap,Scalar,Options,JointCollectionTpl,ReturnType>(model,PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout)); |
261 |
|
8 |
} |
262 |
|
|
|
263 |
|
|
/** |
264 |
|
|
* |
265 |
|
|
* @brief Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity. |
266 |
|
|
* |
267 |
|
|
* @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such, |
268 |
|
|
* it is expressed in the tangent space only, not the configuration space. |
269 |
|
|
* Calling \f$ f(q, v) \f$ the integrate function, these jacobians satisfy the following relationships in the |
270 |
|
|
* tangent space: |
271 |
|
|
* - Jacobian relative to q: \f$ f(q \oplus \delta q, v) \ominus f(q, v) = J_q \delta q + o(\delta q)\f$. |
272 |
|
|
* - Jacobian relative to v: \f$ f(q, v + \delta v) \ominus f(q, v) = J_v \delta v + o(\delta v)\f$. |
273 |
|
|
* |
274 |
|
|
* @param[in] model Model of the kinematic tree on which the integration operation is performed. |
275 |
|
|
* @param[in] q Initial configuration (size model.nq) |
276 |
|
|
* @param[in] v Joint velocity (size model.nv) |
277 |
|
|
* @param[out] J Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv). |
278 |
|
|
* @param[in] arg Argument (either q or v) with respect to which the differentiation is performed. |
279 |
|
|
* |
280 |
|
|
*/ |
281 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType> |
282 |
|
|
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
283 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
284 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v, |
285 |
|
|
const Eigen::MatrixBase<JacobianMatrixType> & J, |
286 |
|
|
const ArgumentPosition arg, |
287 |
|
|
const AssignmentOperatorType op=SETTO); |
288 |
|
|
|
289 |
|
|
/** |
290 |
|
|
* |
291 |
|
|
* @brief Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity. |
292 |
|
|
* |
293 |
|
|
* @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such, |
294 |
|
|
* it is expressed in the tangent space only, not the configuration space. |
295 |
|
|
* Calling \f$ f(q, v) \f$ the integrate function, these jacobians satisfy the following relationships in the |
296 |
|
|
* tangent space: |
297 |
|
|
* - Jacobian relative to q: \f$ f(q \oplus \delta q, v) \ominus f(q, v) = J_q(q, v) \delta q + o(\delta q)\f$. |
298 |
|
|
* - Jacobian relative to v: \f$ f(q, v + \delta v) \ominus f(q, v) = J_v(q, v) \delta v + o(\delta v)\f$. |
299 |
|
|
* |
300 |
|
|
* @param[in] model Model of the kinematic tree on which the integration operation is performed. |
301 |
|
|
* @param[in] q Initial configuration (size model.nq) |
302 |
|
|
* @param[in] v Joint velocity (size model.nv) |
303 |
|
|
* @param[out] J Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv). |
304 |
|
|
* @param[in] arg Argument (either q or v) with respect to which the differentiation is performed. |
305 |
|
|
* |
306 |
|
|
*/ |
307 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType> |
308 |
|
8 |
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
309 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
310 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v, |
311 |
|
|
const Eigen::MatrixBase<JacobianMatrixType> & J, |
312 |
|
|
const ArgumentPosition arg) |
313 |
|
|
{ |
314 |
|
8 |
dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,SETTO); |
315 |
|
8 |
} |
316 |
|
|
|
317 |
|
|
|
318 |
|
|
/** |
319 |
|
|
* |
320 |
|
|
* @brief Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity. |
321 |
|
|
* |
322 |
|
|
* @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such, |
323 |
|
|
* it is expressed in the tangent space only, not the configuration space. |
324 |
|
|
* Calling \f$ f(q, v) \f$ the integrate function, these jacobians satisfy the following relationships in the |
325 |
|
|
* tangent space: |
326 |
|
|
* - Jacobian relative to q: \f$ f(q \oplus \delta q, v) \ominus f(q, v) = J_q(q, v) \delta q + o(\delta q)\f$. |
327 |
|
|
* - Jacobian relative to v: \f$ f(q, v + \delta v) \ominus f(q, v) = J_v(q, v) \delta v + o(\delta v)\f$. |
328 |
|
|
* |
329 |
|
|
* @param[in] model Model of the kinematic tree on which the integration operation is performed. |
330 |
|
|
* @param[in] q Initial configuration (size model.nq) |
331 |
|
|
* @param[in] v Joint velocity (size model.nv) |
332 |
|
|
* @param[out] J Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv). |
333 |
|
|
* @param[in] arg Argument (either q or v) with respect to which the differentiation is performed. |
334 |
|
|
* |
335 |
|
|
*/ |
336 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType> |
337 |
|
14 |
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
338 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
339 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v, |
340 |
|
|
const Eigen::MatrixBase<JacobianMatrixType> & J, |
341 |
|
|
const ArgumentPosition arg, |
342 |
|
|
const AssignmentOperatorType op) |
343 |
|
|
{ |
344 |
|
14 |
dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,op); |
345 |
|
14 |
} |
346 |
|
|
|
347 |
|
|
/** |
348 |
|
|
* |
349 |
|
|
* @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. |
350 |
|
|
* |
351 |
|
|
* @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$, |
352 |
|
|
* to the tangent space at \f$ q \f$. |
353 |
|
|
* It performs the product with the Jacobian of integrate by exploiting at best the sparsity of the underlying operations. |
354 |
|
|
* 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 |
355 |
|
|
* \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector. |
356 |
|
|
* 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$. |
357 |
|
|
* In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity. |
358 |
|
|
* For Lie groups, its corresponds to the canonical vector field transportation. |
359 |
|
|
* |
360 |
|
|
* @param[in] model Model of the kinematic tree on which the integration operation is performed. |
361 |
|
|
* @param[in] q Initial configuration (size model.nq) |
362 |
|
|
* @param[in] v Joint velocity (size model.nv) |
363 |
|
|
* @param[out] Jin Input matrix (number of rows = model.nv). |
364 |
|
|
* @param[out] Jout Output matrix (same size as Jin). |
365 |
|
|
* @param[in] arg Argument (either ARG0 for q or ARG1 for v) with respect to which the differentation is performed. |
366 |
|
|
* |
367 |
|
|
*/ |
368 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType1, typename JacobianMatrixType2> |
369 |
|
|
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
370 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
371 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v, |
372 |
|
|
const Eigen::MatrixBase<JacobianMatrixType1> & Jin, |
373 |
|
|
const Eigen::MatrixBase<JacobianMatrixType2> & Jout, |
374 |
|
|
const ArgumentPosition arg); |
375 |
|
|
|
376 |
|
|
/** |
377 |
|
|
* |
378 |
|
|
* @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. |
379 |
|
|
* |
380 |
|
|
* @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$, |
381 |
|
|
* to the tangent space at \f$ q \f$. |
382 |
|
|
* It performs the product with the Jacobian of integrate by exploiting at best the sparsity of the underlying operations. |
383 |
|
|
* 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 |
384 |
|
|
* \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector. |
385 |
|
|
* 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$. |
386 |
|
|
* In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity. |
387 |
|
|
* For Lie groups, its corresponds to the canonical vector field transportation. |
388 |
|
|
* |
389 |
|
|
* @param[in] model Model of the kinematic tree on which the integration operation is performed. |
390 |
|
|
* @param[in] q Initial configuration (size model.nq) |
391 |
|
|
* @param[in] v Joint velocity (size model.nv) |
392 |
|
|
* @param[out] Jin Input matrix (number of rows = model.nv). |
393 |
|
|
* @param[out] Jout Output matrix (same size as Jin). |
394 |
|
|
* @param[in] arg Argument (either ARG0 for q or ARG1 for v) with respect to which the differentation is performed. |
395 |
|
|
* |
396 |
|
|
*/ |
397 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType1, typename JacobianMatrixType2> |
398 |
|
2 |
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
399 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
400 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v, |
401 |
|
|
const Eigen::MatrixBase<JacobianMatrixType1> & Jin, |
402 |
|
|
const Eigen::MatrixBase<JacobianMatrixType2> & Jout, |
403 |
|
|
const ArgumentPosition arg) |
404 |
|
|
{ |
405 |
|
2 |
dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType1,JacobianMatrixType2>(model, q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2,Jout),arg); |
406 |
|
2 |
} |
407 |
|
|
|
408 |
|
|
/** |
409 |
|
|
* |
410 |
|
|
* @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. |
411 |
|
|
* |
412 |
|
|
* @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$, |
413 |
|
|
* to the tangent space at \f$ q \f$. |
414 |
|
|
* 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 |
415 |
|
|
* \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector. |
416 |
|
|
* 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$. |
417 |
|
|
* In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity. |
418 |
|
|
* For Lie groups, its corresponds to the canonical vector field transportation. |
419 |
|
|
* |
420 |
|
|
* @param[in] model Model of the kinematic tree on which the integration operation is performed. |
421 |
|
|
* @param[in] q Initial configuration (size model.nq) |
422 |
|
|
* @param[in] v Joint velocity (size model.nv) |
423 |
|
|
* @param[in,out] J Input/output matrix (number of rows = model.nv). |
424 |
|
|
* @param[in] arg Argument (either ARG0 for q or ARG1 for v) with respect to which the differentation is performed. |
425 |
|
|
* |
426 |
|
|
*/ |
427 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType> |
428 |
|
|
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
429 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
430 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v, |
431 |
|
|
const Eigen::MatrixBase<JacobianMatrixType> & J, |
432 |
|
|
const ArgumentPosition arg); |
433 |
|
|
|
434 |
|
|
/** |
435 |
|
|
* |
436 |
|
|
* @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. |
437 |
|
|
* |
438 |
|
|
* @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$, |
439 |
|
|
* to the tangent space at \f$ q \f$. |
440 |
|
|
* 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 |
441 |
|
|
* \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector. |
442 |
|
|
* 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$. |
443 |
|
|
* In the context of configuration spaces assimilated as vectorial spaces, this operation corresponds to Identity. |
444 |
|
|
* For Lie groups, its corresponds to the canonical vector field transportation. |
445 |
|
|
* |
446 |
|
|
* @param[in] model Model of the kinematic tree on which the integration operation is performed. |
447 |
|
|
* @param[in] q Initial configuration (size model.nq) |
448 |
|
|
* @param[in] v Joint velocity (size model.nv) |
449 |
|
|
* @param[in,out] J Input/output matrix (number of rows = model.nv). |
450 |
|
|
* @param[in] arg Argument (either ARG0 for q or ARG1 for v) with respect to which the differentation is performed. |
451 |
|
|
* |
452 |
|
|
*/ |
453 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType> |
454 |
|
2 |
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
455 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
456 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v, |
457 |
|
|
const Eigen::MatrixBase<JacobianMatrixType> & J, |
458 |
|
|
const ArgumentPosition arg) |
459 |
|
|
{ |
460 |
|
2 |
dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg); |
461 |
|
2 |
} |
462 |
|
|
|
463 |
|
|
/** |
464 |
|
|
* |
465 |
|
|
* @brief Computes the Jacobian of a small variation of the configuration vector into the tangent space at identity. |
466 |
|
|
* |
467 |
|
|
* @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such, |
468 |
|
|
* it is expressed in the tangent space only, not the configuration space. |
469 |
|
|
* Calling \f$ d(q0, q1) \f$ the difference function, these jacobians satisfy the following relationships in the |
470 |
|
|
* tangent space: |
471 |
|
|
* - Jacobian relative to q0: \f$ d(q_0 \oplus \delta q_0, q_1) \ominus d(q_0, q_1) = J_{q_0} \delta q_0 + o(\| \delta q_0 \|)\f$. |
472 |
|
|
* - Jacobian relative to q1: \f$ d(q_0, q_1 \oplus \delta q_1) \ominus d(q_0, q_1) = J_{q_1} \delta q_1 + o(\| \delta q_1 \|)\f$. |
473 |
|
|
* |
474 |
|
|
* @param[in] model Model of the kinematic tree on which the difference operation is performed. |
475 |
|
|
* @param[in] q0 Initial configuration (size model.nq) |
476 |
|
|
* @param[in] q1 Joint velocity (size model.nv) |
477 |
|
|
* @param[out] J Jacobian of the Difference operation, either with respect to q0 or q1 (size model.nv x model.nv). |
478 |
|
|
* @param[in] arg Argument (either q0 or q1) with respect to which the differentiation is performed. |
479 |
|
|
* |
480 |
|
|
*/ |
481 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix> |
482 |
|
|
void dDifference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
483 |
|
|
const Eigen::MatrixBase<ConfigVector1> & q0, |
484 |
|
|
const Eigen::MatrixBase<ConfigVector2> & q1, |
485 |
|
|
const Eigen::MatrixBase<JacobianMatrix> & J, |
486 |
|
|
const ArgumentPosition arg); |
487 |
|
|
|
488 |
|
|
/** |
489 |
|
|
* |
490 |
|
|
* @brief Computes the Jacobian of a small variation of the configuration vector into the tangent space at identity. |
491 |
|
|
* |
492 |
|
|
* @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such, |
493 |
|
|
* it is expressed in the tangent space only, not the configuration space. |
494 |
|
|
* Calling \f$ d(q0, q1) \f$ the difference function, these jacobians satisfy the following relationships in the |
495 |
|
|
* tangent space: |
496 |
|
|
* - Jacobian relative to q0: \f$ d(q_0 \oplus \delta q_0, q_1) \ominus d(q_0, q_1) = J_{q_0} \delta q_0 + o(\| \delta q_0 \|)\f$. |
497 |
|
|
* - Jacobian relative to q1: \f$ d(q_0, q_1 \oplus \delta q_1) \ominus d(q_0, q_1) = J_{q_1} \delta q_1 + o(\| \delta q_1 \|)\f$. |
498 |
|
|
* |
499 |
|
|
* @param[in] model Model of the kinematic tree on which the difference operation is performed. |
500 |
|
|
* @param[in] q0 Initial configuration (size model.nq) |
501 |
|
|
* @param[in] q1 Joint velocity (size model.nv) |
502 |
|
|
* @param[out] J Jacobian of the Difference operation, either with respect to q0 or q1 (size model.nv x model.nv). |
503 |
|
|
* @param[in] arg Argument (either q0 or q1) with respect to which the differentiation is performed. |
504 |
|
|
* |
505 |
|
|
*/ |
506 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix> |
507 |
|
6 |
void dDifference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
508 |
|
|
const Eigen::MatrixBase<ConfigVector1> & q0, |
509 |
|
|
const Eigen::MatrixBase<ConfigVector2> & q1, |
510 |
|
|
const Eigen::MatrixBase<JacobianMatrix> & J, |
511 |
|
|
const ArgumentPosition arg) |
512 |
|
|
{ |
513 |
|
|
dDifference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector1,ConfigVector2,JacobianMatrix> |
514 |
|
6 |
(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J),arg); |
515 |
|
6 |
} |
516 |
|
|
/** |
517 |
|
|
* |
518 |
|
|
* @brief Overall squared distance between two configuration vectors |
519 |
|
|
* |
520 |
|
|
* @param[in] model Model we want to compute the distance |
521 |
|
|
* @param[in] q0 Configuration 0 (size model.nq) |
522 |
|
|
* @param[in] q1 Configuration 1 (size model.nq) |
523 |
|
|
* |
524 |
|
|
* @return The squared distance between the two configurations |
525 |
|
|
* |
526 |
|
|
*/ |
527 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
528 |
|
|
Scalar squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
529 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
530 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1); |
531 |
|
|
|
532 |
|
|
/** |
533 |
|
|
* |
534 |
|
|
* @brief Overall squared distance between two configuration vectors, namely \f$ || q_{1} \ominus q_{0} ||_2^{2} \f$. |
535 |
|
|
* |
536 |
|
|
* @param[in] model Model of the kinematic tree |
537 |
|
|
* @param[in] q0 Configuration from (size model.nq) |
538 |
|
|
* @param[in] q1 Configuration to (size model.nq) |
539 |
|
|
* |
540 |
|
|
* @return The squared distance between the two configurations q0 and q1. |
541 |
|
|
* |
542 |
|
|
*/ |
543 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
544 |
|
|
inline Scalar |
545 |
|
|
squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
546 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
547 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1) |
548 |
|
|
{ |
549 |
|
|
return squaredDistanceSum<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived()); |
550 |
|
|
} |
551 |
|
|
|
552 |
|
|
/** |
553 |
|
|
* |
554 |
|
|
* @brief Distance between two configuration vectors, namely \f$ || q_{1} \ominus q_{0} ||_2 \f$. |
555 |
|
|
* |
556 |
|
|
* @param[in] model Model we want to compute the distance |
557 |
|
|
* @param[in] q0 Configuration 0 (size model.nq) |
558 |
|
|
* @param[in] q1 Configuration 1 (size model.nq) |
559 |
|
|
* |
560 |
|
|
* @return The distance between the two configurations q0 and q1. |
561 |
|
|
* |
562 |
|
|
*/ |
563 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
564 |
|
|
Scalar distance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
565 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
566 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1); |
567 |
|
|
|
568 |
|
|
/** |
569 |
|
|
* |
570 |
|
|
* @brief Distance between two configuration vectors |
571 |
|
|
* |
572 |
|
|
* @param[in] model Model we want to compute the distance |
573 |
|
|
* @param[in] q0 Configuration 0 (size model.nq) |
574 |
|
|
* @param[in] q1 Configuration 1 (size model.nq) |
575 |
|
|
* |
576 |
|
|
* @return The distance between the two configurations q0 and q1. |
577 |
|
|
* |
578 |
|
|
*/ |
579 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
580 |
|
|
inline Scalar |
581 |
|
2 |
distance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
582 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
583 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1) |
584 |
|
|
{ |
585 |
|
2 |
return distance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived()); |
586 |
|
|
} |
587 |
|
|
|
588 |
|
|
/** |
589 |
|
|
* |
590 |
|
|
* @brief Normalize a configuration vector. |
591 |
|
|
* |
592 |
|
|
* @param[in] model Model of the kinematic tree. |
593 |
|
|
* @param[in,out] q Configuration to normalize (size model.nq). |
594 |
|
|
* |
595 |
|
|
*/ |
596 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
597 |
|
|
inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
598 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & qout); |
599 |
|
|
|
600 |
|
|
/** |
601 |
|
|
* |
602 |
|
|
* @brief Normalize a configuration vector. |
603 |
|
|
* |
604 |
|
|
* @param[in] model Model of the kinematic tree. |
605 |
|
|
* @param[in,out] q Configuration to normalize (size model.nq). |
606 |
|
|
* |
607 |
|
|
*/ |
608 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
609 |
|
11 |
inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
610 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & qout) |
611 |
|
|
{ |
612 |
|
11 |
normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout)); |
613 |
|
11 |
} |
614 |
|
|
|
615 |
|
|
/** |
616 |
|
|
* |
617 |
|
|
* @brief Check whether a configuration vector is normalized within the given precision provided by prec. |
618 |
|
|
* |
619 |
|
|
* @param[in] model Model of the kinematic tree. |
620 |
|
|
* @param[in] q Configuration to check (size model.nq). |
621 |
|
|
* @param[in] prec Precision. |
622 |
|
|
* |
623 |
|
|
* @return Whether the configuration is normalized or not, within the given precision. |
624 |
|
|
*/ |
625 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
626 |
|
|
inline bool isNormalized(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
627 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
628 |
|
|
const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()); |
629 |
|
|
|
630 |
|
|
/** |
631 |
|
|
* |
632 |
|
|
* @brief Check whether a configuration vector is normalized within the given precision provided by prec. |
633 |
|
|
* |
634 |
|
|
* @param[in] model Model of the kinematic tree. |
635 |
|
|
* @param[in] q Configuration to check (size model.nq). |
636 |
|
|
* @param[in] prec Precision. |
637 |
|
|
* |
638 |
|
|
* @return Whether the configuration is normalized or not, within the given precision. |
639 |
|
|
*/ |
640 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
641 |
|
9 |
inline bool isNormalized(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
642 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
643 |
|
|
const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) |
644 |
|
|
{ |
645 |
|
9 |
return isNormalized<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,q,prec); |
646 |
|
|
} |
647 |
|
|
|
648 |
|
|
/** |
649 |
|
|
* |
650 |
|
|
* @brief Return true if the given configurations are equivalents, within the given precision. |
651 |
|
|
* @remarks Two configurations can be equivalent but not equally coefficient wise (e.g two quaternions with opposite coefficients give rise to the same orientation, i.e. they are equivalent.). |
652 |
|
|
* |
653 |
|
|
* @param[in] model Model of the kinematic tree. |
654 |
|
|
* @param[in] q1 The first configuraiton to compare. |
655 |
|
|
* @param[in] q2 The second configuration to compare. |
656 |
|
|
* @param[in] prec precision of the comparison. |
657 |
|
|
* |
658 |
|
|
* @return Whether the configurations are equivalent or not, within the given precision. |
659 |
|
|
* |
660 |
|
|
*/ |
661 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
662 |
|
|
inline bool |
663 |
|
|
isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
664 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q1, |
665 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q2, |
666 |
|
|
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()); |
667 |
|
|
|
668 |
|
|
/** |
669 |
|
|
* |
670 |
|
|
* @brief Return true if the given configurations are equivalents, within the given precision. |
671 |
|
|
* @remarks Two configurations can be equivalent but not equally coefficient wise (e.g two quaternions with opposite coefficients give rise to the same orientation, i.e. they are equivalent.). |
672 |
|
|
* |
673 |
|
|
* @param[in] model Model of the kinematic tree. |
674 |
|
|
* @param[in] q1 The first configuraiton to compare |
675 |
|
|
* @param[in] q2 The second configuration to compare |
676 |
|
|
* @param[in] prec precision of the comparison. |
677 |
|
|
* |
678 |
|
|
* @return Whether the configurations are equivalent or not |
679 |
|
|
* |
680 |
|
|
*/ |
681 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
682 |
|
|
inline bool |
683 |
|
3 |
isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
684 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q1, |
685 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q2, |
686 |
|
|
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) |
687 |
|
|
{ |
688 |
|
3 |
return isSameConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q1.derived(), q2.derived(), prec); |
689 |
|
|
} |
690 |
|
|
|
691 |
|
|
/** |
692 |
|
|
* |
693 |
|
|
* @brief Return the Jacobian of the integrate function for the components of the config vector. |
694 |
|
|
* |
695 |
|
|
* @param[in] model Model of the kinematic tree. |
696 |
|
|
* @param[out] jacobian The Jacobian of the integrate operation. |
697 |
|
|
* |
698 |
|
|
* @details This function is often required for the numerical solvers that are working on the |
699 |
|
|
* tangent of the configuration space, instead of the configuration space itself. |
700 |
|
|
* |
701 |
|
|
*/ |
702 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix> |
703 |
|
|
inline void |
704 |
|
|
integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
705 |
|
|
const Eigen::MatrixBase<ConfigVector> & q, |
706 |
|
|
const Eigen::MatrixBase<JacobianMatrix> & jacobian); |
707 |
|
|
|
708 |
|
|
/** |
709 |
|
|
* |
710 |
|
|
* @brief Return the Jacobian of the integrate function for the components of the config vector. |
711 |
|
|
* |
712 |
|
|
* @param[in] model Model of the kinematic tree. |
713 |
|
|
* @param[out] jacobian The Jacobian of the integrate operation. |
714 |
|
|
* |
715 |
|
|
* @details This function is often required for the numerical solvers that are working on the |
716 |
|
|
* tangent of the configuration space, instead of the configuration space itself. |
717 |
|
|
* |
718 |
|
|
*/ |
719 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix> |
720 |
|
|
inline void |
721 |
|
1 |
integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
722 |
|
|
const Eigen::MatrixBase<ConfigVector> & q, |
723 |
|
|
const Eigen::MatrixBase<JacobianMatrix> & jacobian) |
724 |
|
|
{ |
725 |
|
1 |
integrateCoeffWiseJacobian<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector,JacobianMatrix>(model,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian)); |
726 |
|
1 |
} |
727 |
|
|
|
728 |
|
|
/// \} |
729 |
|
|
|
730 |
|
|
/// \name API that allocates memory |
731 |
|
|
/// \{ |
732 |
|
|
|
733 |
|
|
/** |
734 |
|
|
* |
735 |
|
|
* @brief Integrate a configuration vector for the specified model for a tangent vector during one unit time |
736 |
|
|
* |
737 |
|
|
* @param[in] model Model of the kinematic tree on which the integration operation is performed. |
738 |
|
|
* @param[in] q Initial configuration (size model.nq) |
739 |
|
|
* @param[in] v Joint velocity (size model.nv) |
740 |
|
|
* |
741 |
|
|
* @return The integrated configuration (size model.nq) |
742 |
|
|
* |
743 |
|
|
*/ |
744 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
745 |
|
|
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) |
746 |
|
|
integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
747 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
748 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v); |
749 |
|
|
|
750 |
|
|
/** |
751 |
|
|
* |
752 |
|
|
* @brief Integrate a configuration vector for the specified model for a tangent vector during one unit time. |
753 |
|
|
* |
754 |
|
|
* @param[in] model Model of the kinematic tree on which the integration operation is performed. |
755 |
|
|
* @param[in] q Initial configuration (size model.nq) |
756 |
|
|
* @param[in] v Joint velocity (size model.nv) |
757 |
|
|
* |
758 |
|
|
* @return The integrated configuration (size model.nq) |
759 |
|
|
* |
760 |
|
|
*/ |
761 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
762 |
|
|
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) |
763 |
|
2430 |
integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
764 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
765 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v) |
766 |
|
|
{ |
767 |
|
2430 |
return integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType>(model, q.derived(), v.derived()); |
768 |
|
|
} |
769 |
|
|
|
770 |
|
|
/** |
771 |
|
|
* |
772 |
|
|
* @brief Interpolate two configurations for a given model. |
773 |
|
|
* |
774 |
|
|
* @param[in] model Model of the kinematic tree on which the interpolation operation is performed. |
775 |
|
|
* @param[in] q0 Initial configuration vector (size model.nq) |
776 |
|
|
* @param[in] q1 Final configuration vector (size model.nq) |
777 |
|
|
* @param[in] u u in [0;1] position along the interpolation. |
778 |
|
|
* |
779 |
|
|
* @return The interpolated configuration (q0 if u = 0, q1 if u = 1) |
780 |
|
|
* |
781 |
|
|
*/ |
782 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
783 |
|
|
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) |
784 |
|
|
interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
785 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
786 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
787 |
|
|
const Scalar & u); |
788 |
|
|
|
789 |
|
|
/** |
790 |
|
|
* |
791 |
|
|
* @brief Interpolate two configurations for a given model. |
792 |
|
|
* |
793 |
|
|
* @param[in] model Model of the kinematic tree on which the interpolation operation is performed. |
794 |
|
|
* @param[in] q0 Initial configuration vector (size model.nq) |
795 |
|
|
* @param[in] q1 Final configuration vector (size model.nq) |
796 |
|
|
* @param[in] u u in [0;1] position along the interpolation. |
797 |
|
|
* |
798 |
|
|
* @return The interpolated configuration (q0 if u = 0, q1 if u = 1) |
799 |
|
|
* |
800 |
|
|
*/ |
801 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
802 |
|
|
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) |
803 |
|
2 |
interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
804 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
805 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
806 |
|
|
const Scalar & u) |
807 |
|
|
{ |
808 |
|
2 |
return interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived(), u); |
809 |
|
|
} |
810 |
|
|
|
811 |
|
|
/** |
812 |
|
|
* |
813 |
|
|
* @brief Compute the tangent vector that must be integrated during one unit time to go from q0 to q1 |
814 |
|
|
* |
815 |
|
|
* @param[in] model Model of the kinematic tree on which the difference operation is performed. |
816 |
|
|
* @param[in] q0 Initial configuration (size model.nq) |
817 |
|
|
* @param[in] q1 Finial configuration (size model.nq) |
818 |
|
|
* |
819 |
|
|
* @return The corresponding velocity (size model.nv) |
820 |
|
|
* |
821 |
|
|
*/ |
822 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
823 |
|
|
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) |
824 |
|
|
difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
825 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
826 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1); |
827 |
|
|
|
828 |
|
|
/** |
829 |
|
|
* |
830 |
|
|
* @brief Compute the tangent vector that must be integrated during one unit time to go from q0 to q1. |
831 |
|
|
* |
832 |
|
|
* @param[in] model Model of the kinematic tree on which the difference operation is performed. |
833 |
|
|
* @param[in] q0 Initial configuration (size model.nq) |
834 |
|
|
* @param[in] q1 Final configuration (size model.nq) |
835 |
|
|
* |
836 |
|
|
* @return The corresponding velocity (size model.nv) |
837 |
|
|
* |
838 |
|
|
*/ |
839 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
840 |
|
|
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) |
841 |
|
115 |
difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
842 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
843 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1) |
844 |
|
|
{ |
845 |
|
115 |
return difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived()); |
846 |
|
|
} |
847 |
|
|
|
848 |
|
|
/** |
849 |
|
|
* |
850 |
|
|
* @brief Squared distance between two configurations. |
851 |
|
|
* |
852 |
|
|
* @param[in] model Model of the kinematic tree on which the squared distance operation is performed. |
853 |
|
|
* @param[in] q0 Configuration 0 (size model.nq) |
854 |
|
|
* @param[in] q1 Configuration 1 (size model.nq) |
855 |
|
|
* |
856 |
|
|
* @return The corresponding squared distances for each joint (size model.njoints-1, corresponding to the number of joints) |
857 |
|
|
* |
858 |
|
|
*/ |
859 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
860 |
|
|
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) |
861 |
|
|
squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
862 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
863 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1); |
864 |
|
|
|
865 |
|
|
/** |
866 |
|
|
* |
867 |
|
|
* @brief Squared distance between two configuration vectors |
868 |
|
|
* |
869 |
|
|
* @param[in] model Model of the kinematic tree on which the squared distance operation is performed. |
870 |
|
|
* @param[in] q0 Configuration 0 (size model.nq) |
871 |
|
|
* @param[in] q1 Configuration 1 (size model.nq) |
872 |
|
|
* |
873 |
|
|
* @return The corresponding squared distances for each joint (size model.njoints-1, corresponding to the number of joints) |
874 |
|
|
* |
875 |
|
|
*/ |
876 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
877 |
|
|
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) |
878 |
|
1 |
squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
879 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
880 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & q1) |
881 |
|
|
{ |
882 |
|
1 |
return squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived()); |
883 |
|
|
} |
884 |
|
|
|
885 |
|
|
/** |
886 |
|
|
* |
887 |
|
|
* @brief Generate a configuration vector uniformly sampled among given limits. |
888 |
|
|
* |
889 |
|
|
* @remarks Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded. |
890 |
|
|
* |
891 |
|
|
* @warning If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample |
892 |
|
|
* |
893 |
|
|
* @param[in] model Model of the kinematic tree on which the uniform sampling operation is performed. |
894 |
|
|
* @param[in] lowerLimits Joints lower limits (size model.nq). |
895 |
|
|
* @param[in] upperLimits Joints upper limits (size model.nq). |
896 |
|
|
* |
897 |
|
|
* @return The resulting configuration vector (size model.nq). |
898 |
|
|
* |
899 |
|
|
*/ |
900 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
901 |
|
|
typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType)) |
902 |
|
|
randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
903 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits, |
904 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits); |
905 |
|
|
|
906 |
|
|
/** |
907 |
|
|
* |
908 |
|
|
* @brief Generate a configuration vector uniformly sampled among provided limits. |
909 |
|
|
* |
910 |
|
|
* @remarks Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded. |
911 |
|
|
* |
912 |
|
|
* @warning If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample |
913 |
|
|
* |
914 |
|
|
* @param[in] model Model of the kinematic tree on which the uniform sampling operation is performed. |
915 |
|
|
* @param[in] lowerLimits Joints lower limits (size model.nq). |
916 |
|
|
* @param[in] upperLimits Joints upper limits (size model.nq). |
917 |
|
|
* |
918 |
|
|
* @return The resulting configuration vector (size model.nq) |
919 |
|
|
|
920 |
|
|
*/ |
921 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
922 |
|
|
typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType)) |
923 |
|
34 |
randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
924 |
|
|
const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits, |
925 |
|
|
const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits) |
926 |
|
|
{ |
927 |
|
34 |
return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, lowerLimits.derived(), upperLimits.derived()); |
928 |
|
|
} |
929 |
|
|
|
930 |
|
|
/** |
931 |
|
|
* |
932 |
|
|
* @brief Generate a configuration vector uniformly sampled among the joint limits of the specified Model. |
933 |
|
|
* |
934 |
|
|
* @remarks Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded. |
935 |
|
|
* |
936 |
|
|
* @warning If limits are infinite (no one specified when adding a body or no modification directly in my_model.{lowerPositionLimit,upperPositionLimit}, |
937 |
|
|
* exceptions may be thrown in the joint implementation of uniformlySample |
938 |
|
|
* |
939 |
|
|
* @param[in] model Model of the kinematic tree on which the uniform sampling operation is performed. |
940 |
|
|
* |
941 |
|
|
* @return The resulting configuration vector (size model.nq) |
942 |
|
|
* |
943 |
|
|
*/ |
944 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
945 |
|
|
typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType)) |
946 |
|
|
randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model); |
947 |
|
|
|
948 |
|
|
/** |
949 |
|
|
* |
950 |
|
|
* @brief Generate a configuration vector uniformly sampled among the joint limits of the specified Model. |
951 |
|
|
* |
952 |
|
|
* @remarks Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded. |
953 |
|
|
* |
954 |
|
|
* @warning If limits are infinite (no one specified when adding a body or no modification directly in my_model.{lowerPositionLimit,upperPositionLimit}, |
955 |
|
|
* exceptions may be thrown in the joint implementation of uniformlySample |
956 |
|
|
* |
957 |
|
|
* @param[in] model Model of the kinematic tree on which the uniform sampling operation is performed. |
958 |
|
|
* |
959 |
|
|
* @return The resulting configuration vector (size model.nq). |
960 |
|
|
* |
961 |
|
|
*/ |
962 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
963 |
|
|
typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType)) |
964 |
|
94 |
randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) |
965 |
|
|
{ |
966 |
|
94 |
return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl>(model); |
967 |
|
|
} |
968 |
|
|
|
969 |
|
|
/** |
970 |
|
|
* |
971 |
|
|
* @brief Return the neutral configuration element related to the model configuration space. |
972 |
|
|
* |
973 |
|
|
* @param[in] model Model of the kinematic tree on which the neutral element is computed. |
974 |
|
|
* |
975 |
|
|
* @return The neutral configuration element (size model.nq). |
976 |
|
|
* |
977 |
|
|
*/ |
978 |
|
|
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
979 |
|
|
inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> |
980 |
|
|
neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model); |
981 |
|
|
|
982 |
|
|
/** |
983 |
|
|
* @brief Return the neutral configuration element related to the model configuration space. |
984 |
|
|
* |
985 |
|
|
* @param[in] model Model of the kinematic tree on which the neutral element is computed. |
986 |
|
|
* |
987 |
|
|
* @return The neutral configuration element (size model.nq). |
988 |
|
|
*/ |
989 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
990 |
|
|
inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> |
991 |
|
23 |
neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) |
992 |
|
|
{ |
993 |
|
23 |
return neutral<LieGroupMap,Scalar,Options,JointCollectionTpl>(model); |
994 |
|
|
} |
995 |
|
|
|
996 |
|
|
/// \} |
997 |
|
|
|
998 |
|
|
} // namespace pinocchio |
999 |
|
|
|
1000 |
|
|
/* --- Details -------------------------------------------------------------------- */ |
1001 |
|
|
#include "pinocchio/algorithm/joint-configuration.hxx" |
1002 |
|
|
|
1003 |
|
|
#endif // ifndef __pinocchio_algorithm_joint_configuration_hpp__ |