| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/liegroup/special-orthogonal.hpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 146 | 273 | 53.5% |
| Branches: | 109 | 434 | 25.1% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 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 | |||
| 15 | namespace pinocchio | ||
| 16 | { | ||
| 17 | template<int Dim, typename Scalar, int Options = 0> | ||
| 18 | struct SpecialOrthogonalOperationTpl | ||
| 19 | { | ||
| 20 | }; | ||
| 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 | 136 | 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 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | const Scalar tr = R.trace(); |
| 66 | |||
| 67 |
3/4✓ Branch 0 taken 1 times.
✓ Branch 1 taken 135 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
136 | static const Scalar PI_value = PI<Scalar>(); |
| 68 | |||
| 69 | using internal::if_then_else; | ||
| 70 | 272 | Scalar theta = if_then_else( | |
| 71 | ✗ | internal::GT, tr, Scalar(2), | |
| 72 | ✗ | Scalar(0), // then | |
| 73 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | if_then_else( |
| 74 | ✗ | internal::LT, tr, Scalar(-2), | |
| 75 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | if_then_else( |
| 76 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | internal::GE, R(1, 0), Scalar(0), PI_value, |
| 77 | ✗ | static_cast<Scalar>(-PI_value)), // then | |
| 78 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | if_then_else( |
| 79 | internal::GT, tr, | ||
| 80 | ✗ | static_cast<Scalar>(Scalar(2) - Scalar(1e-2)), // TODO: change value | |
| 81 |
2/4✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 136 times.
✗ Branch 5 not taken.
|
136 | static_cast<Scalar>(asin((R(1, 0) - R(0, 1)) / Scalar(2))), // then |
| 82 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | if_then_else( |
| 83 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | internal::GE, R(1, 0), Scalar(0), |
| 84 | ✗ | static_cast<Scalar>(acos(tr / Scalar(2))), // then | |
| 85 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | 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 |
1/4✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
136 | 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 | 136 | 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 | |||
| 108 | /// Get dimension of Lie Group vector representation | ||
| 109 | /// | ||
| 110 | /// For instance, for SO(3), the dimension of the vector representation is | ||
| 111 | /// 4 (quaternion) while the dimension of the tangent space is 3. | ||
| 112 | ✗ | static Index nq() | |
| 113 | { | ||
| 114 | ✗ | return NQ; | |
| 115 | } | ||
| 116 | |||
| 117 | /// Get dimension of Lie Group tangent space | ||
| 118 | ✗ | static Index nv() | |
| 119 | { | ||
| 120 | ✗ | return NV; | |
| 121 | } | ||
| 122 | |||
| 123 | 1 | static ConfigVector_t neutral() | |
| 124 | { | ||
| 125 | 1 | ConfigVector_t n; | |
| 126 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | n << Scalar(1), Scalar(0); |
| 127 | 1 | 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 | 10 | static void random_impl(const Eigen::MatrixBase<Config_t> & qout) | |
| 347 | { | ||
| 348 | 10 | Config_t & out = PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout); | |
| 349 | |||
| 350 |
3/4✓ Branch 0 taken 2 times.
✓ Branch 1 taken 3 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
10 | static const Scalar PI_value = PI<Scalar>(); |
| 351 | 10 | const Scalar angle = -PI_value + Scalar(2) * PI_value * ((Scalar)rand()) / RAND_MAX; | |
| 352 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | SINCOS(angle, &out(1), &out(0)); |
| 353 | 10 | } | |
| 354 | |||
| 355 | template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> | ||
| 356 | 14 | 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 | 14 | random_impl(qout); | |
| 362 | 14 | } | |
| 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 | |||
| 377 | /// Get dimension of Lie Group vector representation | ||
| 378 | /// | ||
| 379 | /// For instance, for SO(3), the dimension of the vector representation is | ||
| 380 | /// 4 (quaternion) while the dimension of the tangent space is 3. | ||
| 381 | 6978 | static Index nq() | |
| 382 | { | ||
| 383 | 6978 | return NQ; | |
| 384 | } | ||
| 385 | |||
| 386 | /// Get dimension of Lie Group tangent space | ||
| 387 | ✗ | static Index nv() | |
| 388 | { | ||
| 389 | ✗ | return NV; | |
| 390 | } | ||
| 391 | |||
| 392 | 4 | static ConfigVector_t neutral() | |
| 393 | { | ||
| 394 | 4 | ConfigVector_t n; | |
| 395 | 4 | n.setZero(); | |
| 396 | 4 | n[3] = Scalar(1); | |
| 397 | 4 | 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 | 260 | 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 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
260 | ConstQuaternionMap_t quat0(q0.derived().data()); |
| 412 |
2/4✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 130 times.
|
260 | assert(quaternion::isNormalized( |
| 413 | quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 414 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
260 | ConstQuaternionMap_t quat1(q1.derived().data()); |
| 415 |
2/4✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 130 times.
|
260 | assert(quaternion::isNormalized( |
| 416 | quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 417 | |||
| 418 |
2/4✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 125 times.
✗ Branch 6 not taken.
|
260 | PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d) = |
| 419 |
2/4✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
|
260 | quaternion::log3(Quaternion_t(quat0.conjugate() * quat1)); |
| 420 | 260 | } | |
| 421 | |||
| 422 | template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> | ||
| 423 | 12 | static void dDifference_impl( | |
| 424 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
| 425 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
| 426 | const Eigen::MatrixBase<JacobianOut_t> & J) | ||
| 427 | { | ||
| 428 | typedef typename SE3::Matrix3 Matrix3; | ||
| 429 | |||
| 430 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
12 | ConstQuaternionMap_t quat0(q0.derived().data()); |
| 431 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
|
12 | assert(quaternion::isNormalized( |
| 432 | quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 433 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
12 | ConstQuaternionMap_t quat1(q1.derived().data()); |
| 434 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
|
12 | assert(quaternion::isNormalized( |
| 435 | quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 436 | |||
| 437 | // TODO: check whether the merge with 2.6.9 is correct | ||
| 438 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | const Quaternion_t q = quat0.conjugate() * quat1; |
| 439 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | const Matrix3 R = q.matrix(); |
| 440 | |||
| 441 | if (arg == ARG0) | ||
| 442 | { | ||
| 443 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | JacobianMatrix_t J1; |
| 444 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | Jlog3(R, J1); |
| 445 | |||
| 446 |
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 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
|
6 | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).noalias() = -J1 * R.transpose(); |
| 447 | } | ||
| 448 | else if (arg == ARG1) | ||
| 449 | { | ||
| 450 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | Jlog3(R, J); |
| 451 | } | ||
| 452 | /* | ||
| 453 | const Quaternion_t quat_diff = quat0.conjugate() * quat1; | ||
| 454 | |||
| 455 | if (arg == ARG0) { | ||
| 456 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 457 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
| 458 | JacobianMatrix_t J1; | ||
| 459 | quaternion::Jlog3(quat_diff, J1); | ||
| 460 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 461 | const Matrix3 R = (quat_diff).matrix(); | ||
| 462 | |||
| 463 | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose(); | ||
| 464 | } else if (arg == ARG1) { | ||
| 465 | quaternion::Jlog3(quat_diff, J); | ||
| 466 | } | ||
| 467 | */ | ||
| 468 | 12 | } | |
| 469 | |||
| 470 | template<class ConfigIn_t, class Velocity_t, class ConfigOut_t> | ||
| 471 | 442 | static void integrate_impl( | |
| 472 | const Eigen::MatrixBase<ConfigIn_t> & q, | ||
| 473 | const Eigen::MatrixBase<Velocity_t> & v, | ||
| 474 | const Eigen::MatrixBase<ConfigOut_t> & qout) | ||
| 475 | { | ||
| 476 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 221 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
442 | ConstQuaternionMap_t quat(q.derived().data()); |
| 477 |
2/4✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 221 times.
|
442 | assert(quaternion::isNormalized( |
| 478 | quat, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 479 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 221 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
442 | QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data()); |
| 480 | |||
| 481 |
1/2✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
|
442 | Quaternion_t pOmega; |
| 482 |
1/2✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
|
442 | quaternion::exp3(v, pOmega); |
| 483 |
2/4✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 221 times.
✗ Branch 5 not taken.
|
442 | quat_map = quat * pOmega; |
| 484 |
1/2✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
|
442 | quaternion::firstOrderNormalize(quat_map); |
| 485 |
2/4✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 221 times.
|
442 | assert(quaternion::isNormalized( |
| 486 | quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 487 | 442 | } | |
| 488 | |||
| 489 | template<class Config_t, class Jacobian_t> | ||
| 490 | ✗ | static void integrateCoeffWiseJacobian_impl( | |
| 491 | const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) | ||
| 492 | { | ||
| 493 | ✗ | assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension"); | |
| 494 | |||
| 495 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType; | ||
| 496 | typedef typename SE3::Vector3 Vector3; | ||
| 497 | typedef typename SE3::Matrix3 Matrix3; | ||
| 498 | |||
| 499 | ✗ | ConstQuaternionMap_t quat_map(q.derived().data()); | |
| 500 | ✗ | assert(quaternion::isNormalized( | |
| 501 | quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 502 | |||
| 503 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 504 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
| 505 | Eigen::Matrix<Scalar, NQ, NV, JacobianPlainType::Options | Eigen::RowMajor> | ||
| 506 | ✗ | Jexp3QuatCoeffWise; | |
| 507 | |||
| 508 | Scalar theta; | ||
| 509 | ✗ | const Vector3 v = quaternion::log3(quat_map, theta); | |
| 510 | ✗ | quaternion::Jexp3CoeffWise(v, Jexp3QuatCoeffWise); | |
| 511 | ✗ | Matrix3 Jlog; | |
| 512 | ✗ | Jlog3(theta, v, Jlog); | |
| 513 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 514 | |||
| 515 | // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the | ||
| 516 | // sign. | ||
| 517 | ✗ | if (quat_map.coeffs()[3] >= 0.) // comes from the log3 for quaternions which may change the | |
| 518 | // sign. | ||
| 519 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = Jexp3QuatCoeffWise * Jlog; | |
| 520 | else | ||
| 521 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = -Jexp3QuatCoeffWise * Jlog; | |
| 522 | |||
| 523 | // Jexp3(quat_map,PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template | ||
| 524 | // topLeftCorner<NQ,NV>()); | ||
| 525 | } | ||
| 526 | |||
| 527 | template<class Config_t, class Tangent_t, class JacobianOut_t> | ||
| 528 | 11 | static void dIntegrate_dq_impl( | |
| 529 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 530 | const Eigen::MatrixBase<Tangent_t> & v, | ||
| 531 | const Eigen::MatrixBase<JacobianOut_t> & J, | ||
| 532 | const AssignmentOperatorType op = SETTO) | ||
| 533 | { | ||
| 534 | 11 | JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J); | |
| 535 |
3/4✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
11 | switch (op) |
| 536 | { | ||
| 537 | 9 | case SETTO: | |
| 538 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
9 | Jout = exp3(-v); |
| 539 | 9 | break; | |
| 540 | 1 | case ADDTO: | |
| 541 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | Jout += exp3(-v); |
| 542 | 1 | break; | |
| 543 | 1 | case RMTO: | |
| 544 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | Jout -= exp3(-v); |
| 545 | 1 | break; | |
| 546 | ✗ | default: | |
| 547 | ✗ | assert(false && "Wrong Op requesed value"); | |
| 548 | break; | ||
| 549 | } | ||
| 550 | 11 | } | |
| 551 | |||
| 552 | template<class Config_t, class Tangent_t, class JacobianOut_t> | ||
| 553 | 11 | static void dIntegrate_dv_impl( | |
| 554 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 555 | const Eigen::MatrixBase<Tangent_t> & v, | ||
| 556 | const Eigen::MatrixBase<JacobianOut_t> & J, | ||
| 557 | const AssignmentOperatorType op = SETTO) | ||
| 558 | { | ||
| 559 |
3/4✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
11 | switch (op) |
| 560 | { | ||
| 561 | 9 | case SETTO: | |
| 562 | 9 | Jexp3<SETTO>(v, J.derived()); | |
| 563 | 9 | break; | |
| 564 | 1 | case ADDTO: | |
| 565 | 1 | Jexp3<ADDTO>(v, J.derived()); | |
| 566 | 1 | break; | |
| 567 | 1 | case RMTO: | |
| 568 | 1 | Jexp3<RMTO>(v, J.derived()); | |
| 569 | 1 | break; | |
| 570 | ✗ | default: | |
| 571 | ✗ | assert(false && "Wrong Op requesed value"); | |
| 572 | break; | ||
| 573 | } | ||
| 574 | 11 | } | |
| 575 | |||
| 576 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
| 577 | 1 | static void dIntegrateTransport_dq_impl( | |
| 578 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 579 | const Eigen::MatrixBase<Tangent_t> & v, | ||
| 580 | const Eigen::MatrixBase<JacobianIn_t> & Jin, | ||
| 581 | const Eigen::MatrixBase<JacobianOut_t> & J_out) | ||
| 582 | { | ||
| 583 | typedef typename SE3::Matrix3 Matrix3; | ||
| 584 | 1 | JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out); | |
| 585 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | const Matrix3 Jtmp3 = exp3(-v); |
| 586 |
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.
|
1 | Jout.noalias() = Jtmp3 * Jin; |
| 587 | 1 | } | |
| 588 | |||
| 589 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
| 590 | 1 | static void dIntegrateTransport_dv_impl( | |
| 591 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 592 | const Eigen::MatrixBase<Tangent_t> & v, | ||
| 593 | const Eigen::MatrixBase<JacobianIn_t> & Jin, | ||
| 594 | const Eigen::MatrixBase<JacobianOut_t> & J_out) | ||
| 595 | { | ||
| 596 | typedef typename SE3::Matrix3 Matrix3; | ||
| 597 | 1 | JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out); | |
| 598 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 599 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
| 600 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Matrix3 Jtmp3; |
| 601 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Jexp3<SETTO>(v, Jtmp3); |
| 602 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 603 |
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.
|
1 | Jout.noalias() = Jtmp3 * Jin; |
| 604 | 1 | } | |
| 605 | |||
| 606 | template<class Config_t, class Tangent_t, class Jacobian_t> | ||
| 607 | 1 | static void dIntegrateTransport_dq_impl( | |
| 608 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 609 | const Eigen::MatrixBase<Tangent_t> & v, | ||
| 610 | const Eigen::MatrixBase<Jacobian_t> & J_out) | ||
| 611 | { | ||
| 612 | typedef typename SE3::Matrix3 Matrix3; | ||
| 613 | 1 | Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out); | |
| 614 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | const Matrix3 Jtmp3 = exp3(-v); |
| 615 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Jout = Jtmp3 * Jout; |
| 616 | 1 | } | |
| 617 | |||
| 618 | template<class Config_t, class Tangent_t, class Jacobian_t> | ||
| 619 | 1 | static void dIntegrateTransport_dv_impl( | |
| 620 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
| 621 | const Eigen::MatrixBase<Tangent_t> & v, | ||
| 622 | const Eigen::MatrixBase<Jacobian_t> & J_out) | ||
| 623 | { | ||
| 624 | typedef typename SE3::Matrix3 Matrix3; | ||
| 625 | 1 | Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out); | |
| 626 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 627 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
| 628 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Matrix3 Jtmp3; |
| 629 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Jexp3<SETTO>(v, Jtmp3); |
| 630 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 631 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Jout = Jtmp3 * Jout; |
| 632 | 1 | } | |
| 633 | |||
| 634 | template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> | ||
| 635 | 2 | static void interpolate_impl( | |
| 636 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
| 637 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
| 638 | const Scalar & u, | ||
| 639 | const Eigen::MatrixBase<ConfigOut_t> & qout) | ||
| 640 | { | ||
| 641 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
2 | ConstQuaternionMap_t quat0(q0.derived().data()); |
| 642 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
|
2 | assert(quaternion::isNormalized( |
| 643 | quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 644 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
2 | ConstQuaternionMap_t quat1(q1.derived().data()); |
| 645 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
|
2 | assert(quaternion::isNormalized( |
| 646 | quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 647 | |||
| 648 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | QuaternionMap_t quat_res(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data()); |
| 649 | |||
| 650 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | TangentVector_t w; |
| 651 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | difference_impl(q0, q1, w); |
| 652 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | integrate_impl(q0, u * w, qout); |
| 653 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
|
2 | assert(quaternion::isNormalized( |
| 654 | quat_res, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 655 | 2 | } | |
| 656 | |||
| 657 | template<class ConfigL_t, class ConfigR_t> | ||
| 658 | 3 | static Scalar squaredDistance_impl( | |
| 659 | const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) | ||
| 660 | { | ||
| 661 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 662 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
| 663 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | TangentVector_t t; |
| 664 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | difference_impl(q0, q1, t); |
| 665 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 666 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | return t.squaredNorm(); |
| 667 | } | ||
| 668 | |||
| 669 | template<class Config_t> | ||
| 670 | 8 | static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) | |
| 671 | { | ||
| 672 | 8 | pinocchio::normalize(qout.const_cast_derived()); | |
| 673 | 8 | } | |
| 674 | |||
| 675 | template<class Config_t> | ||
| 676 | 7 | static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec) | |
| 677 | { | ||
| 678 | 7 | const Scalar norm = qin.norm(); | |
| 679 | using std::abs; | ||
| 680 | 7 | return abs(norm - Scalar(1.0)) < prec; | |
| 681 | } | ||
| 682 | |||
| 683 | template<class Config_t> | ||
| 684 | 4676 | static void random_impl(const Eigen::MatrixBase<Config_t> & qout) | |
| 685 | { | ||
| 686 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 2338 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4676 | QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).data()); |
| 687 |
1/2✓ Branch 1 taken 2338 times.
✗ Branch 2 not taken.
|
4676 | quaternion::uniformRandom(quat_map); |
| 688 | |||
| 689 |
2/4✓ Branch 1 taken 2338 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2338 times.
|
4676 | assert(quaternion::isNormalized( |
| 690 | quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 691 | 4676 | } | |
| 692 | |||
| 693 | template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> | ||
| 694 | 4692 | static void randomConfiguration_impl( | |
| 695 | const Eigen::MatrixBase<ConfigL_t> &, | ||
| 696 | const Eigen::MatrixBase<ConfigR_t> &, | ||
| 697 | const Eigen::MatrixBase<ConfigOut_t> & qout) | ||
| 698 | { | ||
| 699 | 4692 | random_impl(qout); | |
| 700 | 4692 | } | |
| 701 | |||
| 702 | template<class ConfigL_t, class ConfigR_t> | ||
| 703 | 6 | static bool isSameConfiguration_impl( | |
| 704 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
| 705 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
| 706 | const Scalar & prec) | ||
| 707 | { | ||
| 708 | 6 | ConstQuaternionMap_t quat1(q0.derived().data()); | |
| 709 | 6 | assert(quaternion::isNormalized( | |
| 710 | quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 711 | 6 | ConstQuaternionMap_t quat2(q1.derived().data()); | |
| 712 | 6 | assert(quaternion::isNormalized( | |
| 713 | quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
| 714 | |||
| 715 | 12 | return quaternion::defineSameRotation(quat1, quat2, prec); | |
| 716 | } | ||
| 717 | }; // struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options> | ||
| 718 | |||
| 719 | } // namespace pinocchio | ||
| 720 | |||
| 721 | #endif // ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__ | ||
| 722 |