| Directory: | ./ |
|---|---|
| File: | include/pinocchio/spatial/explog.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 235 | 243 | 96.7% |
| Branches: | 410 | 1106 | 37.1% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2023 CNRS INRIA | ||
| 3 | // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
| 4 | // | ||
| 5 | |||
| 6 | #ifndef __pinocchio_spatial_explog_hpp__ | ||
| 7 | #define __pinocchio_spatial_explog_hpp__ | ||
| 8 | |||
| 9 | #include "pinocchio/fwd.hpp" | ||
| 10 | #include "pinocchio/utils/static-if.hpp" | ||
| 11 | #include "pinocchio/math/fwd.hpp" | ||
| 12 | #include "pinocchio/math/sincos.hpp" | ||
| 13 | #include "pinocchio/math/taylor-expansion.hpp" | ||
| 14 | #include "pinocchio/spatial/motion.hpp" | ||
| 15 | #include "pinocchio/spatial/skew.hpp" | ||
| 16 | #include "pinocchio/spatial/se3.hpp" | ||
| 17 | |||
| 18 | #include <Eigen/Geometry> | ||
| 19 | |||
| 20 | #include "pinocchio/spatial/log.hpp" | ||
| 21 | |||
| 22 | namespace pinocchio | ||
| 23 | { | ||
| 24 | /// \brief Exp: so3 -> SO3. | ||
| 25 | /// | ||
| 26 | /// Return the integral of the input angular velocity during time 1. | ||
| 27 | /// | ||
| 28 | /// \param[in] v The angular velocity vector. | ||
| 29 | /// | ||
| 30 | /// \return The rotational matrix associated to the integration of the angular velocity during | ||
| 31 | /// time 1. | ||
| 32 | /// | ||
| 33 | template<typename Vector3Like> | ||
| 34 | typename Eigen:: | ||
| 35 | Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> | ||
| 36 | 2493 | exp3(const Eigen::MatrixBase<Vector3Like> & v) | |
| 37 | { | ||
| 38 |
2/4✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
|
2493 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, v, 3, 1); |
| 39 | |||
| 40 | typedef typename Vector3Like::Scalar Scalar; | ||
| 41 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3LikePlain; | ||
| 42 | typedef Eigen::Matrix<Scalar, 3, 3, Vector3LikePlain::Options> Matrix3; | ||
| 43 |
3/8✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
3 | const static Scalar eps = Eigen::NumTraits<Scalar>::epsilon(); |
| 44 | |||
| 45 |
3/6✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
2496 | const Scalar t2 = v.squaredNorm() + eps * eps; |
| 46 | |||
| 47 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2493 | const Scalar t = math::sqrt(t2); |
| 48 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
3 | Scalar ct, st; |
| 49 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2493 | SINCOS(t, &st, &ct); |
| 50 | |||
| 51 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4986 | const Scalar alpha_vxvx = internal::if_then_else( |
| 52 |
1/2✓ Branch 1 taken 1262 times.
✗ Branch 2 not taken.
|
2490 | internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 53 |
9/18✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
|
2496 | static_cast<Scalar>((1 - ct) / t2), static_cast<Scalar>(Scalar(1) / Scalar(2) - t2 / 24)); |
| 54 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4986 | const Scalar alpha_vx = internal::if_then_else( |
| 55 |
1/2✓ Branch 1 taken 1262 times.
✗ Branch 2 not taken.
|
2490 | internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 56 |
5/10✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
|
2496 | static_cast<Scalar>((st) / t), static_cast<Scalar>(Scalar(1) - t2 / 6)); |
| 57 |
4/8✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1264 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1264 times.
✗ Branch 11 not taken.
|
2496 | Matrix3 res(alpha_vxvx * v * v.transpose()); |
| 58 |
4/8✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2493 | res.coeffRef(0, 1) -= alpha_vx * v[2]; |
| 59 |
4/8✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2493 | res.coeffRef(1, 0) += alpha_vx * v[2]; |
| 60 |
4/8✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2493 | res.coeffRef(0, 2) += alpha_vx * v[1]; |
| 61 |
4/8✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2493 | res.coeffRef(2, 0) -= alpha_vx * v[1]; |
| 62 |
4/8✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2493 | res.coeffRef(1, 2) -= alpha_vx * v[0]; |
| 63 |
4/8✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2493 | res.coeffRef(2, 1) += alpha_vx * v[0]; |
| 64 | |||
| 65 |
3/6✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
4983 | ct = internal::if_then_else( |
| 66 |
1/2✓ Branch 1 taken 1262 times.
✗ Branch 2 not taken.
|
2490 | internal::GT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), ct, |
| 67 |
4/8✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2496 | static_cast<Scalar>(Scalar(1) - t2 / 2)); |
| 68 |
3/6✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1264 times.
✗ Branch 8 not taken.
|
2493 | res.diagonal().array() += ct; |
| 69 | |||
| 70 | 4986 | return res; | |
| 71 | 3 | } | |
| 72 | |||
| 73 | /// \brief Same as \ref log3 | ||
| 74 | /// | ||
| 75 | /// \param[in] R the rotation matrix. | ||
| 76 | /// \param[out] theta the angle value. | ||
| 77 | /// | ||
| 78 | /// \return The angular velocity vector associated to the rotation matrix. | ||
| 79 | /// | ||
| 80 | template<typename Matrix3Like> | ||
| 81 | Eigen:: | ||
| 82 | Matrix<typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> | ||
| 83 | 17563 | log3(const Eigen::MatrixBase<Matrix3Like> & R, typename Matrix3Like::Scalar & theta) | |
| 84 | { | ||
| 85 | typedef typename Matrix3Like::Scalar Scalar; | ||
| 86 | typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3; | ||
| 87 | 17563 | Vector3 res; | |
| 88 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
17563 | log3_impl<Scalar>::run(R, theta, res); |
| 89 | 17563 | return res; | |
| 90 | } | ||
| 91 | |||
| 92 | /// \brief Log: SO(3)-> so(3). | ||
| 93 | /// | ||
| 94 | /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$. | ||
| 95 | /// | ||
| 96 | /// \param[in] R The rotation matrix. | ||
| 97 | /// | ||
| 98 | /// \return The angular velocity vector associated to the rotation matrix. | ||
| 99 | /// | ||
| 100 | template<typename Matrix3Like> | ||
| 101 | Eigen:: | ||
| 102 | Matrix<typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> | ||
| 103 | 57 | log3(const Eigen::MatrixBase<Matrix3Like> & R) | |
| 104 | { | ||
| 105 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
3 | typename Matrix3Like::Scalar theta; |
| 106 |
1/2✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
|
114 | return log3(R.derived(), theta); |
| 107 | 3 | } | |
| 108 | |||
| 109 | /// | ||
| 110 | /// \brief Derivative of \f$ \exp{r} \f$ | ||
| 111 | /// \f[ | ||
| 112 | /// \frac{\sin{||r||}}{||r||} I_3 | ||
| 113 | /// - \frac{1-\cos{||r||}}{||r||^2} \left[ r \right]_x | ||
| 114 | /// + \frac{1}{||r||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T | ||
| 115 | /// \f] | ||
| 116 | /// | ||
| 117 | template<AssignmentOperatorType op, typename Vector3Like, typename Matrix3Like> | ||
| 118 | 498 | void Jexp3(const Eigen::MatrixBase<Vector3Like> & r, const Eigen::MatrixBase<Matrix3Like> & Jexp) | |
| 119 | { | ||
| 120 |
2/4✓ Branch 1 taken 249 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 249 times.
✗ Branch 5 not taken.
|
498 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, r, 3, 1); |
| 121 |
2/4✓ Branch 1 taken 249 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 249 times.
✗ Branch 5 not taken.
|
498 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, Jexp, 3, 3); |
| 122 | |||
| 123 | 498 | Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jexp); | |
| 124 | typedef typename Matrix3Like::Scalar Scalar; | ||
| 125 | |||
| 126 |
1/2✓ Branch 1 taken 249 times.
✗ Branch 2 not taken.
|
498 | const Scalar n2 = r.squaredNorm(); |
| 127 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
498 | const Scalar n = math::sqrt(n2); |
| 128 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
498 | const Scalar n_inv = Scalar(1) / n; |
| 129 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
498 | const Scalar n2_inv = n_inv * n_inv; |
| 130 | ✗ | Scalar cn, sn; | |
| 131 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
498 | SINCOS(n, &sn, &cn); |
| 132 | |||
| 133 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
996 | const Scalar a = internal::if_then_else( |
| 134 |
1/2✓ Branch 1 taken 249 times.
✗ Branch 2 not taken.
|
498 | internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 135 |
1/10✓ Branch 1 taken 249 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
498 | static_cast<Scalar>(Scalar(1) - n2 / Scalar(6)), static_cast<Scalar>(sn * n_inv)); |
| 136 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
996 | const Scalar b = internal::if_then_else( |
| 137 |
1/2✓ Branch 1 taken 249 times.
✗ Branch 2 not taken.
|
498 | internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 138 | ✗ | static_cast<Scalar>(-Scalar(1) / Scalar(2) - n2 / Scalar(24)), | |
| 139 |
1/8✓ Branch 1 taken 249 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
498 | static_cast<Scalar>(-(1 - cn) * n2_inv)); |
| 140 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
996 | const Scalar c = internal::if_then_else( |
| 141 |
1/2✓ Branch 1 taken 249 times.
✗ Branch 2 not taken.
|
498 | internal::LT, n, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 142 | ✗ | static_cast<Scalar>(Scalar(1) / Scalar(6) - n2 / Scalar(120)), | |
| 143 |
1/6✓ Branch 1 taken 249 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
498 | static_cast<Scalar>(n2_inv * (1 - a))); |
| 144 | |||
| 145 | switch (op) | ||
| 146 | { | ||
| 147 | case SETTO: | ||
| 148 |
2/4✓ Branch 1 taken 247 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 247 times.
✗ Branch 5 not taken.
|
494 | Jout.diagonal().setConstant(a); |
| 149 |
2/10✓ Branch 1 taken 247 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 247 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
494 | Jout(0, 1) = -b * r[2]; |
| 150 |
2/8✓ Branch 1 taken 247 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 247 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
494 | Jout(1, 0) = -Jout(0, 1); |
| 151 |
2/8✓ Branch 1 taken 247 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 247 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
494 | Jout(0, 2) = b * r[1]; |
| 152 |
2/8✓ Branch 1 taken 247 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 247 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
494 | Jout(2, 0) = -Jout(0, 2); |
| 153 |
2/10✓ Branch 1 taken 247 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 247 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
494 | Jout(1, 2) = -b * r[0]; |
| 154 |
2/8✓ Branch 1 taken 247 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 247 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
494 | Jout(2, 1) = -Jout(1, 2); |
| 155 |
5/10✓ Branch 1 taken 247 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 247 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 247 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 247 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 247 times.
✗ Branch 14 not taken.
|
494 | Jout.noalias() += c * r * r.transpose(); |
| 156 | 494 | break; | |
| 157 | case ADDTO: | ||
| 158 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | Jout.diagonal().array() += a; |
| 159 |
2/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | Jout(0, 1) += -b * r[2]; |
| 160 |
2/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | Jout(1, 0) += b * r[2]; |
| 161 |
2/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | Jout(0, 2) += b * r[1]; |
| 162 |
2/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | Jout(2, 0) += -b * r[1]; |
| 163 |
2/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | Jout(1, 2) += -b * r[0]; |
| 164 |
2/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | Jout(2, 1) += b * r[0]; |
| 165 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | Jout.noalias() += c * r * r.transpose(); |
| 166 | 2 | break; | |
| 167 | case RMTO: | ||
| 168 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | Jout.diagonal().array() -= a; |
| 169 |
2/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | Jout(0, 1) -= -b * r[2]; |
| 170 |
2/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | Jout(1, 0) -= b * r[2]; |
| 171 |
2/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | Jout(0, 2) -= b * r[1]; |
| 172 |
2/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | Jout(2, 0) -= -b * r[1]; |
| 173 |
2/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | Jout(1, 2) -= -b * r[0]; |
| 174 |
2/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | Jout(2, 1) -= b * r[0]; |
| 175 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | Jout.noalias() -= c * r * r.transpose(); |
| 176 | 2 | break; | |
| 177 | default: | ||
| 178 | assert(false && "Wrong Op requesed value"); | ||
| 179 | break; | ||
| 180 | } | ||
| 181 | 498 | } | |
| 182 | |||
| 183 | /// | ||
| 184 | /// \brief Derivative of \f$ \exp{r} \f$ | ||
| 185 | /// \f[ | ||
| 186 | /// \frac{\sin{||r||}}{||r||} I_3 | ||
| 187 | /// - \frac{1-\cos{||r||}}{||r||^2} \left[ r \right]_x | ||
| 188 | /// + \frac{1}{||r||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T | ||
| 189 | /// \f] | ||
| 190 | /// | ||
| 191 | template<typename Vector3Like, typename Matrix3Like> | ||
| 192 | 9 | void Jexp3(const Eigen::MatrixBase<Vector3Like> & r, const Eigen::MatrixBase<Matrix3Like> & Jexp) | |
| 193 | { | ||
| 194 | 9 | Jexp3<SETTO>(r, Jexp); | |
| 195 | 9 | } | |
| 196 | |||
| 197 | /** \brief Derivative of log3 | ||
| 198 | * | ||
| 199 | * This function is the right derivative of @ref log3, that is, for \f$R \in | ||
| 200 | * SO(3)\f$ and \f$\omega t in \mathfrak{so}(3)\f$, it provides the linear | ||
| 201 | * approximation: | ||
| 202 | * | ||
| 203 | * \f[ | ||
| 204 | * \log_3(R \oplus \omega t) = \log_3(R \exp_3(\omega t)) \approx \log_3(R) + \text{Jlog3}(R) | ||
| 205 | * \omega t \f] | ||
| 206 | * | ||
| 207 | * \param[in] theta the angle value. | ||
| 208 | * \param[in] log the output of log3. | ||
| 209 | * \param[out] Jlog the jacobian | ||
| 210 | * | ||
| 211 | * Equivalently, \f$\text{Jlog3}\f$ is the right Jacobian of \f$\log_3\f$: | ||
| 212 | * | ||
| 213 | * \f[ | ||
| 214 | * \text{Jlog3}(R) = \frac{\partial \log_3(R)}{\partial R} | ||
| 215 | * \f] | ||
| 216 | * | ||
| 217 | * Note that this is the right Jacobian: \f$\text{Jlog3}(R) : T_{R} SO(3) \to T_{\log_6(R)} | ||
| 218 | * \mathfrak{so}(3)\f$. (By convention, calculations in Pinocchio always perform right | ||
| 219 | * differentiation, i.e., Jacobians are in local coordinates (also known as body coordinates), | ||
| 220 | * unless otherwise specified.) | ||
| 221 | * | ||
| 222 | * If we denote by \f$\theta = \log_3(R)\f$ and \f$\log = \log_3(R, | ||
| 223 | * \theta)\f$, then \f$\text{Jlog} = \text{Jlog}_3(R)\f$ can be calculated as: | ||
| 224 | * | ||
| 225 | * \f[ | ||
| 226 | * \begin{align*} | ||
| 227 | * \text{Jlog} & = \frac{\theta \sin(\theta)}{2 (1 - \cos(\theta))} I_3 | ||
| 228 | * + \frac{1}{2} \widehat{\log} | ||
| 229 | * + \left(\frac{1}{\theta^2} - \frac{\sin(\theta)}{2\theta(1-\cos(\theta))}\right) | ||
| 230 | * \log \log^T \\ & = I_3 | ||
| 231 | * + \frac{1}{2} \widehat{\log} | ||
| 232 | * + \left(\frac{1}{\theta^2} - \frac{1 + \cos \theta}{2 \theta \sin \theta}\right) | ||
| 233 | * \widehat{\log}^2 \end{align*} \f] | ||
| 234 | * | ||
| 235 | * where \f$\widehat{v}\f$ denotes the skew-symmetric matrix obtained from the 3D vector \f$v\f$. | ||
| 236 | * | ||
| 237 | * \note The inputs must be such that \f$ \theta = \Vert \log \Vert \f$. | ||
| 238 | */ | ||
| 239 | template<typename Scalar, typename Vector3Like, typename Matrix3Like> | ||
| 240 | 1159 | void Jlog3( | |
| 241 | const Scalar & theta, | ||
| 242 | const Eigen::MatrixBase<Vector3Like> & log, | ||
| 243 | const Eigen::MatrixBase<Matrix3Like> & Jlog) | ||
| 244 | { | ||
| 245 | 1159 | Jlog3_impl<Scalar>::run(theta, log, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jlog)); | |
| 246 | 1159 | } | |
| 247 | |||
| 248 | /** \brief Derivative of log3 | ||
| 249 | * | ||
| 250 | * \param[in] R the rotation matrix. | ||
| 251 | * \param[out] Jlog the jacobian | ||
| 252 | * | ||
| 253 | * Equivalent to | ||
| 254 | * \code | ||
| 255 | * double theta; | ||
| 256 | * Vector3 log = pinocchio::log3 (R, theta); | ||
| 257 | * pinocchio::Jlog3 (theta, log, Jlog); | ||
| 258 | * \endcode | ||
| 259 | */ | ||
| 260 | template<typename Matrix3Like1, typename Matrix3Like2> | ||
| 261 | void | ||
| 262 | 376 | Jlog3(const Eigen::MatrixBase<Matrix3Like1> & R, const Eigen::MatrixBase<Matrix3Like2> & Jlog) | |
| 263 | { | ||
| 264 | typedef typename Matrix3Like1::Scalar Scalar; | ||
| 265 | typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3; | ||
| 266 | |||
| 267 | ✗ | Scalar t; | |
| 268 |
1/2✓ Branch 1 taken 204 times.
✗ Branch 2 not taken.
|
376 | Vector3 w(log3(R, t)); |
| 269 |
1/2✓ Branch 2 taken 204 times.
✗ Branch 3 not taken.
|
376 | Jlog3(t, w, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2, Jlog)); |
| 270 | 376 | } | |
| 271 | |||
| 272 | template<typename Scalar, typename Vector3Like1, typename Vector3Like2, typename Matrix3Like> | ||
| 273 | 3 | void Hlog3( | |
| 274 | const Scalar & theta, | ||
| 275 | const Eigen::MatrixBase<Vector3Like1> & log, | ||
| 276 | const Eigen::MatrixBase<Vector3Like2> & v, | ||
| 277 | const Eigen::MatrixBase<Matrix3Like> & vt_Hlog) | ||
| 278 | { | ||
| 279 | typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3; | ||
| 280 | 3 | Matrix3Like & vt_Hlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, vt_Hlog); | |
| 281 | |||
| 282 | // theta = (log^T * log)^.5 | ||
| 283 | // dt/dl = .5 * 2 * log^T * (log^T * log)^-.5 | ||
| 284 | // = log^T / theta | ||
| 285 | // dt_dl = log / theta | ||
| 286 | Scalar ctheta, stheta; | ||
| 287 | 3 | SINCOS(theta, &stheta, &ctheta); | |
| 288 | |||
| 289 | 3 | Scalar denom = .5 / (1 - ctheta), a = theta * stheta * denom, | |
| 290 | 3 | da_dt = (stheta - theta) * denom, // da / dtheta | |
| 291 | 3 | b = (1 - a) / (theta * theta), | |
| 292 | // db_dt = - (2 * (1 - a) / theta + da_dt ) / theta**2; // db / dtheta | ||
| 293 | 3 | db_dt = -(2 / theta - (theta + stheta) * denom) / (theta * theta); // db / dtheta | |
| 294 | |||
| 295 | // Compute dl_dv_v = Jlog * v | ||
| 296 | // Jlog = a I3 + .5 [ log ] + b * log * log^T | ||
| 297 | // if v == log, then Jlog * v == v | ||
| 298 |
10/20✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
|
3 | Vector3 dl_dv_v(a * v + .5 * log.cross(v) + b * log * log.transpose() * v); |
| 299 | |||
| 300 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Scalar dt_dv_v = log.dot(dl_dv_v) / theta; |
| 301 | |||
| 302 | // Derivative of b * log * log^T | ||
| 303 |
5/10✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
|
3 | vt_Hlog_.noalias() = db_dt * dt_dv_v * log * log.transpose(); |
| 304 |
5/10✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
|
3 | vt_Hlog_.noalias() += b * dl_dv_v * log.transpose(); |
| 305 |
5/10✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
|
3 | vt_Hlog_.noalias() += b * log * dl_dv_v.transpose(); |
| 306 | // Derivative of .5 * [ log ] | ||
| 307 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | addSkew(.5 * dl_dv_v, vt_Hlog_); |
| 308 | // Derivative of a * I3 | ||
| 309 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
3 | vt_Hlog_.diagonal().array() += da_dt * dt_dv_v; |
| 310 | 3 | } | |
| 311 | |||
| 312 | /** \brief Second order derivative of log3 | ||
| 313 | * | ||
| 314 | * This computes \f$ v^T H_{log} \f$. | ||
| 315 | * | ||
| 316 | * \param[in] R the rotation matrix. | ||
| 317 | * \param[in] v the 3D vector. | ||
| 318 | * \param[out] vt_Hlog the product of the Hessian with the input vector | ||
| 319 | */ | ||
| 320 | template<typename Matrix3Like1, typename Vector3Like, typename Matrix3Like2> | ||
| 321 | 3 | void Hlog3( | |
| 322 | const Eigen::MatrixBase<Matrix3Like1> & R, | ||
| 323 | const Eigen::MatrixBase<Vector3Like> & v, | ||
| 324 | const Eigen::MatrixBase<Matrix3Like2> & vt_Hlog) | ||
| 325 | { | ||
| 326 | typedef typename Matrix3Like1::Scalar Scalar; | ||
| 327 | typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like1)::Options> Vector3; | ||
| 328 | |||
| 329 | Scalar t; | ||
| 330 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Vector3 w(log3(R, t)); |
| 331 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | Hlog3(t, w, v, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2, vt_Hlog)); |
| 332 | 3 | } | |
| 333 | |||
| 334 | /// | ||
| 335 | /// \brief Exp: se3 -> SE3. | ||
| 336 | /// | ||
| 337 | /// Return the integral of the input twist during time 1. | ||
| 338 | /// | ||
| 339 | /// \param[in] nu The input twist. | ||
| 340 | /// | ||
| 341 | /// \return The rigid transformation associated to the integration of the twist during time 1. | ||
| 342 | /// | ||
| 343 | template<typename MotionDerived> | ||
| 344 | SE3Tpl< | ||
| 345 | typename MotionDerived::Scalar, | ||
| 346 | PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options> | ||
| 347 | 76895 | exp6(const MotionDense<MotionDerived> & nu) | |
| 348 | { | ||
| 349 | typedef typename MotionDerived::Scalar Scalar; | ||
| 350 | enum | ||
| 351 | { | ||
| 352 | Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options | ||
| 353 | }; | ||
| 354 | |||
| 355 | typedef SE3Tpl<Scalar, Options> SE3; | ||
| 356 | |||
| 357 |
1/2✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
|
76895 | SE3 res; |
| 358 |
1/2✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
|
76895 | typename SE3::LinearType & trans = res.translation(); |
| 359 |
1/2✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
|
76895 | typename SE3::AngularType & rot = res.rotation(); |
| 360 | |||
| 361 |
1/2✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
|
76895 | const typename MotionDerived::ConstAngularType & w = nu.angular(); |
| 362 |
1/2✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
|
76895 | const typename MotionDerived::ConstLinearType & v = nu.linear(); |
| 363 |
4/8✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
2 | const static Scalar eps = Eigen::NumTraits<Scalar>::epsilon(); |
| 364 | |||
| 365 |
4/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2 | Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term; |
| 366 |
3/6✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
76897 | const Scalar t2 = w.squaredNorm() + eps * eps; |
| 367 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
76895 | const Scalar t = math::sqrt(t2); |
| 368 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | Scalar ct, st; |
| 369 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
76895 | SINCOS(t, &st, &ct); |
| 370 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
76895 | const Scalar inv_t2 = Scalar(1) / t2; |
| 371 | |||
| 372 |
3/6✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
153788 | alpha_wxv = internal::if_then_else( |
| 373 |
1/2✓ Branch 1 taken 45105 times.
✗ Branch 2 not taken.
|
76893 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 374 |
6/12✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
|
4 | static_cast<Scalar>(Scalar(1) / Scalar(2) - t2 / 24), |
| 375 |
3/6✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
76897 | static_cast<Scalar>((Scalar(1) - ct) * inv_t2)); |
| 376 | |||
| 377 |
3/6✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
153788 | alpha_v = internal::if_then_else( |
| 378 |
1/2✓ Branch 1 taken 45105 times.
✗ Branch 2 not taken.
|
76893 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 379 |
5/10✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
|
76897 | static_cast<Scalar>(Scalar(1) - t2 / 6), static_cast<Scalar>((st) / t)); |
| 380 | |||
| 381 |
3/6✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
153788 | alpha_w = internal::if_then_else( |
| 382 |
1/2✓ Branch 1 taken 45105 times.
✗ Branch 2 not taken.
|
76893 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 383 |
6/12✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
|
4 | static_cast<Scalar>((Scalar(1) / Scalar(6) - t2 / 120)), |
| 384 |
3/6✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
76897 | static_cast<Scalar>((Scalar(1) - alpha_v) * inv_t2)); |
| 385 | |||
| 386 |
3/6✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
153788 | diagonal_term = internal::if_then_else( |
| 387 |
1/2✓ Branch 1 taken 45105 times.
✗ Branch 2 not taken.
|
76893 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 388 |
4/8✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
76897 | static_cast<Scalar>(Scalar(1) - t2 / 2), ct); |
| 389 | |||
| 390 | // Linear | ||
| 391 |
10/20✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45107 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45107 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 45107 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 45107 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 45107 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 45107 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 45107 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
|
76895 | trans.noalias() = (alpha_v * v + (alpha_w * w.dot(v)) * w + alpha_wxv * w.cross(v)); |
| 392 | |||
| 393 | // Rotational | ||
| 394 |
5/10✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45107 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45107 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 45107 times.
✗ Branch 14 not taken.
|
76895 | rot.noalias() = alpha_wxv * w * w.transpose(); |
| 395 |
4/8✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
76895 | rot.coeffRef(0, 1) -= alpha_v * w[2]; |
| 396 |
4/8✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
76895 | rot.coeffRef(1, 0) += alpha_v * w[2]; |
| 397 |
4/8✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
76895 | rot.coeffRef(0, 2) += alpha_v * w[1]; |
| 398 |
4/8✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
76895 | rot.coeffRef(2, 0) -= alpha_v * w[1]; |
| 399 |
4/8✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
76895 | rot.coeffRef(1, 2) -= alpha_v * w[0]; |
| 400 |
4/8✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
76895 | rot.coeffRef(2, 1) += alpha_v * w[0]; |
| 401 |
3/6✓ Branch 1 taken 45107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45107 times.
✗ Branch 8 not taken.
|
76895 | rot.diagonal().array() += diagonal_term; |
| 402 | |||
| 403 | 153790 | return res; | |
| 404 | 2 | } | |
| 405 | |||
| 406 | /// \brief Exp: se3 -> SE3. | ||
| 407 | /// | ||
| 408 | /// Return the integral of the input spatial velocity during time 1. | ||
| 409 | /// | ||
| 410 | /// \param[in] v The twist represented by a vector. | ||
| 411 | /// | ||
| 412 | /// \return The rigid transformation associated to the integration of the twist vector during | ||
| 413 | /// time 1. | ||
| 414 | /// | ||
| 415 | template<typename Vector6Like> | ||
| 416 | SE3Tpl<typename Vector6Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options> | ||
| 417 | 4 | exp6(const Eigen::MatrixBase<Vector6Like> & v) | |
| 418 | { | ||
| 419 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector6Like, v, 6, 1); |
| 420 | |||
| 421 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | MotionRef<const Vector6Like> nu(v.derived()); |
| 422 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | return exp6(nu); |
| 423 | } | ||
| 424 | |||
| 425 | /// \brief Log: SE3 -> se3. | ||
| 426 | /// | ||
| 427 | /// Pseudo-inverse of exp from \f$ SE3 \to { v,\omega \in \mathfrak{se}(3), ||\omega|| < 2\pi } | ||
| 428 | /// \f$. | ||
| 429 | /// | ||
| 430 | /// \param[in] M The rigid transformation. | ||
| 431 | /// | ||
| 432 | /// \return The twist associated to the rigid transformation during time 1. | ||
| 433 | /// | ||
| 434 | template<typename Scalar, int Options> | ||
| 435 | 14823 | MotionTpl<Scalar, Options> log6(const SE3Tpl<Scalar, Options> & M) | |
| 436 | { | ||
| 437 | typedef MotionTpl<Scalar, Options> Motion; | ||
| 438 | 14823 | Motion mout; | |
| 439 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
14823 | log6_impl<Scalar>::run(M, mout); |
| 440 | 14823 | return mout; | |
| 441 | } | ||
| 442 | |||
| 443 | /// \brief Log: SE3 -> se3. | ||
| 444 | /// | ||
| 445 | /// Pseudo-inverse of exp from \f$ SE3 \to { v,\omega \in \mathfrak{se}(3), ||\omega|| < 2\pi } | ||
| 446 | /// \f$, using the quaternion representation of the rotation. | ||
| 447 | /// | ||
| 448 | /// \param[in] quat The rotation quaternion. | ||
| 449 | /// \param[in] vec The translation vector. | ||
| 450 | /// | ||
| 451 | /// \return The twist associated to the rigid transformation during time 1. | ||
| 452 | /// | ||
| 453 | template<typename Vector3Like, typename QuaternionLike> | ||
| 454 | 8075 | MotionTpl<typename Vector3Like::Scalar, Vector3Like::Options> log6( | |
| 455 | const Eigen::QuaternionBase<QuaternionLike> & quat, const Eigen::MatrixBase<Vector3Like> & vec) | ||
| 456 | { | ||
| 457 | typedef typename Vector3Like::Scalar Scalar; | ||
| 458 | typedef MotionTpl<Scalar, Vector3Like::Options> Motion; | ||
| 459 | 8075 | Motion mout; | |
| 460 |
1/2✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
|
8075 | log6_impl<Scalar>::run(quat, vec, mout); |
| 461 | 8075 | return mout; | |
| 462 | } | ||
| 463 | |||
| 464 | /// \brief Log: SE3 -> se3. | ||
| 465 | /// | ||
| 466 | /// Pseudo-inverse of exp from \f$ SE3 \to { v,\omega \in \mathfrak{se}(3), ||\omega|| < 2\pi } | ||
| 467 | /// \f$. | ||
| 468 | /// | ||
| 469 | /// \param[in] M The rigid transformation represented as an homogenous matrix. | ||
| 470 | /// | ||
| 471 | /// \return The twist associated to the rigid transformation during time 1. | ||
| 472 | /// | ||
| 473 | template<typename Matrix4Like> | ||
| 474 | MotionTpl<typename Matrix4Like::Scalar, Eigen::internal::traits<Matrix4Like>::Options> | ||
| 475 | 3 | log6(const Eigen::MatrixBase<Matrix4Like> & M) | |
| 476 | { | ||
| 477 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, M, 4, 4); |
| 478 | |||
| 479 | typedef typename Matrix4Like::Scalar Scalar; | ||
| 480 | enum | ||
| 481 | { | ||
| 482 | Options = Eigen::internal::traits<Matrix4Like>::Options | ||
| 483 | }; | ||
| 484 | typedef MotionTpl<Scalar, Options> Motion; | ||
| 485 | typedef SE3Tpl<Scalar, Options> SE3; | ||
| 486 | |||
| 487 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | SE3 m(M); |
| 488 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Motion mout; |
| 489 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | log6_impl<Scalar>::run(m, mout); |
| 490 | 6 | return mout; | |
| 491 | } | ||
| 492 | |||
| 493 | /// \brief Derivative of exp6 | ||
| 494 | /// Computed as the inverse of Jlog6 | ||
| 495 | template<AssignmentOperatorType op, typename MotionDerived, typename Matrix6Like> | ||
| 496 | 274 | void Jexp6(const MotionDense<MotionDerived> & nu, const Eigen::MatrixBase<Matrix6Like> & Jexp) | |
| 497 | { | ||
| 498 |
2/4✓ Branch 1 taken 137 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 137 times.
✗ Branch 5 not taken.
|
274 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, Jexp, 6, 6); |
| 499 | |||
| 500 | typedef typename MotionDerived::Scalar Scalar; | ||
| 501 | typedef typename MotionDerived::Vector3 Vector3; | ||
| 502 | typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3; | ||
| 503 | 274 | Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, Jexp); | |
| 504 | |||
| 505 |
1/2✓ Branch 1 taken 137 times.
✗ Branch 2 not taken.
|
274 | const typename MotionDerived::ConstLinearType & v = nu.linear(); |
| 506 |
1/2✓ Branch 1 taken 137 times.
✗ Branch 2 not taken.
|
274 | const typename MotionDerived::ConstAngularType & w = nu.angular(); |
| 507 |
1/2✓ Branch 1 taken 137 times.
✗ Branch 2 not taken.
|
274 | const Scalar t2 = w.squaredNorm(); |
| 508 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
274 | const Scalar t = math::sqrt(t2); |
| 509 | |||
| 510 |
0/6✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
274 | const Scalar tinv = Scalar(1) / t, t2inv = tinv * tinv; |
| 511 | ✗ | Scalar st, ct; | |
| 512 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
274 | SINCOS(t, &st, &ct); |
| 513 |
0/12✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
274 | const Scalar inv_2_2ct = Scalar(1) / (Scalar(2) * (Scalar(1) - ct)); |
| 514 | |||
| 515 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
548 | const Scalar beta = internal::if_then_else( |
| 516 |
1/2✓ Branch 1 taken 137 times.
✗ Branch 2 not taken.
|
274 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 517 | ✗ | static_cast<Scalar>(Scalar(1) / Scalar(12) + t2 / Scalar(720)), | |
| 518 |
1/6✓ Branch 1 taken 137 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
274 | static_cast<Scalar>(t2inv - st * tinv * inv_2_2ct)); |
| 519 | |||
| 520 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
548 | const Scalar beta_dot_over_theta = internal::if_then_else( |
| 521 |
1/2✓ Branch 1 taken 137 times.
✗ Branch 2 not taken.
|
274 | internal::LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(), |
| 522 | ✗ | static_cast<Scalar>(Scalar(1) / Scalar(360)), | |
| 523 | ✗ | static_cast<Scalar>( | |
| 524 |
1/4✓ Branch 1 taken 137 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
274 | -Scalar(2) * t2inv * t2inv + (Scalar(1) + st * tinv) * t2inv * inv_2_2ct)); |
| 525 | |||
| 526 | switch (op) | ||
| 527 | { | ||
| 528 | case SETTO: { | ||
| 529 |
2/4✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
|
270 | Jexp3<SETTO>(w, Jout.template bottomRightCorner<3, 3>()); |
| 530 |
3/6✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
|
270 | Jout.template topLeftCorner<3, 3>() = Jout.template bottomRightCorner<3, 3>(); |
| 531 |
4/8✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 135 times.
✗ Branch 11 not taken.
|
270 | const Vector3 p = Jout.template topLeftCorner<3, 3>().transpose() * v; |
| 532 |
1/2✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
|
270 | const Scalar wTp(w.dot(p)); |
| 533 |
11/38✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 135 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 135 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 135 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 135 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 135 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 135 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 135 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 135 times.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
|
810 | const Matrix3 J( |
| 534 |
2/4✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
|
270 | alphaSkew(.5, p) + (beta_dot_over_theta * wTp) * w * w.transpose() |
| 535 |
2/4✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
|
270 | - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p * w.transpose() |
| 536 |
2/4✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
|
540 | + wTp * beta * Matrix3::Identity() + beta * w * p.transpose()); |
| 537 |
6/12✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 135 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 135 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 135 times.
✗ Branch 17 not taken.
|
270 | Jout.template topRightCorner<3, 3>().noalias() = -Jout.template topLeftCorner<3, 3>() * J; |
| 538 |
2/4✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
|
270 | Jout.template bottomLeftCorner<3, 3>().setZero(); |
| 539 | 270 | break; | |
| 540 | } | ||
| 541 | case ADDTO: { | ||
| 542 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 543 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
| 544 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Matrix3 Jtmp3; |
| 545 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Jexp3<SETTO>(w, Jtmp3); |
| 546 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 547 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout.template bottomRightCorner<3, 3>() += Jtmp3; |
| 548 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout.template topLeftCorner<3, 3>() += Jtmp3; |
| 549 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | const Vector3 p = Jtmp3.transpose() * v; |
| 550 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Scalar wTp(w.dot(p)); |
| 551 |
11/38✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
|
6 | const Matrix3 J( |
| 552 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | alphaSkew(.5, p) + (beta_dot_over_theta * wTp) * w * w.transpose() |
| 553 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p * w.transpose() |
| 554 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | + wTp * beta * Matrix3::Identity() + beta * w * p.transpose()); |
| 555 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | Jout.template topRightCorner<3, 3>().noalias() += -Jtmp3 * J; |
| 556 | 2 | break; | |
| 557 | } | ||
| 558 | case RMTO: { | ||
| 559 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 560 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
| 561 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Matrix3 Jtmp3; |
| 562 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Jexp3<SETTO>(w, Jtmp3); |
| 563 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 564 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout.template bottomRightCorner<3, 3>() -= Jtmp3; |
| 565 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Jout.template topLeftCorner<3, 3>() -= Jtmp3; |
| 566 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | const Vector3 p = Jtmp3.transpose() * v; |
| 567 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Scalar wTp(w.dot(p)); |
| 568 |
11/38✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
|
6 | const Matrix3 J( |
| 569 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | alphaSkew(.5, p) + (beta_dot_over_theta * wTp) * w * w.transpose() |
| 570 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p * w.transpose() |
| 571 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | + wTp * beta * Matrix3::Identity() + beta * w * p.transpose()); |
| 572 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | Jout.template topRightCorner<3, 3>().noalias() -= -Jtmp3 * J; |
| 573 | 2 | break; | |
| 574 | } | ||
| 575 | default: | ||
| 576 | assert(false && "Wrong Op requesed value"); | ||
| 577 | break; | ||
| 578 | } | ||
| 579 | 274 | } | |
| 580 | |||
| 581 | /// \brief Derivative of exp6 | ||
| 582 | /// Computed as the inverse of Jlog6 | ||
| 583 | template<typename MotionDerived, typename Matrix6Like> | ||
| 584 | 80 | void Jexp6(const MotionDense<MotionDerived> & nu, const Eigen::MatrixBase<Matrix6Like> & Jexp) | |
| 585 | { | ||
| 586 | 80 | Jexp6<SETTO>(nu, Jexp); | |
| 587 | 80 | } | |
| 588 | |||
| 589 | /// \brief Derivative of exp6 | ||
| 590 | /// Computed as the inverse of Jlog6 | ||
| 591 | template<typename MotionDerived> | ||
| 592 | Eigen::Matrix<typename MotionDerived::Scalar, 6, 6, MotionDerived::Options> | ||
| 593 | 1 | Jexp6(const MotionDense<MotionDerived> & nu) | |
| 594 | { | ||
| 595 | typedef typename MotionDerived::Scalar Scalar; | ||
| 596 | enum | ||
| 597 | { | ||
| 598 | Options = MotionDerived::Options | ||
| 599 | }; | ||
| 600 | typedef Eigen::Matrix<Scalar, 6, 6, Options> ReturnType; | ||
| 601 | |||
| 602 | 1 | ReturnType res; | |
| 603 | 1 | Jexp6(nu, res); | |
| 604 | 1 | return res; | |
| 605 | } | ||
| 606 | |||
| 607 | /** \brief Derivative of log6 | ||
| 608 | * | ||
| 609 | * This function is the right derivative of @ref log6, that is, for \f$M \in | ||
| 610 | * SE(3)\f$ and \f$\xi in \mathfrak{se}(3)\f$, it provides the linear | ||
| 611 | * approximation: | ||
| 612 | * | ||
| 613 | * \f[ | ||
| 614 | * \log_6(M \oplus \xi) = \log_6(M \exp_6(\xi)) \approx \log_6(M) + \text{Jlog6}(M) \xi | ||
| 615 | * \f] | ||
| 616 | * | ||
| 617 | * Equivalently, \f$\text{Jlog6}\f$ is the right Jacobian of \f$\log_6\f$: | ||
| 618 | * | ||
| 619 | * \f[ | ||
| 620 | * \text{Jlog6}(M) = \frac{\partial \log_6(M)}{\partial M} | ||
| 621 | * \f] | ||
| 622 | * | ||
| 623 | * Note that this is the right Jacobian: \f$\text{Jlog6}(M) : T_{M} SE(3) \to T_{\log_6(M)} | ||
| 624 | * \mathfrak{se}(3)\f$. (By convention, calculations in Pinocchio always perform right | ||
| 625 | * differentiation, i.e., Jacobians are in local coordinates (also known as body coordinates), | ||
| 626 | * unless otherwise specified.) | ||
| 627 | * | ||
| 628 | * Internally, it is calculated using the following formulas: | ||
| 629 | * | ||
| 630 | * \f[ | ||
| 631 | * \text{Jlog6}(M) = | ||
| 632 | * \left(\begin{array}{cc} | ||
| 633 | * \text{Jlog3}(R) & J * \text{Jlog3}(R) \\ | ||
| 634 | * 0 & \text{Jlog3}(R) \\ | ||
| 635 | * \end{array}\right) | ||
| 636 | * \f] | ||
| 637 | * where | ||
| 638 | * \f[ M = | ||
| 639 | * \left(\begin{array}{cc} | ||
| 640 | * \exp(\mathbf{r}) & \mathbf{p} \\ | ||
| 641 | * 0 & 1 \\ | ||
| 642 | * \end{array}\right) | ||
| 643 | * \f] | ||
| 644 | * \f[ | ||
| 645 | * \begin{eqnarray} | ||
| 646 | * J &=& | ||
| 647 | * \left.\frac{1}{2}[\mathbf{p}]_{\times} + \beta'(||r||) | ||
| 648 | * \frac{\mathbf{r}^T\mathbf{p}}{||r||}\mathbf{r}\mathbf{r}^T | ||
| 649 | * - (||r||\beta'(||r||) + 2 \beta(||r||)) \mathbf{p}\mathbf{r}^T\right.\\ | ||
| 650 | * &&\left. + \mathbf{r}^T\mathbf{p}\beta(||r||)I_3 + \beta(||r||)\mathbf{r}\mathbf{p}^T\right. | ||
| 651 | * \end{eqnarray} | ||
| 652 | * \f] | ||
| 653 | * and | ||
| 654 | * \f[ \beta(x)=\left(\frac{1}{x^2} - \frac{\sin x}{2x(1-\cos x)}\right) \f] | ||
| 655 | * | ||
| 656 | * | ||
| 657 | * \cheatsheet For \f$(A,B) \in SE(3)^2\f$, let \f$M_1(A, B) = A B\f$ and | ||
| 658 | * \f$m_1 = \log_6(M_1) \f$. Then, we have the following partial (right) | ||
| 659 | * Jacobians: \n | ||
| 660 | * - \f$ \frac{\partial m_1}{\partial A} = Jlog_6(M_1) Ad_B^{-1} \f$, | ||
| 661 | * - \f$ \frac{\partial m_1}{\partial B} = Jlog_6(M_1) \f$. | ||
| 662 | * | ||
| 663 | * \cheatsheet Let \f$A \in SE(3)\f$, \f$M_2(A) = A^{-1}\f$ and \f$m_2 = | ||
| 664 | * \log_6(M_2)\f$. Then, we have the following partial (right) Jacobian: \n | ||
| 665 | * - \f$ \frac{\partial m_2}{\partial A} = - Jlog_6(M_2) Ad_A \f$. | ||
| 666 | */ | ||
| 667 | template<typename Scalar, int Options, typename Matrix6Like> | ||
| 668 | 420 | void Jlog6(const SE3Tpl<Scalar, Options> & M, const Eigen::MatrixBase<Matrix6Like> & Jlog) | |
| 669 | { | ||
| 670 | 420 | Jlog6_impl<Scalar>::run(M, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, Jlog)); | |
| 671 | 420 | } | |
| 672 | |||
| 673 | /// | ||
| 674 | /// \copydoc Jlog6(const SE3Tpl<Scalar, Options> &, const Eigen::MatrixBase<Matrix6Like> &) | ||
| 675 | /// | ||
| 676 | /// \param[in] M The rigid transformation. | ||
| 677 | /// | ||
| 678 | template<typename Scalar, int Options> | ||
| 679 | 27 | Eigen::Matrix<Scalar, 6, 6, Options> Jlog6(const SE3Tpl<Scalar, Options> & M) | |
| 680 | { | ||
| 681 | typedef Eigen::Matrix<Scalar, 6, 6, Options> ReturnType; | ||
| 682 | |||
| 683 | 27 | ReturnType res; | |
| 684 | 27 | Jlog6(M, res); | |
| 685 | 27 | return res; | |
| 686 | } | ||
| 687 | |||
| 688 | template<typename Scalar, int Options> | ||
| 689 | template<typename OtherScalar> | ||
| 690 | 26 | SE3Tpl<Scalar, Options> SE3Tpl<Scalar, Options>::Interpolate( | |
| 691 | const SE3Tpl & A, const SE3Tpl & B, const OtherScalar & alpha) | ||
| 692 | { | ||
| 693 | typedef SE3Tpl<Scalar, Options> ReturnType; | ||
| 694 | typedef MotionTpl<Scalar, Options> Motion; | ||
| 695 | |||
| 696 |
2/4✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
|
26 | Motion dv = log6(A.actInv(B)); |
| 697 |
3/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
26 | ReturnType res = A * exp6(alpha * dv); |
| 698 | 52 | return res; | |
| 699 | } | ||
| 700 | |||
| 701 | } // namespace pinocchio | ||
| 702 | |||
| 703 | #include "pinocchio/spatial/explog-quaternion.hpp" | ||
| 704 | #include "pinocchio/spatial/log.hxx" | ||
| 705 | |||
| 706 | #endif // #ifndef __pinocchio_spatial_explog_hpp__ | ||
| 707 |