pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
special-orthogonal.hpp
1//
2// Copyright (c) 2016-2021 CNRS INRIA
3//
4
5#ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__
6#define __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__
7
8#include <limits>
9
10#include "pinocchio/spatial/explog.hpp"
11#include "pinocchio/math/quaternion.hpp"
12#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
13#include "pinocchio/utils/static-if.hpp"
14
15namespace pinocchio
16{
17 template<int Dim, typename Scalar, int Options = 0>
21
22 template<int Dim, typename Scalar, int Options>
23 struct traits<SpecialOrthogonalOperationTpl<Dim, Scalar, Options>>
24 {
25 };
26
27 template<typename _Scalar, int _Options>
28 struct traits<SpecialOrthogonalOperationTpl<2, _Scalar, _Options>>
29 {
30 typedef _Scalar Scalar;
31 enum
32 {
33 Options = _Options,
34 NQ = 2,
35 NV = 1
36 };
37 };
38
39 template<typename _Scalar, int _Options>
40 struct traits<SpecialOrthogonalOperationTpl<3, _Scalar, _Options>>
41 {
42 typedef _Scalar Scalar;
43 enum
44 {
45 Options = _Options,
46 NQ = 4,
47 NV = 3
48 };
49 };
50
51 template<typename _Scalar, int _Options>
52 struct SpecialOrthogonalOperationTpl<2, _Scalar, _Options>
53 : public LieGroupBase<SpecialOrthogonalOperationTpl<2, _Scalar, _Options>>
54 {
55 PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialOrthogonalOperationTpl);
56 typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
57 typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
58
59 template<typename Matrix2Like>
60 static typename Matrix2Like::Scalar log(const Eigen::MatrixBase<Matrix2Like> & R)
61 {
62 typedef typename Matrix2Like::Scalar Scalar;
63 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
64
65 const Scalar tr = R.trace();
66
67 static const Scalar PI_value = PI<Scalar>();
68
69 using internal::if_then_else;
70 Scalar theta = if_then_else(
71 internal::GT, tr, Scalar(2),
72 Scalar(0), // then
73 if_then_else(
74 internal::LT, tr, Scalar(-2),
75 if_then_else(
76 internal::GE, R(1, 0), Scalar(0), PI_value,
77 static_cast<Scalar>(-PI_value)), // then
78 if_then_else(
79 internal::GT, tr,
80 static_cast<Scalar>(Scalar(2) - Scalar(1e-2)), // TODO: change value
81 static_cast<Scalar>(asin((R(1, 0) - R(0, 1)) / Scalar(2))), // then
82 if_then_else(
83 internal::GE, R(1, 0), Scalar(0),
84 static_cast<Scalar>(acos(tr / Scalar(2))), // then
85 static_cast<Scalar>(-acos(tr / Scalar(2)))))));
86
87 // const bool pos = (R (1, 0) > Scalar(0));
88 // if (tr > Scalar(2)) theta = Scalar(0); // acos((3-1)/2)
89 // else if (tr < Scalar(-2)) theta = (pos ? PI_value : -PI_value); // acos((-1-1)/2)
90 // Around 0, asin is numerically more stable than acos because
91 // acos(x) = PI/2 - x and asin(x) = x (the precision of x is not lost in PI/2).
92 // else if (tr > Scalar(2) - 1e-2) theta = asin ((R(1,0) - R(0,1)) / Scalar(2));
93 // else theta = (pos ? acos (tr/Scalar(2)) : -acos(tr/Scalar(2)));
94 assert(check_expression_if_real<Scalar>(theta == theta) && "theta is NaN"); // theta != NaN
95 // assert ((cos (theta) * R(0,0) + sin (theta) * R(1,0) > 0) &&
96 // (cos (theta) * R(1,0) - sin (theta) * R(0,0) < 1e-6));
97 return theta;
98 }
99
100 template<typename Matrix2Like>
101 static typename Matrix2Like::Scalar Jlog(const Eigen::MatrixBase<Matrix2Like> &)
102 {
103 typedef typename Matrix2Like::Scalar Scalar;
104 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
105 return Scalar(1);
106 }
107
112 static Index nq()
113 {
114 return NQ;
115 }
116
118 static Index nv()
119 {
120 return NV;
121 }
122
123 static ConfigVector_t neutral()
124 {
125 ConfigVector_t n;
126 n << Scalar(1), Scalar(0);
127 return n;
128 }
129
130 static std::string name()
131 {
132 return std::string("SO(2)");
133 }
134
135 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
136 static void difference_impl(
137 const Eigen::MatrixBase<ConfigL_t> & q0,
138 const Eigen::MatrixBase<ConfigR_t> & q1,
139 const Eigen::MatrixBase<Tangent_t> & d)
140 {
141 Matrix2 R; // R0.transpose() * R1;
142 R(0, 0) = R(1, 1) = q0.dot(q1);
143 R(1, 0) = q0(0) * q1(1) - q0(1) * q1(0);
144 R(0, 1) = -R(1, 0);
145 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d)[0] = log(R);
146 }
147
148 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
149 static void dDifference_impl(
150 const Eigen::MatrixBase<ConfigL_t> & q0,
151 const Eigen::MatrixBase<ConfigR_t> & q1,
152 const Eigen::MatrixBase<JacobianOut_t> & J)
153 {
154 Matrix2 R; // R0.transpose() * R1;
155 R(0, 0) = R(1, 1) = q0.dot(q1);
156 R(1, 0) = q0(0) * q1(1) - q0(1) * q1(0);
157 R(0, 1) = -R(1, 0);
158
159 Scalar w(Jlog(R));
160 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).coeffRef(0, 0) = ((arg == ARG0) ? -w : w);
161 }
162
163 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
164 static void integrate_impl(
165 const Eigen::MatrixBase<ConfigIn_t> & q,
166 const Eigen::MatrixBase<Velocity_t> & v,
167 const Eigen::MatrixBase<ConfigOut_t> & qout)
168 {
169 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
170
171 const Scalar ca = q(0);
172 const Scalar sa = q(1);
173 const Scalar & omega = v(0);
174
175 Scalar cosOmega, sinOmega;
176 SINCOS(omega, &sinOmega, &cosOmega);
177 // TODO check the cost of atan2 vs SINCOS
178
179 out << cosOmega * ca - sinOmega * sa, sinOmega * ca + cosOmega * sa;
180 // First order approximation of the normalization of the unit complex
181 // See quaternion::firstOrderNormalize for equations.
182 const Scalar norm2 = out.squaredNorm();
183 out *= (3 - norm2) / 2;
184 assert(pinocchio::isNormalized(out));
185 }
186
187 template<class Config_t, class Jacobian_t>
188 static void integrateCoeffWiseJacobian_impl(
189 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
190 {
191 assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
192 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
193 Jout << -q[1], q[0];
194 }
195
196 template<class Config_t, class Tangent_t, class JacobianOut_t>
197 static void dIntegrate_dq_impl(
198 const Eigen::MatrixBase<Config_t> & /*q*/,
199 const Eigen::MatrixBase<Tangent_t> & /*v*/,
200 const Eigen::MatrixBase<JacobianOut_t> & J,
201 const AssignmentOperatorType op = SETTO)
202 {
203 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
204 switch (op)
205 {
206 case SETTO:
207 Jout(0, 0) = Scalar(1);
208 break;
209 case ADDTO:
210 Jout(0, 0) += Scalar(1);
211 break;
212 case RMTO:
213 Jout(0, 0) -= Scalar(1);
214 break;
215 default:
216 assert(false && "Wrong Op requesed value");
217 break;
218 }
219 }
220
221 template<class Config_t, class Tangent_t, class JacobianOut_t>
222 static void dIntegrate_dv_impl(
223 const Eigen::MatrixBase<Config_t> & /*q*/,
224 const Eigen::MatrixBase<Tangent_t> & /*v*/,
225 const Eigen::MatrixBase<JacobianOut_t> & J,
226 const AssignmentOperatorType op = SETTO)
227 {
228 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
229 switch (op)
230 {
231 case SETTO:
232 Jout(0, 0) = Scalar(1);
233 break;
234 case ADDTO:
235 Jout(0, 0) += Scalar(1);
236 break;
237 case RMTO:
238 Jout(0, 0) -= Scalar(1);
239 break;
240 default:
241 assert(false && "Wrong Op requesed value");
242 break;
243 }
244 }
245
246 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
247 static void dIntegrateTransport_dq_impl(
248 const Eigen::MatrixBase<Config_t> & /*q*/,
249 const Eigen::MatrixBase<Tangent_t> & /*v*/,
250 const Eigen::MatrixBase<JacobianIn_t> & Jin,
251 const Eigen::MatrixBase<JacobianOut_t> & Jout)
252 {
253 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin;
254 }
255
256 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
257 static void dIntegrateTransport_dv_impl(
258 const Eigen::MatrixBase<Config_t> & /*q*/,
259 const Eigen::MatrixBase<Tangent_t> & /*v*/,
260 const Eigen::MatrixBase<JacobianIn_t> & Jin,
261 const Eigen::MatrixBase<JacobianOut_t> & Jout)
262 {
263 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin;
264 }
265
266 template<class Config_t, class Tangent_t, class Jacobian_t>
267 static void dIntegrateTransport_dq_impl(
268 const Eigen::MatrixBase<Config_t> & /*q*/,
269 const Eigen::MatrixBase<Tangent_t> & /*v*/,
270 const Eigen::MatrixBase<Jacobian_t> & /*J*/)
271 {
272 }
273
274 template<class Config_t, class Tangent_t, class Jacobian_t>
275 static void dIntegrateTransport_dv_impl(
276 const Eigen::MatrixBase<Config_t> & /*q*/,
277 const Eigen::MatrixBase<Tangent_t> & /*v*/,
278 const Eigen::MatrixBase<Jacobian_t> & /*J*/)
279 {
280 }
281
282 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
283 static void interpolate_impl(
284 const Eigen::MatrixBase<ConfigL_t> & q0,
285 const Eigen::MatrixBase<ConfigR_t> & q1,
286 const Scalar & u,
287 const Eigen::MatrixBase<ConfigOut_t> & qout)
288 {
289 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
290
291 assert(
292 check_expression_if_real<Scalar>(abs(q0.norm() - 1) < 1e-8)
293 && "initial configuration not normalized");
294 assert(
295 check_expression_if_real<Scalar>(abs(q1.norm() - 1) < 1e-8)
296 && "final configuration not normalized");
297 const Scalar cosTheta = q0.dot(q1);
298 const Scalar sinTheta = q0(0) * q1(1) - q0(1) * q1(0);
299 const Scalar theta = atan2(sinTheta, cosTheta);
300 assert(check_expression_if_real<Scalar>(fabs(sin(theta) - sinTheta) < 1e-8));
301
302 static const Scalar PI_value = PI<Scalar>();
303 static const Scalar PI_value_lower = PI_value - static_cast<Scalar>(1e-6);
304 using namespace internal;
305
306 // const Scalar theta0 = atan2(q0(1), q0(0));
307 const Scalar abs_theta = fabs(theta);
308 out[0] = if_then_else(
309 LT, abs_theta, static_cast<Scalar>(1e-6),
310 static_cast<Scalar>((Scalar(1) - u) * q0[0] + u * q1[0]), // then
311 if_then_else(
312 LT, abs_theta, PI_value_lower, // else
313 static_cast<Scalar>(
314 (sin((Scalar(1) - u) * theta) / sinTheta) * q0[0]
315 + (sin(u * theta) / sinTheta) * q1[0]), // then
316 q0(0) // cos(theta0) // else
317 ));
318
319 out[1] = if_then_else(
320 LT, abs_theta, static_cast<Scalar>(1e-6),
321 static_cast<Scalar>((Scalar(1) - u) * q0[1] + u * q1[1]), // then
322 if_then_else(
323 LT, abs_theta, PI_value_lower, // else
324 static_cast<Scalar>(
325 (sin((Scalar(1) - u) * theta) / sinTheta) * q0[1]
326 + (sin(u * theta) / sinTheta) * q1[1]), // then
327 q0(1) // sin(theta0) // else
328 ));
329 }
330
331 template<class Config_t>
332 static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
333 {
334 pinocchio::normalize(qout.const_cast_derived());
335 }
336
337 template<class Config_t>
338 static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
339 {
340 const Scalar norm = qin.norm();
341 using std::abs;
342 return abs(norm - Scalar(1.0)) < prec;
343 }
344
345 template<class Config_t>
346 static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
347 {
348 Config_t & out = PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout);
349
350 static const Scalar PI_value = PI<Scalar>();
351 const Scalar angle = -PI_value + Scalar(2) * PI_value * ((Scalar)rand()) / RAND_MAX;
352 SINCOS(angle, &out(1), &out(0));
353 }
354
355 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
356 static void randomConfiguration_impl(
357 const Eigen::MatrixBase<ConfigL_t> &,
358 const Eigen::MatrixBase<ConfigR_t> &,
359 const Eigen::MatrixBase<ConfigOut_t> & qout)
360 {
361 random_impl(qout);
362 }
363 }; // struct SpecialOrthogonalOperationTpl<2,_Scalar,_Options>
364
365 template<typename _Scalar, int _Options>
366 struct SpecialOrthogonalOperationTpl<3, _Scalar, _Options>
367 : public LieGroupBase<SpecialOrthogonalOperationTpl<3, _Scalar, _Options>>
368 {
369 PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialOrthogonalOperationTpl);
370
371 typedef Eigen::Quaternion<Scalar> Quaternion_t;
372 typedef Eigen::Map<Quaternion_t> QuaternionMap_t;
373 typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
374 typedef SE3Tpl<Scalar, Options> SE3;
375 typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
376
381 static Index nq()
382 {
383 return NQ;
384 }
385
387 static Index nv()
388 {
389 return NV;
390 }
391
392 static ConfigVector_t neutral()
393 {
394 ConfigVector_t n;
395 n.setZero();
396 n[3] = Scalar(1);
397 return n;
398 }
399
400 static std::string name()
401 {
402 return std::string("SO(3)");
403 }
404
405 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
406 static void difference_impl(
407 const Eigen::MatrixBase<ConfigL_t> & q0,
408 const Eigen::MatrixBase<ConfigR_t> & q1,
409 const Eigen::MatrixBase<Tangent_t> & d)
410 {
411 ConstQuaternionMap_t quat0(q0.derived().data());
412 assert(quaternion::isNormalized(quat0));
413 ConstQuaternionMap_t quat1(q1.derived().data());
414 assert(quaternion::isNormalized(quat1));
415
416 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d) =
417 quaternion::log3(Quaternion_t(quat0.conjugate() * quat1));
418 }
419
420 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
421 static void dDifference_impl(
422 const Eigen::MatrixBase<ConfigL_t> & q0,
423 const Eigen::MatrixBase<ConfigR_t> & q1,
424 const Eigen::MatrixBase<JacobianOut_t> & J)
425 {
426 typedef typename SE3::Matrix3 Matrix3;
427
428 ConstQuaternionMap_t quat0(q0.derived().data());
429 assert(quaternion::isNormalized(quat0));
430 ConstQuaternionMap_t quat1(q1.derived().data());
431 assert(quaternion::isNormalized(quat1));
432
433 // TODO: check whether the merge with 2.6.9 is correct
434 const Quaternion_t q = quat0.conjugate() * quat1;
435 const Matrix3 R = q.matrix();
436
437 if (arg == ARG0)
438 {
439 JacobianMatrix_t J1;
440 Jlog3(R, J1);
441
442 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).noalias() = -J1 * R.transpose();
443 }
444 else if (arg == ARG1)
445 {
446 Jlog3(R, J);
447 }
448 /*
449 const Quaternion_t quat_diff = quat0.conjugate() * quat1;
450
451 if (arg == ARG0) {
452PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
453PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
454 JacobianMatrix_t J1;
455 quaternion::Jlog3(quat_diff, J1);
456PINOCCHIO_COMPILER_DIAGNOSTIC_POP
457 const Matrix3 R = (quat_diff).matrix();
458
459 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose();
460 } else if (arg == ARG1) {
461 quaternion::Jlog3(quat_diff, J);
462 }
463 */
464 }
465
466 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
467 static void integrate_impl(
468 const Eigen::MatrixBase<ConfigIn_t> & q,
469 const Eigen::MatrixBase<Velocity_t> & v,
470 const Eigen::MatrixBase<ConfigOut_t> & qout)
471 {
472 ConstQuaternionMap_t quat(q.derived().data());
473 assert(quaternion::isNormalized(quat));
474 QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data());
475
476 Quaternion_t pOmega;
477 quaternion::exp3(v, pOmega);
478 quat_map = quat * pOmega;
479 quaternion::firstOrderNormalize(quat_map);
480 assert(quaternion::isNormalized(quat_map));
481 }
482
483 template<class Config_t, class Jacobian_t>
484 static void integrateCoeffWiseJacobian_impl(
485 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
486 {
487 assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
488
489 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
490 typedef typename SE3::Vector3 Vector3;
491 typedef typename SE3::Matrix3 Matrix3;
492
493 ConstQuaternionMap_t quat_map(q.derived().data());
494 assert(quaternion::isNormalized(quat_map));
495
496 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
497 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
498 Eigen::Matrix<Scalar, NQ, NV, JacobianPlainType::Options | Eigen::RowMajor>
499 Jexp3QuatCoeffWise;
500
501 Scalar theta;
502 const Vector3 v = quaternion::log3(quat_map, theta);
503 quaternion::Jexp3CoeffWise(v, Jexp3QuatCoeffWise);
504 Matrix3 Jlog;
505 Jlog3(theta, v, Jlog);
506 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
507
508 // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the
509 // sign.
510 if (quat_map.coeffs()[3] >= 0.) // comes from the log3 for quaternions which may change the
511 // sign.
512 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = Jexp3QuatCoeffWise * Jlog;
513 else
514 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = -Jexp3QuatCoeffWise * Jlog;
515
516 // Jexp3(quat_map,PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template
517 // topLeftCorner<NQ,NV>());
518 }
519
520 template<class Config_t, class Tangent_t, class JacobianOut_t>
521 static void dIntegrate_dq_impl(
522 const Eigen::MatrixBase<Config_t> & /*q*/,
523 const Eigen::MatrixBase<Tangent_t> & v,
524 const Eigen::MatrixBase<JacobianOut_t> & J,
525 const AssignmentOperatorType op = SETTO)
526 {
527 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
528 switch (op)
529 {
530 case SETTO:
531 Jout = exp3(-v);
532 break;
533 case ADDTO:
534 Jout += exp3(-v);
535 break;
536 case RMTO:
537 Jout -= exp3(-v);
538 break;
539 default:
540 assert(false && "Wrong Op requesed value");
541 break;
542 }
543 }
544
545 template<class Config_t, class Tangent_t, class JacobianOut_t>
546 static void dIntegrate_dv_impl(
547 const Eigen::MatrixBase<Config_t> & /*q*/,
548 const Eigen::MatrixBase<Tangent_t> & v,
549 const Eigen::MatrixBase<JacobianOut_t> & J,
550 const AssignmentOperatorType op = SETTO)
551 {
552 switch (op)
553 {
554 case SETTO:
555 Jexp3<SETTO>(v, J.derived());
556 break;
557 case ADDTO:
558 Jexp3<ADDTO>(v, J.derived());
559 break;
560 case RMTO:
561 Jexp3<RMTO>(v, J.derived());
562 break;
563 default:
564 assert(false && "Wrong Op requesed value");
565 break;
566 }
567 }
568
569 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
570 static void dIntegrateTransport_dq_impl(
571 const Eigen::MatrixBase<Config_t> & /*q*/,
572 const Eigen::MatrixBase<Tangent_t> & v,
573 const Eigen::MatrixBase<JacobianIn_t> & Jin,
574 const Eigen::MatrixBase<JacobianOut_t> & J_out)
575 {
576 typedef typename SE3::Matrix3 Matrix3;
577 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
578 const Matrix3 Jtmp3 = exp3(-v);
579 Jout.noalias() = Jtmp3 * Jin;
580 }
581
582 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
583 static void dIntegrateTransport_dv_impl(
584 const Eigen::MatrixBase<Config_t> & /*q*/,
585 const Eigen::MatrixBase<Tangent_t> & v,
586 const Eigen::MatrixBase<JacobianIn_t> & Jin,
587 const Eigen::MatrixBase<JacobianOut_t> & J_out)
588 {
589 typedef typename SE3::Matrix3 Matrix3;
590 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
591 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
592 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
593 Matrix3 Jtmp3;
594 Jexp3<SETTO>(v, Jtmp3);
595 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
596 Jout.noalias() = Jtmp3 * Jin;
597 }
598
599 template<class Config_t, class Tangent_t, class Jacobian_t>
600 static void dIntegrateTransport_dq_impl(
601 const Eigen::MatrixBase<Config_t> & /*q*/,
602 const Eigen::MatrixBase<Tangent_t> & v,
603 const Eigen::MatrixBase<Jacobian_t> & J_out)
604 {
605 typedef typename SE3::Matrix3 Matrix3;
606 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
607 const Matrix3 Jtmp3 = exp3(-v);
608 Jout = Jtmp3 * Jout;
609 }
610
611 template<class Config_t, class Tangent_t, class Jacobian_t>
612 static void dIntegrateTransport_dv_impl(
613 const Eigen::MatrixBase<Config_t> & /*q*/,
614 const Eigen::MatrixBase<Tangent_t> & v,
615 const Eigen::MatrixBase<Jacobian_t> & J_out)
616 {
617 typedef typename SE3::Matrix3 Matrix3;
618 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
619 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
620 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
621 Matrix3 Jtmp3;
622 Jexp3<SETTO>(v, Jtmp3);
623 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
624 Jout = Jtmp3 * Jout;
625 }
626
627 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
628 static void interpolate_impl(
629 const Eigen::MatrixBase<ConfigL_t> & q0,
630 const Eigen::MatrixBase<ConfigR_t> & q1,
631 const Scalar & u,
632 const Eigen::MatrixBase<ConfigOut_t> & qout)
633 {
634 ConstQuaternionMap_t quat0(q0.derived().data());
635 assert(quaternion::isNormalized(quat0));
636 ConstQuaternionMap_t quat1(q1.derived().data());
637 assert(quaternion::isNormalized(quat1));
638
639 QuaternionMap_t quat_res(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data());
640
641 TangentVector_t w;
642 difference_impl(q0, q1, w);
643 integrate_impl(q0, u * w, qout);
644 assert(quaternion::isNormalized(quat_res));
645 }
646
647 template<class ConfigL_t, class ConfigR_t>
648 static Scalar squaredDistance_impl(
649 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1)
650 {
651 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
652 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
653 TangentVector_t t;
654 difference_impl(q0, q1, t);
655 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
656 return t.squaredNorm();
657 }
658
659 template<class Config_t>
660 static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
661 {
662 pinocchio::normalize(qout.const_cast_derived());
663 }
664
665 template<class Config_t>
666 static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
667 {
668 const Scalar norm = qin.norm();
669 using std::abs;
670 return abs(norm - Scalar(1.0)) < prec;
671 }
672
673 template<class Config_t>
674 static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
675 {
676 QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).data());
677 quaternion::uniformRandom(quat_map);
678
679 assert(quaternion::isNormalized(quat_map));
680 }
681
682 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
683 static void randomConfiguration_impl(
684 const Eigen::MatrixBase<ConfigL_t> &,
685 const Eigen::MatrixBase<ConfigR_t> &,
686 const Eigen::MatrixBase<ConfigOut_t> & qout)
687 {
688 random_impl(qout);
689 }
690
691 template<class ConfigL_t, class ConfigR_t>
692 static bool isSameConfiguration_impl(
693 const Eigen::MatrixBase<ConfigL_t> & q0,
694 const Eigen::MatrixBase<ConfigR_t> & q1,
695 const Scalar & prec)
696 {
697 ConstQuaternionMap_t quat1(q0.derived().data());
698 assert(quaternion::isNormalized(quat1));
699 ConstQuaternionMap_t quat2(q1.derived().data());
700 assert(quaternion::isNormalized(quat1));
701
702 return quaternion::defineSameRotation(quat1, quat2, prec);
703 }
704 }; // struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options>
705
706} // namespace pinocchio
707
708#endif // ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__
pinocchio::quaternion::defineSameRotation
bool defineSameRotation(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2, const typename D1::RealScalar &prec=Eigen::NumTraits< typename D1::Scalar >::dummy_precision())
Check if two quaternions define the same rotations.
Definition quaternion.hpp:58
pinocchio::quaternion::isNormalized
bool isNormalized(const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec)
Check whether the input quaternion is Normalized within the given precision.
Definition quaternion.hpp:230
pinocchio::quaternion::Jexp3CoeffWise
void Jexp3CoeffWise(const Eigen::MatrixBase< Vector3Like > &v, const Eigen::MatrixBase< Matrix43Like > &Jexp)
Derivative of where is a small perturbation of at identity.
Definition explog-quaternion.hpp:292
pinocchio::quaternion::firstOrderNormalize
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
Definition quaternion.hpp:90
pinocchio::quaternion::log3
Eigen::Matrix< typename QuaternionLike::Scalar, 3, 1, typename QuaternionLike::Vector3 ::Options > log3(const Eigen::QuaternionBase< QuaternionLike > &quat, typename QuaternionLike::Scalar &theta)
Same as log3 but with a unit quaternion as input.
Definition explog-quaternion.hpp:211
pinocchio::quaternion::exp3
void exp3(const Eigen::MatrixBase< Vector3Like > &v, Eigen::QuaternionBase< QuaternionLike > &quat_out)
Exp: so3 -> SO3 (quaternion)
Definition explog-quaternion.hpp:27
pinocchio::quaternion::uniformRandom
void uniformRandom(Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition quaternion.hpp:115
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
pinocchio::nv
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition explog.hpp:240
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition sincos.hpp:27
pinocchio::nq
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
pinocchio::exp3
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, Vector3Like ::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
Definition explog.hpp:36
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
pinocchio::isNormalized
bool isNormalized(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Check whether a configuration vector is normalized within the given precision provided by prec.
pinocchio::SpecialOrthogonalOperationTpl< 2, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition special-orthogonal.hpp:118
pinocchio::SpecialOrthogonalOperationTpl< 2, _Scalar, _Options >::nq
static Index nq()
Definition special-orthogonal.hpp:112
pinocchio::SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nv
static Index nv()
Get dimension of Lie Group tangent space.
Definition special-orthogonal.hpp:387
pinocchio::SpecialOrthogonalOperationTpl< 3, _Scalar, _Options >::nq
static Index nq()
Definition special-orthogonal.hpp:381
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72