| Directory: | ./ |
|---|---|
| File: | include/pinocchio/spatial/inertia.hpp |
| Date: | 2025-04-30 16:14:33 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 516 | 521 | 99.0% |
| Branches: | 620 | 2222 | 27.9% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2018 CNRS | ||
| 3 | // Copyright (c) 2018-2025 INRIA | ||
| 4 | // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
| 5 | // | ||
| 6 | |||
| 7 | #ifndef __pinocchio_spatial_inertia_hpp__ | ||
| 8 | #define __pinocchio_spatial_inertia_hpp__ | ||
| 9 | |||
| 10 | #include "pinocchio/math/fwd.hpp" | ||
| 11 | #include "pinocchio/spatial/symmetric3.hpp" | ||
| 12 | #include "pinocchio/spatial/force.hpp" | ||
| 13 | #include "pinocchio/spatial/motion.hpp" | ||
| 14 | #include "pinocchio/spatial/skew.hpp" | ||
| 15 | |||
| 16 | namespace pinocchio | ||
| 17 | { | ||
| 18 | template<class Derived> | ||
| 19 | struct InertiaBase : NumericalBase<Derived> | ||
| 20 | { | ||
| 21 | SPATIAL_TYPEDEF_TEMPLATE(Derived); | ||
| 22 | |||
| 23 | 112581 | Derived & derived() | |
| 24 | { | ||
| 25 | 112581 | return *static_cast<Derived *>(this); | |
| 26 | } | ||
| 27 | 393793 | const Derived & derived() const | |
| 28 | { | ||
| 29 | 397715 | return *static_cast<const Derived *>(this); | |
| 30 | } | ||
| 31 | |||
| 32 | Derived & const_cast_derived() const | ||
| 33 | { | ||
| 34 | return *const_cast<Derived *>(&derived()); | ||
| 35 | } | ||
| 36 | |||
| 37 | Scalar mass() const | ||
| 38 | { | ||
| 39 | return static_cast<const Derived *>(this)->mass(); | ||
| 40 | } | ||
| 41 | Scalar & mass() | ||
| 42 | { | ||
| 43 | return static_cast<const Derived *>(this)->mass(); | ||
| 44 | } | ||
| 45 | const Vector3 & lever() const | ||
| 46 | { | ||
| 47 | return static_cast<const Derived *>(this)->lever(); | ||
| 48 | } | ||
| 49 | Vector3 & lever() | ||
| 50 | { | ||
| 51 | return static_cast<const Derived *>(this)->lever(); | ||
| 52 | } | ||
| 53 | const Symmetric3 & inertia() const | ||
| 54 | { | ||
| 55 | return static_cast<const Derived *>(this)->inertia(); | ||
| 56 | } | ||
| 57 | Symmetric3 & inertia() | ||
| 58 | { | ||
| 59 | return static_cast<const Derived *>(this)->inertia(); | ||
| 60 | } | ||
| 61 | |||
| 62 | template<typename Matrix6Like> | ||
| 63 | void matrix(const Eigen::MatrixBase<Matrix6Like> & mat) const | ||
| 64 | { | ||
| 65 | derived().matrix_impl(mat); | ||
| 66 | } | ||
| 67 | 11196 | Matrix6 matrix() const | |
| 68 | { | ||
| 69 | 11196 | return derived().matrix_impl(); | |
| 70 | } | ||
| 71 | 2322 | operator Matrix6() const | |
| 72 | { | ||
| 73 | 2322 | return matrix(); | |
| 74 | } | ||
| 75 | |||
| 76 | template<typename Matrix6Like> | ||
| 77 | void inverse(const Eigen::MatrixBase<Matrix6Like> & mat) const | ||
| 78 | { | ||
| 79 | derived().inverse_impl(mat); | ||
| 80 | } | ||
| 81 | 1 | Matrix6 inverse() const | |
| 82 | { | ||
| 83 | 1 | return derived().inverse_impl(); | |
| 84 | } | ||
| 85 | |||
| 86 | Derived & operator=(const Derived & clone) | ||
| 87 | { | ||
| 88 | return derived().__equl__(clone); | ||
| 89 | } | ||
| 90 | 11773 | bool operator==(const Derived & other) const | |
| 91 | { | ||
| 92 | 11773 | return derived().isEqual(other); | |
| 93 | } | ||
| 94 | 647 | bool operator!=(const Derived & other) const | |
| 95 | { | ||
| 96 | 647 | return !(*this == other); | |
| 97 | } | ||
| 98 | |||
| 99 | 112578 | Derived & operator+=(const Derived & Yb) | |
| 100 | { | ||
| 101 | 112578 | return derived().__pequ__(Yb); | |
| 102 | } | ||
| 103 | 100007 | Derived operator+(const Derived & Yb) const | |
| 104 | { | ||
| 105 | 100007 | return derived().__plus__(Yb); | |
| 106 | } | ||
| 107 | 3 | Derived & operator-=(const Derived & Yb) | |
| 108 | { | ||
| 109 | 3 | return derived().__mequ__(Yb); | |
| 110 | } | ||
| 111 | 1 | Derived operator-(const Derived & Yb) const | |
| 112 | { | ||
| 113 | 1 | return derived().__minus__(Yb); | |
| 114 | } | ||
| 115 | |||
| 116 | template<typename MotionDerived> | ||
| 117 | ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options> | ||
| 118 | 173370 | operator*(const MotionDense<MotionDerived> & v) const | |
| 119 | { | ||
| 120 | 173370 | return derived().__mult__(v); | |
| 121 | } | ||
| 122 | |||
| 123 | template<typename MotionDerived> | ||
| 124 | 7523 | Scalar vtiv(const MotionDense<MotionDerived> & v) const | |
| 125 | { | ||
| 126 | 7523 | return derived().vtiv_impl(v); | |
| 127 | } | ||
| 128 | |||
| 129 | template<typename MotionDerived> | ||
| 130 | Matrix6 variation(const MotionDense<MotionDerived> & v) const | ||
| 131 | { | ||
| 132 | return derived().variation_impl(v); | ||
| 133 | } | ||
| 134 | |||
| 135 | /// \brief Time variation operator. | ||
| 136 | /// It computes the time derivative of an inertia I corresponding to the formula \f$ | ||
| 137 | /// \dot{I} = v \times^{*} I \f$. | ||
| 138 | /// | ||
| 139 | /// \param[in] v The spatial velocity of the frame supporting the inertia. | ||
| 140 | /// \param[in] I The spatial inertia in motion. | ||
| 141 | /// \param[out] Iout The time derivative of the inertia I. | ||
| 142 | /// | ||
| 143 | template<typename MotionDerived, typename M6> | ||
| 144 | static void | ||
| 145 | 131 | vxi(const MotionDense<MotionDerived> & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout) | |
| 146 | { | ||
| 147 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6); | ||
| 148 | 131 | Derived::vxi_impl(v, I, Iout); | |
| 149 | 131 | } | |
| 150 | |||
| 151 | template<typename MotionDerived> | ||
| 152 | 129 | Matrix6 vxi(const MotionDense<MotionDerived> & v) const | |
| 153 | { | ||
| 154 | 129 | Matrix6 Iout; | |
| 155 |
0/2✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
129 | vxi(v, derived(), Iout); |
| 156 | 129 | return Iout; | |
| 157 | } | ||
| 158 | |||
| 159 | /// \brief Time variation operator. | ||
| 160 | /// It computes the time derivative of an inertia I corresponding to the formula \f$ | ||
| 161 | /// \dot{I} = v \times^{*} I \f$. | ||
| 162 | /// | ||
| 163 | /// \param[in] v The spatial velocity of the frame supporting the inertia. | ||
| 164 | /// \param[in] I The spatial inertia in motion. | ||
| 165 | /// \param[out] Iout The time derivative of the inertia I. | ||
| 166 | /// | ||
| 167 | template<typename MotionDerived, typename M6> | ||
| 168 | static void | ||
| 169 | 3 | ivx(const MotionDense<MotionDerived> & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout) | |
| 170 | { | ||
| 171 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6); | ||
| 172 | 3 | Derived::ivx_impl(v, I, Iout); | |
| 173 | 3 | } | |
| 174 | |||
| 175 | template<typename MotionDerived> | ||
| 176 | 1 | Matrix6 ivx(const MotionDense<MotionDerived> & v) const | |
| 177 | { | ||
| 178 | 1 | Matrix6 Iout; | |
| 179 |
0/2✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
1 | ivx(v, derived(), Iout); |
| 180 | 1 | return Iout; | |
| 181 | } | ||
| 182 | |||
| 183 | void setZero() | ||
| 184 | { | ||
| 185 | derived().setZero(); | ||
| 186 | } | ||
| 187 | void setIdentity() | ||
| 188 | { | ||
| 189 | derived().setIdentity(); | ||
| 190 | } | ||
| 191 | void setRandom() | ||
| 192 | { | ||
| 193 | derived().setRandom(); | ||
| 194 | } | ||
| 195 | |||
| 196 | 1481 | bool isApprox( | |
| 197 | const Derived & other, | ||
| 198 | const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | ||
| 199 | { | ||
| 200 | 1481 | return derived().isApprox_impl(other, prec); | |
| 201 | } | ||
| 202 | |||
| 203 | 2334 | bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
| 204 | { | ||
| 205 | 2334 | return derived().isZero_impl(prec); | |
| 206 | } | ||
| 207 | |||
| 208 | /// aI = aXb.act(bI) | ||
| 209 | template<typename S2, int O2> | ||
| 210 | 113254 | Derived se3Action(const SE3Tpl<S2, O2> & M) const | |
| 211 | { | ||
| 212 | 113254 | return derived().se3Action_impl(M); | |
| 213 | } | ||
| 214 | |||
| 215 | /// bI = aXb.actInv(aI) | ||
| 216 | template<typename S2, int O2> | ||
| 217 | 9 | Derived se3ActionInverse(const SE3Tpl<S2, O2> & M) const | |
| 218 | { | ||
| 219 | 9 | return derived().se3ActionInverse_impl(M); | |
| 220 | } | ||
| 221 | |||
| 222 | 100009 | void disp(std::ostream & os) const | |
| 223 | { | ||
| 224 | 100009 | static_cast<const Derived *>(this)->disp_impl(os); | |
| 225 | 100009 | } | |
| 226 | 100009 | friend std::ostream & operator<<(std::ostream & os, const InertiaBase<Derived> & X) | |
| 227 | { | ||
| 228 | 100009 | X.disp(os); | |
| 229 | 100009 | return os; | |
| 230 | } | ||
| 231 | }; | ||
| 232 | |||
| 233 | // class InertiaBase | ||
| 234 | template<typename T, int U> | ||
| 235 | struct traits<InertiaTpl<T, U>> | ||
| 236 | { | ||
| 237 | typedef T Scalar; | ||
| 238 | typedef Eigen::Matrix<T, 3, 1, U> Vector3; | ||
| 239 | typedef Eigen::Matrix<T, 4, 1, U> Vector4; | ||
| 240 | typedef Eigen::Matrix<T, 6, 1, U> Vector6; | ||
| 241 | typedef Eigen::Matrix<T, 3, 3, U> Matrix3; | ||
| 242 | typedef Eigen::Matrix<T, 4, 4, U> Matrix4; | ||
| 243 | typedef Eigen::Matrix<T, 6, 6, U> Matrix6; | ||
| 244 | typedef Eigen::Matrix<T, 10, 10, U> Matrix10; | ||
| 245 | typedef Matrix6 ActionMatrix_t; | ||
| 246 | typedef Vector3 Angular_t; | ||
| 247 | typedef Vector3 Linear_t; | ||
| 248 | typedef const Vector3 ConstAngular_t; | ||
| 249 | typedef const Vector3 ConstLinear_t; | ||
| 250 | typedef Eigen::Quaternion<T, U> Quaternion_t; | ||
| 251 | typedef SE3Tpl<T, U> SE3; | ||
| 252 | typedef ForceTpl<T, U> Force; | ||
| 253 | typedef MotionTpl<T, U> Motion; | ||
| 254 | typedef Symmetric3Tpl<T, U> Symmetric3; | ||
| 255 | typedef PseudoInertiaTpl<T, U> PseudoInertia; | ||
| 256 | typedef LogCholeskyParametersTpl<T, U> LogCholeskyParameters; | ||
| 257 | enum | ||
| 258 | { | ||
| 259 | LINEAR = 0, | ||
| 260 | ANGULAR = 3 | ||
| 261 | }; | ||
| 262 | }; // traits InertiaTpl | ||
| 263 | |||
| 264 | template<typename _Scalar, int _Options> | ||
| 265 | struct InertiaTpl : public InertiaBase<InertiaTpl<_Scalar, _Options>> | ||
| 266 | { | ||
| 267 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 268 | |||
| 269 | SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl); | ||
| 270 | enum | ||
| 271 | { | ||
| 272 | Options = _Options | ||
| 273 | }; | ||
| 274 | |||
| 275 | typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare; | ||
| 276 | typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10; | ||
| 277 | typedef Eigen::Matrix<Scalar, 10, 10, Options> Matrix10; | ||
| 278 | typedef PseudoInertiaTpl<Scalar, Options> PseudoInertia; | ||
| 279 | typedef LogCholeskyParametersTpl<Scalar, Options> LogCholeskyParameters; | ||
| 280 | |||
| 281 | // Constructors | ||
| 282 | 1922 | InertiaTpl() | |
| 283 |
2/4✓ Branch 2 taken 438 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 438 times.
✗ Branch 6 not taken.
|
1922 | { |
| 284 | 1922 | } | |
| 285 | |||
| 286 | 3054 | InertiaTpl(const Scalar & mass, const Vector3 & com, const Matrix3 & rotational_inertia) | |
| 287 | 3054 | : m_mass(mass) | |
| 288 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
3054 | , m_com(com) |
| 289 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
3054 | , m_inertia(rotational_inertia) |
| 290 | { | ||
| 291 | 3054 | } | |
| 292 | |||
| 293 | 1 | explicit InertiaTpl(const Matrix6 & I6) | |
| 294 | 1 | { | |
| 295 |
5/10✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
|
1 | assert(check_expression_if_real<Scalar>(pinocchio::isZero(I6 - I6.transpose()))); |
| 296 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | mass() = I6(LINEAR, LINEAR); |
| 297 | const typename Matrix6::template ConstFixedBlockXpr<3, 3>::Type mc_cross = | ||
| 298 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | I6.template block<3, 3>(ANGULAR, LINEAR); |
| 299 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | lever() = unSkew(mc_cross); |
| 300 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | lever() /= mass(); |
| 301 | |||
| 302 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Matrix3 I3(mc_cross * mc_cross); |
| 303 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | I3 /= mass(); |
| 304 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | I3 += I6.template block<3, 3>(ANGULAR, ANGULAR); |
| 305 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | const Symmetric3 S3(I3); |
| 306 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | inertia() = S3; |
| 307 | 1 | } | |
| 308 | |||
| 309 | 503096 | InertiaTpl(const Scalar & mass, const Vector3 & com, const Symmetric3 & rotational_inertia) | |
| 310 | 503096 | : m_mass(mass) | |
| 311 |
1/2✓ Branch 1 taken 2489 times.
✗ Branch 2 not taken.
|
503096 | , m_com(com) |
| 312 |
1/2✓ Branch 1 taken 2489 times.
✗ Branch 2 not taken.
|
503096 | , m_inertia(rotational_inertia) |
| 313 | { | ||
| 314 | 503096 | } | |
| 315 | |||
| 316 | 822273 | InertiaTpl(const InertiaTpl & clone) // Copy constructor | |
| 317 | 822273 | : m_mass(clone.mass()) | |
| 318 |
1/2✓ Branch 2 taken 3107 times.
✗ Branch 3 not taken.
|
822273 | , m_com(clone.lever()) |
| 319 |
1/2✓ Branch 2 taken 3107 times.
✗ Branch 3 not taken.
|
822273 | , m_inertia(clone.inertia()) |
| 320 | { | ||
| 321 | 822273 | } | |
| 322 | |||
| 323 | 126021 | InertiaTpl & operator=(const InertiaTpl & clone) // Copy assignment operator | |
| 324 | { | ||
| 325 |
1/2✓ Branch 2 taken 1533 times.
✗ Branch 3 not taken.
|
126021 | m_mass = clone.mass(); |
| 326 | 126021 | m_com = clone.lever(); | |
| 327 | 126021 | m_inertia = clone.inertia(); | |
| 328 | 126021 | return *this; | |
| 329 | } | ||
| 330 | |||
| 331 | template<typename S2, int O2> | ||
| 332 | 1 | explicit InertiaTpl(const InertiaTpl<S2, O2> & clone) | |
| 333 | 1 | { | |
| 334 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | *this = clone.template cast<Scalar>(); |
| 335 | 1 | } | |
| 336 | |||
| 337 | // Initializers | ||
| 338 | 66712 | static InertiaTpl Zero() | |
| 339 | { | ||
| 340 |
4/8✓ Branch 2 taken 65449 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65449 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65449 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 991 times.
✗ Branch 12 not taken.
|
133424 | return InertiaTpl(Scalar(0), Vector3::Zero(), Symmetric3::Zero()); |
| 341 | } | ||
| 342 | |||
| 343 | 2435 | void setZero() | |
| 344 | { | ||
| 345 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2435 | mass() = Scalar(0); |
| 346 | 2435 | lever().setZero(); | |
| 347 | 2435 | inertia().setZero(); | |
| 348 | 2435 | } | |
| 349 | |||
| 350 | 292 | static InertiaTpl Identity() | |
| 351 | { | ||
| 352 |
3/8✓ Branch 2 taken 292 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 292 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 292 times.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
|
584 | return InertiaTpl(Scalar(1), Vector3::Zero(), Symmetric3::Identity()); |
| 353 | } | ||
| 354 | |||
| 355 | 1 | void setIdentity() | |
| 356 | { | ||
| 357 |
0/2✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
1 | mass() = Scalar(1); |
| 358 | 1 | lever().setZero(); | |
| 359 | 1 | inertia().setIdentity(); | |
| 360 | 1 | } | |
| 361 | |||
| 362 | 212886 | static InertiaTpl Random() | |
| 363 | { | ||
| 364 | // We have to shoot "I" definite positive and not only symmetric. | ||
| 365 | return InertiaTpl( | ||
| 366 |
4/12✓ Branch 2 taken 212886 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 212886 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 212886 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 212886 times.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
|
425772 | Eigen::internal::random<Scalar>() + 1, Vector3::Random(), Symmetric3::RandomPositive()); |
| 367 | } | ||
| 368 | |||
| 369 | /// | ||
| 370 | /// \brief Computes the Inertia of a sphere defined by its mass and its radius. | ||
| 371 | /// | ||
| 372 | /// \param[in] mass of the sphere. | ||
| 373 | /// \param[in] radius of the sphere. | ||
| 374 | /// | ||
| 375 | 7 | static InertiaTpl FromSphere(const Scalar mass, const Scalar radius) | |
| 376 | { | ||
| 377 |
0/8✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
|
7 | return FromEllipsoid(mass, radius, radius, radius); |
| 378 | } | ||
| 379 | |||
| 380 | /// | ||
| 381 | /// \brief Computes the Inertia of an ellipsoid defined by its mass and main semi-axis | ||
| 382 | /// dimensions (x,y,z). | ||
| 383 | /// | ||
| 384 | /// \param[in] mass of the ellipsoid. | ||
| 385 | /// \param[in] x semi-axis dimension along the local X axis. | ||
| 386 | /// \param[in] y semi-axis dimension along the local Y axis. | ||
| 387 | /// \param[in] z semi-axis dimension along the local Z axis. | ||
| 388 | /// | ||
| 389 | static InertiaTpl | ||
| 390 | 14 | FromEllipsoid(const Scalar mass, const Scalar x, const Scalar y, const Scalar z) | |
| 391 | { | ||
| 392 |
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.
|
14 | const Scalar a = mass * (y * y + z * z) / Scalar(5); |
| 393 |
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.
|
14 | const Scalar b = mass * (x * x + z * z) / Scalar(5); |
| 394 |
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.
|
14 | const Scalar c = mass * (y * y + x * x) / Scalar(5); |
| 395 | return InertiaTpl( | ||
| 396 |
4/14✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
14 | mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, Scalar(0), Scalar(0), c)); |
| 397 | } | ||
| 398 | |||
| 399 | /// | ||
| 400 | /// \brief Computes the Inertia of a cylinder defined by its mass, radius and length along the Z | ||
| 401 | /// axis. | ||
| 402 | /// | ||
| 403 | /// \param[in] mass of the cylinder. | ||
| 404 | /// \param[in] radius of the cylinder. | ||
| 405 | /// \param[in] length of the cylinder. | ||
| 406 | /// | ||
| 407 | 10 | static InertiaTpl FromCylinder(const Scalar mass, const Scalar radius, const Scalar length) | |
| 408 | { | ||
| 409 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
10 | const Scalar radius_square = radius * radius; |
| 410 |
0/14✗ 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.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
10 | const Scalar a = mass * (radius_square / Scalar(4) + length * length / Scalar(12)); |
| 411 |
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.
|
10 | const Scalar c = mass * (radius_square / Scalar(2)); |
| 412 | return InertiaTpl( | ||
| 413 |
4/14✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
10 | mass, Vector3::Zero(), Symmetric3(a, Scalar(0), a, Scalar(0), Scalar(0), c)); |
| 414 | } | ||
| 415 | |||
| 416 | /// | ||
| 417 | /// \brief Computes the Inertia of a box defined by its mass and main dimensions (x,y,z). | ||
| 418 | /// | ||
| 419 | /// \param[in] mass of the box. | ||
| 420 | /// \param[in] x dimension along the local X axis. | ||
| 421 | /// \param[in] y dimension along the local Y axis. | ||
| 422 | /// \param[in] z dimension along the local Z axis. | ||
| 423 | /// | ||
| 424 | 14 | static InertiaTpl FromBox(const Scalar mass, const Scalar x, const Scalar y, const Scalar z) | |
| 425 | { | ||
| 426 |
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.
|
14 | const Scalar a = mass * (y * y + z * z) / Scalar(12); |
| 427 |
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.
|
14 | const Scalar b = mass * (x * x + z * z) / Scalar(12); |
| 428 |
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.
|
14 | const Scalar c = mass * (y * y + x * x) / Scalar(12); |
| 429 | return InertiaTpl( | ||
| 430 |
4/14✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
14 | mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, Scalar(0), Scalar(0), c)); |
| 431 | } | ||
| 432 | |||
| 433 | /// | ||
| 434 | /// \brief Computes the Inertia of a capsule defined by its mass, radius and length along the Z | ||
| 435 | /// axis. Assumes a uniform density. | ||
| 436 | /// | ||
| 437 | /// \param[in] mass of the capsule. | ||
| 438 | /// \param[in] radius of the capsule. | ||
| 439 | /// \param[in] height of the capsule. | ||
| 440 | 26 | static InertiaTpl FromCapsule(const Scalar mass, const Scalar radius, const Scalar height) | |
| 441 | { | ||
| 442 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
26 | const Scalar pi = boost::math::constants::pi<Scalar>(); |
| 443 | |||
| 444 | // first need to compute mass repartition between cylinder and halfsphere | ||
| 445 |
1/6✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
26 | const Scalar v_cyl = pi * math::pow(radius, 2) * height; |
| 446 |
1/12✓ Branch 1 taken 26 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.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
26 | const Scalar v_hs = Scalar(2) / Scalar(3) * math::pow(radius, 3) * pi; |
| 447 |
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.
|
26 | const Scalar total_v = v_cyl + Scalar(2) * v_hs; |
| 448 | |||
| 449 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
26 | const Scalar m_cyl = mass * (v_cyl / total_v); |
| 450 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
26 | const Scalar m_hs = mass * (v_hs / total_v); |
| 451 | |||
| 452 | // Then Distance between halfSphere center and cylindere center. | ||
| 453 |
0/10✗ 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.
|
26 | const Scalar dist_hc_cyl = height / Scalar(2) + Scalar(0.375) * radius; |
| 454 | |||
| 455 | // Computes inertia terms | ||
| 456 |
0/8✗ 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.
|
26 | const Scalar ix_c = |
| 457 |
2/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
26 | m_cyl * (math::pow(height, 2) / Scalar(12) + math::pow(radius, 2) / Scalar(4)); |
| 458 |
1/8✓ Branch 1 taken 26 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.
|
26 | const Scalar iz_c = m_cyl * math::pow(radius, 2) / Scalar(2); |
| 459 | |||
| 460 | // For halfsphere inertia see, "Dynamics: Theory and Applications" McGraw-Hill, New York, | ||
| 461 | // 1985, by T.R. Kane and D.A. Levinson, Appendix 23. | ||
| 462 |
1/8✓ Branch 1 taken 26 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.
|
26 | const Scalar ix_hs = m_hs * math::pow(radius, 2) * Scalar(0.259375); |
| 463 |
1/8✓ Branch 1 taken 26 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.
|
26 | const Scalar iz_hs = m_hs * math::pow(radius, 2) * Scalar(0.4); |
| 464 | |||
| 465 | // Put everything together using the parallel axis theorem | ||
| 466 |
1/12✓ Branch 1 taken 26 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.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
26 | const Scalar ix = ix_c + Scalar(2) * (ix_hs + m_hs * math::pow(dist_hc_cyl, 2)); |
| 467 |
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.
|
26 | const Scalar iz = iz_c + Scalar(2) * iz_hs; |
| 468 | |||
| 469 | return InertiaTpl( | ||
| 470 |
4/14✓ 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 taken 26 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
26 | mass, Vector3::Zero(), Symmetric3(ix, Scalar(0), ix, Scalar(0), Scalar(0), iz)); |
| 471 | } | ||
| 472 | |||
| 473 | 3 | void setRandom() | |
| 474 | { | ||
| 475 |
0/6✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
3 | mass() = static_cast<Scalar>(std::rand()) / static_cast<Scalar>(RAND_MAX); |
| 476 | 3 | lever().setRandom(); | |
| 477 | 3 | inertia().setRandom(); | |
| 478 | 3 | } | |
| 479 | |||
| 480 | template<typename Matrix6Like> | ||
| 481 | 11196 | void matrix_impl(const Eigen::MatrixBase<Matrix6Like> & M_) const | |
| 482 | { | ||
| 483 | 11196 | Matrix6Like & M = M_.const_cast_derived(); | |
| 484 | |||
| 485 |
1/2✓ Branch 2 taken 11079 times.
✗ Branch 3 not taken.
|
11196 | M.template block<3, 3>(LINEAR, LINEAR).setZero(); |
| 486 |
4/7✓ Branch 2 taken 11079 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 208 times.
✓ Branch 6 taken 10871 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 208 times.
✗ Branch 9 not taken.
|
11196 | M.template block<3, 3>(LINEAR, LINEAR).diagonal().fill(mass()); |
| 487 |
5/8✓ Branch 3 taken 208 times.
✓ Branch 4 taken 10871 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 208 times.
✓ Branch 7 taken 10871 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 208 times.
✗ Branch 10 not taken.
|
11196 | M.template block<3, 3>(ANGULAR, LINEAR) = alphaSkew(mass(), lever()); |
| 488 |
3/6✓ Branch 2 taken 11079 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11079 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11079 times.
✗ Branch 9 not taken.
|
11196 | M.template block<3, 3>(LINEAR, ANGULAR) = -M.template block<3, 3>(ANGULAR, LINEAR); |
| 489 |
4/8✓ Branch 1 taken 11079 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11079 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11079 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11079 times.
✗ Branch 11 not taken.
|
11196 | M.template block<3, 3>(ANGULAR, ANGULAR) = |
| 490 | 11466 | (inertia() - AlphaSkewSquare(mass(), lever())).matrix(); | |
| 491 | 11196 | } | |
| 492 | |||
| 493 | 11196 | Matrix6 matrix_impl() const | |
| 494 | { | ||
| 495 | 11196 | Matrix6 M; | |
| 496 |
1/2✓ Branch 1 taken 208 times.
✗ Branch 2 not taken.
|
11196 | matrix_impl(M); |
| 497 | 11196 | return M; | |
| 498 | } | ||
| 499 | |||
| 500 | template<typename Matrix6Like> | ||
| 501 | 1 | void inverse_impl(const Eigen::MatrixBase<Matrix6Like> & M_) const | |
| 502 | { | ||
| 503 | 1 | Matrix6Like & M = M_.const_cast_derived(); | |
| 504 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | inertia().inverse(M.template block<3, 3>(ANGULAR, ANGULAR)); |
| 505 | |||
| 506 |
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.
|
1 | M.template block<3, 3>(LINEAR, ANGULAR).noalias() = |
| 507 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | -M.template block<3, 3>(ANGULAR, ANGULAR).colwise().cross(lever()); |
| 508 |
4/8✓ 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.
|
1 | M.template block<3, 3>(ANGULAR, LINEAR) = M.template block<3, 3>(LINEAR, ANGULAR).transpose(); |
| 509 | |||
| 510 |
3/11✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
1 | const Scalar &cx = lever()[0], cy = lever()[1], cz = lever()[2]; |
| 511 | |||
| 512 |
7/14✓ 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.
|
2 | M.template block<3, 3>(LINEAR, LINEAR).col(0).noalias() = |
| 513 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | cy * M.template block<3, 3>(LINEAR, ANGULAR).col(2) |
| 514 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | - cz * M.template block<3, 3>(LINEAR, ANGULAR).col(1); |
| 515 | |||
| 516 |
7/14✓ 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.
|
2 | M.template block<3, 3>(LINEAR, LINEAR).col(1).noalias() = |
| 517 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | cz * M.template block<3, 3>(LINEAR, ANGULAR).col(0) |
| 518 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | - cx * M.template block<3, 3>(LINEAR, ANGULAR).col(2); |
| 519 | |||
| 520 |
7/14✓ 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.
|
2 | M.template block<3, 3>(LINEAR, LINEAR).col(2).noalias() = |
| 521 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | cx * M.template block<3, 3>(LINEAR, ANGULAR).col(1) |
| 522 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | - cy * M.template block<3, 3>(LINEAR, ANGULAR).col(0); |
| 523 | |||
| 524 |
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.
|
1 | const Scalar m_inv = Scalar(1) / mass(); |
| 525 |
4/8✓ 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.
|
1 | M.template block<3, 3>(LINEAR, LINEAR).diagonal().array() += m_inv; |
| 526 | 1 | } | |
| 527 | |||
| 528 | 1 | Matrix6 inverse_impl() const | |
| 529 | { | ||
| 530 | 1 | Matrix6 res; | |
| 531 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
1 | inverse_impl(res); |
| 532 | 1 | return res; | |
| 533 | } | ||
| 534 | |||
| 535 | /** Returns the representation of the matrix as a vector of dynamic parameters. | ||
| 536 | * The parameters are given as | ||
| 537 | * \f$ v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T \f$ | ||
| 538 | * where \f$ c \f$ is the center of mass, \f$ I = I_C + mS^T(c)S(c) \f$ and \f$ I_C \f$ has its | ||
| 539 | * origin at the barycenter and \f$ S(c) \f$ is the the skew matrix representation of the cross | ||
| 540 | * product operator. | ||
| 541 | */ | ||
| 542 | 122 | Vector10 toDynamicParameters() const | |
| 543 | { | ||
| 544 | 122 | Vector10 v; | |
| 545 | |||
| 546 |
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.
|
122 | v[0] = mass(); |
| 547 |
4/14✗ Branch 2 not taken.
✓ Branch 3 taken 122 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 122 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 122 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 122 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
122 | v.template segment<3>(1).noalias() = mass() * lever(); |
| 548 |
3/13✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 5 taken 122 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 122 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 122 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
122 | v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(), lever())).data(); |
| 549 | |||
| 550 | 122 | return v; | |
| 551 | } | ||
| 552 | |||
| 553 | /** Builds and inertia matrix from a vector of dynamic parameters. | ||
| 554 | * | ||
| 555 | * @param[in] params The dynamic parameters. | ||
| 556 | * | ||
| 557 | * The parameters are given as | ||
| 558 | * \f$ v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T \f$ | ||
| 559 | * where \f$ I = I_C + mS^T(c)S(c) \f$ and \f$ I_C \f$ has its origin at the barycenter. | ||
| 560 | */ | ||
| 561 | template<typename Vector10Like> | ||
| 562 | 11 | static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & params) | |
| 563 | { | ||
| 564 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
11 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1); |
| 565 | |||
| 566 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
11 | const Scalar & mass = params[0]; |
| 567 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
11 | Vector3 lever = params.template segment<3>(1); |
| 568 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
11 | lever /= mass; |
| 569 | |||
| 570 | return InertiaTpl( | ||
| 571 |
6/12✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
|
11 | mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass, lever)); |
| 572 | } | ||
| 573 | |||
| 574 | /** | ||
| 575 | |||
| 576 | * @brief Create an InertiaTpl object from a PseudoInertia object. | ||
| 577 | * | ||
| 578 | * @param pseudo_inertia A PseudoInertia object. | ||
| 579 | * @return An InertiaTpl object. | ||
| 580 | */ | ||
| 581 | |||
| 582 | ✗ | static InertiaTpl FromPseudoInertia(const PseudoInertia & pseudo_inertia) | |
| 583 | { | ||
| 584 | ✗ | return pseudo_inertia.toInertia(); | |
| 585 | } | ||
| 586 | |||
| 587 | /** | ||
| 588 | * @brief Convert the InertiaTpl object to a 4x4 pseudo inertia matrix. | ||
| 589 | * | ||
| 590 | * @return A 4x4 pseudo inertia matrix. | ||
| 591 | */ | ||
| 592 | 3 | PseudoInertia toPseudoInertia() const | |
| 593 | { | ||
| 594 | 3 | PseudoInertia pseudo_inertia = PseudoInertia::FromInertia(*this); | |
| 595 | 3 | return pseudo_inertia; | |
| 596 | } | ||
| 597 | |||
| 598 | /** | ||
| 599 | * @brief Create an InertiaTpl object from log Cholesky parameters. | ||
| 600 | * | ||
| 601 | * @param log_cholesky A log cholesky parameters object | ||
| 602 | * | ||
| 603 | * @return An InertiaTpl object. | ||
| 604 | */ | ||
| 605 | 2 | static InertiaTpl FromLogCholeskyParameters(const LogCholeskyParameters & log_cholesky) | |
| 606 | { | ||
| 607 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Vector10 dynamic_params = log_cholesky.toDynamicParameters(); |
| 608 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return FromDynamicParameters(dynamic_params); |
| 609 | } | ||
| 610 | |||
| 611 | // Arithmetic operators | ||
| 612 | InertiaTpl & __equl__(const InertiaTpl & clone) | ||
| 613 | { | ||
| 614 | mass() = clone.mass(); | ||
| 615 | lever() = clone.lever(); | ||
| 616 | inertia() = clone.inertia(); | ||
| 617 | return *this; | ||
| 618 | } | ||
| 619 | |||
| 620 | // Required by std::vector boost::python bindings. | ||
| 621 | 11774 | bool isEqual(const InertiaTpl & Y2) const | |
| 622 | { | ||
| 623 |
4/6✓ Branch 2 taken 8117 times.
✓ Branch 3 taken 2 times.
✓ Branch 7 taken 8117 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 8117 times.
✗ Branch 13 not taken.
|
11774 | return (mass() == Y2.mass()) && (lever() == Y2.lever()) && (inertia() == Y2.inertia()); |
| 624 | } | ||
| 625 | |||
| 626 | 1481 | bool isApprox_impl( | |
| 627 | const InertiaTpl & other, | ||
| 628 | const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | ||
| 629 | { | ||
| 630 | using math::fabs; | ||
| 631 | 1481 | return fabs(static_cast<Scalar>(mass() - other.mass())) <= prec | |
| 632 |
5/10✓ Branch 0 taken 1477 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 1477 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1477 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 1477 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 1477 times.
✗ Branch 15 not taken.
|
1481 | && lever().isApprox(other.lever(), prec) && inertia().isApprox(other.inertia(), prec); |
| 633 | } | ||
| 634 | |||
| 635 | 2334 | bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
| 636 | { | ||
| 637 | using math::fabs; | ||
| 638 |
6/10✓ Branch 2 taken 392 times.
✓ Branch 3 taken 1942 times.
✓ Branch 6 taken 392 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 392 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 392 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 392 times.
✗ Branch 15 not taken.
|
2334 | return fabs(mass()) <= prec && lever().isZero(prec) && inertia().isZero(prec); |
| 639 | } | ||
| 640 | |||
| 641 | 100007 | InertiaTpl __plus__(const InertiaTpl & Yb) const | |
| 642 | { | ||
| 643 | /* Y_{a+b} = ( m_a+m_b, | ||
| 644 | * (m_a*c_a + m_b*c_b ) / (m_a + m_b), | ||
| 645 | * I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x ) | ||
| 646 | */ | ||
| 647 | |||
| 648 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
100007 | const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); |
| 649 | |||
| 650 |
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.
|
100007 | const Scalar & mab = mass() + Yb.mass(); |
| 651 |
1/6✓ Branch 1 taken 100007 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
100007 | const Scalar mab_inv = Scalar(1) / math::max(mab, eps); |
| 652 |
2/4✓ Branch 3 taken 100007 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100007 times.
✗ Branch 7 not taken.
|
100007 | const Vector3 & AB = (lever() - Yb.lever()).eval(); |
| 653 | return InertiaTpl( | ||
| 654 | 200014 | mab, (mass() * lever() + Yb.mass() * Yb.lever()) * mab_inv, | |
| 655 | 100007 | inertia() + Yb.inertia() | |
| 656 |
9/39✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 100007 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 100007 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 100007 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 100007 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 100007 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 100007 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 100007 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 100007 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 100007 times.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
|
400028 | - (mass() * Yb.mass() * mab_inv) * typename Symmetric3::SkewSquare(AB)); |
| 657 | } | ||
| 658 | |||
| 659 | 112578 | InertiaTpl & __pequ__(const InertiaTpl & Yb) | |
| 660 | { | ||
| 661 |
4/8✓ Branch 0 taken 10 times.
✓ Branch 1 taken 116 times.
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
189 | static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); |
| 662 | |||
| 663 | 112578 | const InertiaTpl & Ya = *this; | |
| 664 |
2/4✓ Branch 1 taken 126 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 126 times.
✗ Branch 6 not taken.
|
112578 | const Scalar & mab = mass() + Yb.mass(); |
| 665 |
3/6✓ Branch 1 taken 112457 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 126 times.
✗ Branch 8 not taken.
|
112767 | const Scalar mab_inv = Scalar(1) / math::max(mab, eps); |
| 666 |
2/4✓ Branch 3 taken 112457 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 112457 times.
✗ Branch 7 not taken.
|
112578 | const Vector3 & AB = (Ya.lever() - Yb.lever()).eval(); |
| 667 |
3/5✓ Branch 2 taken 126 times.
✓ Branch 3 taken 112331 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 126 times.
✗ Branch 7 not taken.
|
112578 | lever() *= (mass() * mab_inv); |
| 668 |
6/10✓ Branch 2 taken 126 times.
✓ Branch 3 taken 112331 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 126 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 112331 times.
✓ Branch 8 taken 126 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 126 times.
✗ Branch 13 not taken.
|
112578 | lever() += (Yb.mass() * mab_inv) * Yb.lever(); // c *= mab_inv; |
| 669 |
1/2✓ Branch 3 taken 112457 times.
✗ Branch 4 not taken.
|
112578 | inertia() += Yb.inertia(); |
| 670 |
7/13✓ Branch 2 taken 126 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 112331 times.
✓ Branch 5 taken 126 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 112457 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 126 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 126 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 126 times.
✗ Branch 19 not taken.
|
112578 | inertia() -= (Ya.mass() * Yb.mass() * mab_inv) * typename Symmetric3::SkewSquare(AB); |
| 671 |
1/2✓ Branch 2 taken 126 times.
✗ Branch 3 not taken.
|
112578 | mass() = mab; |
| 672 | 112578 | return *this; | |
| 673 | 189 | } | |
| 674 | |||
| 675 | 1 | InertiaTpl __minus__(const InertiaTpl & Yb) const | |
| 676 | { | ||
| 677 | /* Y_{a} = ( m_{a+b}+m_b, | ||
| 678 | * (m_{a+b}*c_{a+b} - m_b*c_b ) / (m_a), | ||
| 679 | * I_{a+b} - I_b + (m_a*m_b)/(m_a+m_b) * AB_x * AB_x ) | ||
| 680 | */ | ||
| 681 | |||
| 682 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
1 | const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); |
| 683 | |||
| 684 |
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.
|
1 | const Scalar ma = mass() - Yb.mass(); |
| 685 |
2/7✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
1 | assert(check_expression_if_real<Scalar>(ma >= Scalar(0))); |
| 686 | |||
| 687 |
1/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
1 | const Scalar ma_inv = Scalar(1) / math::max(ma, eps); |
| 688 |
5/19✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
|
1 | const Vector3 c_a((mass() * lever() - Yb.mass() * Yb.lever()) * ma_inv); |
| 689 | |||
| 690 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | const Vector3 AB = c_a - Yb.lever(); |
| 691 | |||
| 692 | return InertiaTpl( | ||
| 693 | ma, c_a, | ||
| 694 |
5/23✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
|
1 | inertia() - Yb.inertia() + (ma * Yb.mass() / mass()) * typename Symmetric3::SkewSquare(AB)); |
| 695 | } | ||
| 696 | |||
| 697 | 3 | InertiaTpl & __mequ__(const InertiaTpl & Yb) | |
| 698 | { | ||
| 699 | ✗ | static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); | |
| 700 | |||
| 701 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
3 | const Scalar ma = mass() - Yb.mass(); |
| 702 |
2/7✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
3 | assert(check_expression_if_real<Scalar>(ma >= Scalar(0))); |
| 703 | |||
| 704 |
1/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
3 | const Scalar ma_inv = Scalar(1) / math::max(ma, eps); |
| 705 | |||
| 706 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
3 | lever() *= (mass() * ma_inv); |
| 707 |
3/14✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
|
3 | lever().noalias() -= (Yb.mass() * ma_inv) * Yb.lever(); |
| 708 | |||
| 709 |
2/4✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
|
3 | const Vector3 AB = lever() - Yb.lever(); |
| 710 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | inertia() -= Yb.inertia(); |
| 711 |
3/15✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
3 | inertia() += (ma * Yb.mass() / mass()) * typename Symmetric3::SkewSquare(AB); |
| 712 |
0/2✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
3 | mass() = ma; |
| 713 | |||
| 714 | 3 | return *this; | |
| 715 | } | ||
| 716 | |||
| 717 | template<typename MotionDerived> | ||
| 718 | ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options> | ||
| 719 | 173370 | __mult__(const MotionDense<MotionDerived> & v) const | |
| 720 | { | ||
| 721 | typedef ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options> | ||
| 722 | ReturnType; | ||
| 723 | 173370 | ReturnType f; | |
| 724 |
1/2✓ Branch 1 taken 492 times.
✗ Branch 2 not taken.
|
173370 | __mult__(v, f); |
| 725 | 173370 | return f; | |
| 726 | } | ||
| 727 | |||
| 728 | template<typename MotionDerived, typename ForceDerived> | ||
| 729 | 365428 | void __mult__(const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f) const | |
| 730 | { | ||
| 731 |
12/20✓ Branch 3 taken 261779 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 261779 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 261779 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 918 times.
✓ Branch 13 taken 260861 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 918 times.
✓ Branch 16 taken 260861 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 918 times.
✓ Branch 19 taken 260861 times.
✗ Branch 20 not taken.
✓ Branch 21 taken 918 times.
✓ Branch 22 taken 260861 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 918 times.
✗ Branch 25 not taken.
|
365428 | f.linear().noalias() = mass() * (v.linear() - lever().cross(v.angular())); |
| 732 |
2/4✓ Branch 2 taken 261779 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 261779 times.
✗ Branch 7 not taken.
|
365428 | Symmetric3::rhsMult(inertia(), v.angular(), f.angular()); |
| 733 |
3/6✓ Branch 3 taken 261779 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 261779 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 261779 times.
✗ Branch 10 not taken.
|
365428 | f.angular() += lever().cross(f.linear()); |
| 734 | // f.angular().noalias() = c.cross(f.linear()) + I*v.angular(); | ||
| 735 | 365428 | } | |
| 736 | |||
| 737 | template<typename MotionDerived> | ||
| 738 | 7523 | Scalar vtiv_impl(const MotionDense<MotionDerived> & v) const | |
| 739 | { | ||
| 740 |
2/4✓ Branch 2 taken 7523 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7523 times.
✗ Branch 6 not taken.
|
7523 | const Vector3 cxw(lever().cross(v.angular())); |
| 741 |
13/22✓ Branch 1 taken 1 times.
✓ Branch 2 taken 7522 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 7522 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
✓ Branch 8 taken 7522 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✓ Branch 11 taken 7522 times.
✗ Branch 12 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.
|
7524 | Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2) * v.linear().dot(cxw)); |
| 742 |
7/12✓ Branch 2 taken 7523 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 7522 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 7522 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
|
7524 | const Vector3 mcxcxw(-mass() * lever().cross(cxw)); |
| 743 |
3/6✓ Branch 1 taken 7523 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7523 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
7523 | res += v.angular().dot(mcxcxw); |
| 744 |
4/8✓ Branch 2 taken 7523 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7523 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7523 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
|
7523 | res += inertia().vtiv(v.angular()); |
| 745 | |||
| 746 | 7524 | return res; | |
| 747 | 1 | } | |
| 748 | |||
| 749 | template<typename MotionDerived> | ||
| 750 | 15291 | Matrix6 variation(const MotionDense<MotionDerived> & v) const | |
| 751 | { | ||
| 752 |
1/2✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
|
15291 | Matrix6 res; |
| 753 |
3/5✓ Branch 1 taken 27 times.
✓ Branch 2 taken 14900 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
15291 | const Motion mv(v * mass()); |
| 754 | |||
| 755 |
8/16✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14927 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14927 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 14927 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 14927 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 14927 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 14927 times.
✗ Branch 23 not taken.
|
45873 | res.template block<3, 3>(LINEAR, ANGULAR) = |
| 756 |
3/6✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 14927 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 14927 times.
✗ Branch 10 not taken.
|
45900 | -skew(mv.linear()) - skewSquare(mv.angular(), lever()) + skewSquare(lever(), mv.angular()); |
| 757 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
|
15291 | res.template block<3, 3>(ANGULAR, LINEAR) = |
| 758 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
|
15291 | res.template block<3, 3>(LINEAR, ANGULAR).transpose(); |
| 759 | |||
| 760 | // res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as | ||
| 761 | // temporary variable res.template block<3,3>(ANGULAR,ANGULAR) = res.template | ||
| 762 | // block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose(); | ||
| 763 |
6/12✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14927 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14927 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 14927 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 14927 times.
✗ Branch 17 not taken.
|
30582 | res.template block<3, 3>(ANGULAR, ANGULAR) = |
| 764 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 14927 times.
✗ Branch 7 not taken.
|
30609 | -skewSquare(mv.linear(), lever()) - skewSquare(lever(), mv.linear()); |
| 765 | |||
| 766 |
4/8✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14927 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14927 times.
✗ Branch 11 not taken.
|
15291 | res.template block<3, 3>(LINEAR, LINEAR) = |
| 767 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
15318 | (inertia() - AlphaSkewSquare(mass(), lever())).matrix(); |
| 768 | |||
| 769 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
|
15291 | res.template block<3, 3>(ANGULAR, ANGULAR) -= |
| 770 |
4/8✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14927 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14927 times.
✗ Branch 11 not taken.
|
15318 | res.template block<3, 3>(LINEAR, LINEAR) * skew(v.angular()); |
| 771 |
3/6✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14927 times.
✗ Branch 8 not taken.
|
15291 | res.template block<3, 3>(ANGULAR, ANGULAR) += |
| 772 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
|
15291 | cross(v.angular(), res.template block<3, 3>(LINEAR, LINEAR)); |
| 773 | |||
| 774 |
2/4✓ Branch 1 taken 14927 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14927 times.
✗ Branch 5 not taken.
|
15291 | res.template block<3, 3>(LINEAR, LINEAR).setZero(); |
| 775 | 30582 | return res; | |
| 776 | 27 | } | |
| 777 | |||
| 778 | template<typename MotionDerived, typename M6> | ||
| 779 | 131 | static void vxi_impl( | |
| 780 | const MotionDense<MotionDerived> & v, | ||
| 781 | const InertiaTpl & I, | ||
| 782 | const Eigen::MatrixBase<M6> & Iout) | ||
| 783 | { | ||
| 784 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6); | ||
| 785 | 131 | M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout); | |
| 786 | |||
| 787 | // Block 1,1 | ||
| 788 |
3/9✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 131 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
131 | alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR)); |
| 789 | // Iout_.template block<3,3>(LINEAR,LINEAR) = alphaSkew(I.mass(),v.angular()); | ||
| 790 |
2/8✗ Branch 2 not taken.
✓ Branch 3 taken 131 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 131 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
131 | const Vector3 mc(I.mass() * I.lever()); |
| 791 | |||
| 792 | // Block 1,2 | ||
| 793 |
4/8✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 131 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 131 times.
✗ Branch 11 not taken.
|
131 | skewSquare(-v.angular(), mc, Iout_.template block<3, 3>(LINEAR, ANGULAR)); |
| 794 | |||
| 795 | //// Block 2,1 | ||
| 796 |
3/9✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 131 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
131 | alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(ANGULAR, LINEAR)); |
| 797 |
3/6✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 131 times.
✗ Branch 8 not taken.
|
131 | Iout_.template block<3, 3>(ANGULAR, LINEAR) -= Iout_.template block<3, 3>(LINEAR, ANGULAR); |
| 798 | |||
| 799 | //// Block 2,2 | ||
| 800 |
4/8✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 131 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 131 times.
✗ Branch 11 not taken.
|
131 | skewSquare(-v.linear(), mc, Iout_.template block<3, 3>(ANGULAR, ANGULAR)); |
| 801 | |||
| 802 | // TODO: I do not why, but depending on the CPU, these three lines can give | ||
| 803 | // wrong output. | ||
| 804 | // typename Symmetric3::AlphaSkewSquare mcxcx(I.mass(),I.lever()); | ||
| 805 | // const Symmetric3 I_mcxcx(I.inertia() - mcxcx); | ||
| 806 | // Iout_.template block<3,3>(ANGULAR,ANGULAR) += I_mcxcx.vxs(v.angular()); | ||
| 807 |
1/6✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
131 | Symmetric3 mcxcx(typename Symmetric3::AlphaSkewSquare(I.mass(), I.lever())); |
| 808 |
4/8✓ Branch 2 taken 131 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 131 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 131 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 131 times.
✗ Branch 12 not taken.
|
131 | Iout_.template block<3, 3>(ANGULAR, ANGULAR) += I.inertia().vxs(v.angular()); |
| 809 |
4/8✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 131 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 131 times.
✗ Branch 11 not taken.
|
131 | Iout_.template block<3, 3>(ANGULAR, ANGULAR) -= mcxcx.vxs(v.angular()); |
| 810 | 131 | } | |
| 811 | |||
| 812 | template<typename MotionDerived, typename M6> | ||
| 813 | 3 | static void ivx_impl( | |
| 814 | const MotionDense<MotionDerived> & v, | ||
| 815 | const InertiaTpl & I, | ||
| 816 | const Eigen::MatrixBase<M6> & Iout) | ||
| 817 | { | ||
| 818 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6); | ||
| 819 | 3 | M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout); | |
| 820 | |||
| 821 | // Block 1,1 | ||
| 822 |
3/9✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
3 | alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR)); |
| 823 | |||
| 824 | // Block 2,1 | ||
| 825 |
2/8✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
3 | const Vector3 mc(I.mass() * I.lever()); |
| 826 |
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 | skewSquare(mc, v.angular(), Iout_.template block<3, 3>(ANGULAR, LINEAR)); |
| 827 | |||
| 828 | // Block 1,2 | ||
| 829 |
3/9✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
3 | alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(LINEAR, ANGULAR)); |
| 830 | |||
| 831 | // Block 2,2 | ||
| 832 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | cross( |
| 833 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | -I.lever(), Iout_.template block<3, 3>(ANGULAR, LINEAR), |
| 834 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Iout_.template block<3, 3>(ANGULAR, ANGULAR)); |
| 835 |
4/8✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
|
3 | Iout_.template block<3, 3>(ANGULAR, ANGULAR) += I.inertia().svx(v.angular()); |
| 836 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
|
12 | for (int k = 0; k < 3; ++k) |
| 837 |
4/8✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
|
9 | Iout_.template block<3, 3>(ANGULAR, ANGULAR).col(k) += |
| 838 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
18 | I.lever().cross(Iout_.template block<3, 3>(LINEAR, ANGULAR).col(k)); |
| 839 | |||
| 840 | // Block 1,2 | ||
| 841 |
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 | Iout_.template block<3, 3>(LINEAR, ANGULAR) -= Iout_.template block<3, 3>(ANGULAR, LINEAR); |
| 842 | 3 | } | |
| 843 | |||
| 844 | // Getters | ||
| 845 | 2646706 | Scalar mass() const | |
| 846 | { | ||
| 847 | 2646706 | return m_mass; | |
| 848 | } | ||
| 849 | 2560410 | const Vector3 & lever() const | |
| 850 | { | ||
| 851 | 2626255 | return m_com; | |
| 852 | } | ||
| 853 | 1753542 | const Symmetric3 & inertia() const | |
| 854 | { | ||
| 855 | 1817202 | return m_inertia; | |
| 856 | } | ||
| 857 | |||
| 858 | 349410 | Scalar & mass() | |
| 859 | { | ||
| 860 | 349410 | return m_mass; | |
| 861 | } | ||
| 862 | 240644 | Vector3 & lever() | |
| 863 | { | ||
| 864 | 240893 | return m_com; | |
| 865 | } | ||
| 866 | 232039 | Symmetric3 & inertia() | |
| 867 | { | ||
| 868 | 232285 | return m_inertia; | |
| 869 | } | ||
| 870 | |||
| 871 | /// aI = aXb.act(bI) | ||
| 872 | template<typename S2, int O2> | ||
| 873 | 113254 | InertiaTpl se3Action_impl(const SE3Tpl<S2, O2> & M) const | |
| 874 | { | ||
| 875 | /* The multiplication RIR' has a particular form that could be used, however it | ||
| 876 | * does not seems to be more efficient, see http://stackoverflow.m_comom/questions/ | ||
| 877 | * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/ | ||
| 878 | return InertiaTpl( | ||
| 879 |
8/15✓ Branch 5 taken 113076 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 113076 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 113076 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 113076 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 113076 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 154 times.
✓ Branch 21 taken 112922 times.
✗ Branch 22 not taken.
✓ Branch 23 taken 154 times.
✗ Branch 24 not taken.
|
226263 | mass(), M.translation() + M.rotation() * lever(), inertia().rotate(M.rotation())); |
| 880 | } | ||
| 881 | |||
| 882 | /// bI = aXb.actInv(aI) | ||
| 883 | template<typename S2, int O2> | ||
| 884 | 9 | InertiaTpl se3ActionInverse_impl(const SE3Tpl<S2, O2> & M) const | |
| 885 | { | ||
| 886 | return InertiaTpl( | ||
| 887 |
3/6✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
27 | mass(), M.rotation().transpose() * (lever() - M.translation()), |
| 888 |
5/12✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
45 | inertia().rotate(M.rotation().transpose())); |
| 889 | } | ||
| 890 | |||
| 891 | template<typename MotionDerived> | ||
| 892 | 976 | Force vxiv(const MotionDense<MotionDerived> & v) const | |
| 893 | { | ||
| 894 |
4/12✓ Branch 2 taken 976 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 976 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 976 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 976 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
976 | const Vector3 & mcxw = mass() * lever().cross(v.angular()); |
| 895 |
4/13✓ Branch 1 taken 976 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 976 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 976 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 976 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
976 | const Vector3 & mv_mcxw = mass() * v.linear() - mcxw; |
| 896 | return Force( | ||
| 897 |
1/2✓ Branch 1 taken 976 times.
✗ Branch 2 not taken.
|
976 | v.angular().cross(mv_mcxw), |
| 898 |
2/4✓ Branch 1 taken 976 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 976 times.
✗ Branch 5 not taken.
|
1952 | v.angular().cross(lever().cross(mv_mcxw) + inertia() * v.angular()) |
| 899 |
9/18✓ Branch 1 taken 976 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 976 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 976 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 976 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 976 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 976 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 976 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 976 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 976 times.
✗ Branch 26 not taken.
|
3904 | - v.linear().cross(mcxw)); |
| 900 | } | ||
| 901 | |||
| 902 | 100009 | void disp_impl(std::ostream & os) const | |
| 903 | { | ||
| 904 |
0/2✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
100009 | os << " m = " << mass() << "\n" |
| 905 |
1/9✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 100009 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
|
100009 | << " c = " << lever().transpose() << "\n" |
| 906 |
2/4✓ Branch 1 taken 100009 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100009 times.
✗ Branch 5 not taken.
|
100009 | << " I = \n" |
| 907 |
3/6✓ Branch 2 taken 100009 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100009 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100009 times.
✗ Branch 9 not taken.
|
100009 | << inertia().matrix() << ""; |
| 908 | 100009 | } | |
| 909 | |||
| 910 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 911 | template<typename NewScalar> | ||
| 912 | 7261 | InertiaTpl<NewScalar, Options> cast() const | |
| 913 | { | ||
| 914 | return InertiaTpl<NewScalar, Options>( | ||
| 915 |
4/6✓ Branch 2 taken 4779 times.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4779 times.
✗ Branch 7 not taken.
|
9390 | pinocchio::cast<NewScalar>(mass()), lever().template cast<NewScalar>(), |
| 916 |
4/8✓ Branch 3 taken 6128 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1348 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1348 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1344 times.
✗ Branch 13 not taken.
|
22227 | inertia().template cast<NewScalar>()); |
| 917 | } | ||
| 918 | |||
| 919 | // TODO: adjust code | ||
| 920 | // /// \brief Check whether *this is a valid inertia, resulting from a positive mass | ||
| 921 | // distribution bool isValid() const | ||
| 922 | // { | ||
| 923 | // return | ||
| 924 | // (m_mass > Scalar(0) && m_inertia.isValid()) | ||
| 925 | // || (m_mass == Scalar(0) && (m_inertia.data().array() == Scalar(0)).all()); | ||
| 926 | // } | ||
| 927 | |||
| 928 | protected: | ||
| 929 | Scalar m_mass; | ||
| 930 | Vector3 m_com; | ||
| 931 | Symmetric3 m_inertia; | ||
| 932 | |||
| 933 | }; // class InertiaTpl | ||
| 934 | |||
| 935 | /** | ||
| 936 | * @brief A structure representing a pseudo inertia matrix. | ||
| 937 | * | ||
| 938 | * References: | ||
| 939 | * - Wensing, Patrick M., Sangbae Kim, and Jean-Jacques E. Slotine. "Linear matrix inequalities | ||
| 940 | * for physically consistent inertial parameter identification: A statistical perspective on the | ||
| 941 | * mass distribution." IEEE Robotics and Automation Letters 3.1 (2017): 60-67. | ||
| 942 | */ | ||
| 943 | template<typename _Scalar, int _Options> | ||
| 944 | struct PseudoInertiaTpl | ||
| 945 | { | ||
| 946 | typedef _Scalar Scalar; | ||
| 947 | enum | ||
| 948 | { | ||
| 949 | Options = _Options, | ||
| 950 | }; | ||
| 951 | typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; | ||
| 952 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
| 953 | typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3; | ||
| 954 | typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10; | ||
| 955 | typedef LogCholeskyParametersTpl<Scalar, Options> LogCholeskyParameters; | ||
| 956 | |||
| 957 | Scalar mass; ///< Mass of the pseudo inertia | ||
| 958 | Vector3 h; ///< Vector part of the pseudo inertia | ||
| 959 | Matrix3 sigma; ///< 3x3 matrix part of the pseudo inertia | ||
| 960 | |||
| 961 | 10 | PseudoInertiaTpl(Scalar mass, const Vector3 & h, const Matrix3 & sigma) | |
| 962 | 10 | : mass(mass) | |
| 963 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
10 | , h(h) |
| 964 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
10 | , sigma(sigma) |
| 965 | { | ||
| 966 | 10 | } | |
| 967 | |||
| 968 | /** | ||
| 969 | * @brief Converts the PseudoInertiaTpl object to a 4x4 matrix. | ||
| 970 | * @return A 4x4 pseudo inertia matrix. | ||
| 971 | */ | ||
| 972 | 6 | Matrix4 toMatrix() const | |
| 973 | { | ||
| 974 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
6 | Matrix4 pseudo_inertia = Matrix4::Zero(); |
| 975 |
1/5✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
6 | pseudo_inertia.template block<3, 3>(0, 0) = sigma; |
| 976 |
1/5✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
6 | pseudo_inertia.template block<3, 1>(0, 3) = h; |
| 977 |
2/8✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
6 | pseudo_inertia.template block<1, 3>(3, 0) = h.transpose(); |
| 978 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
6 | pseudo_inertia(3, 3) = mass; |
| 979 | 6 | return pseudo_inertia; | |
| 980 | } | ||
| 981 | |||
| 982 | /** | ||
| 983 | * @brief Constructs a PseudoInertiaTpl object from a 4x4 pseudo inertia matrix. | ||
| 984 | * @param pseudo_inertia A 4x4 pseudo inertia matrix. | ||
| 985 | * @return A PseudoInertiaTpl object. | ||
| 986 | */ | ||
| 987 | 2 | static PseudoInertiaTpl FromMatrix(const Matrix4 & pseudo_inertia) | |
| 988 | { | ||
| 989 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | Scalar mass = pseudo_inertia(3, 3); |
| 990 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | Vector3 h = pseudo_inertia.template block<3, 1>(0, 3); |
| 991 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | Matrix3 sigma = pseudo_inertia.template block<3, 3>(0, 0); |
| 992 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
4 | return PseudoInertiaTpl(mass, h, sigma); |
| 993 | } | ||
| 994 | |||
| 995 | /** | ||
| 996 | * @brief Constructs a PseudoInertiaTpl object from dynamic parameters. | ||
| 997 | * @param dynamic_params A 10-dimensional vector of dynamic parameters. | ||
| 998 | * @return A PseudoInertiaTpl object. | ||
| 999 | */ | ||
| 1000 | template<typename Vector10Like> | ||
| 1001 | static PseudoInertiaTpl | ||
| 1002 | 12 | FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & dynamic_params) | |
| 1003 | { | ||
| 1004 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
12 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, dynamic_params, 10, 1); |
| 1005 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
12 | Scalar mass = dynamic_params[0]; |
| 1006 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
12 | Vector3 h = dynamic_params.template segment<3>(1); |
| 1007 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
12 | Matrix3 I_bar; |
| 1008 | // clang-format off | ||
| 1009 |
6/12✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
|
24 | I_bar << dynamic_params[4], dynamic_params[5], dynamic_params[7], |
| 1010 |
6/12✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
|
12 | dynamic_params[5], dynamic_params[6], dynamic_params[8], |
| 1011 |
6/12✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
|
12 | dynamic_params[7], dynamic_params[8], dynamic_params[9]; |
| 1012 | // clang-format on | ||
| 1013 | |||
| 1014 |
5/14✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
|
12 | Matrix3 sigma = 0.5 * I_bar.trace() * Matrix3::Identity() - I_bar; |
| 1015 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
24 | return PseudoInertiaTpl(mass, h, sigma); |
| 1016 | } | ||
| 1017 | |||
| 1018 | /** | ||
| 1019 | * @brief Converts the PseudoInertiaTpl object to dynamic parameters. | ||
| 1020 | * @return A 10-dimensional vector of dynamic parameters. | ||
| 1021 | */ | ||
| 1022 | 4 | Vector10 toDynamicParameters() const | |
| 1023 | { | ||
| 1024 |
5/10✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
|
4 | Matrix3 I_bar = sigma.trace() * Matrix3::Identity() - sigma; |
| 1025 | |||
| 1026 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | Vector10 dynamic_params; |
| 1027 |
1/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
4 | dynamic_params[0] = mass; |
| 1028 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | dynamic_params.template segment<3>(1) = h; |
| 1029 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[4] = I_bar(0, 0); |
| 1030 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[5] = I_bar(0, 1); |
| 1031 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[6] = I_bar(1, 1); |
| 1032 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[7] = I_bar(0, 2); |
| 1033 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[8] = I_bar(1, 2); |
| 1034 |
2/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
4 | dynamic_params[9] = I_bar(2, 2); |
| 1035 | |||
| 1036 | 8 | return dynamic_params; | |
| 1037 | } | ||
| 1038 | |||
| 1039 | /** | ||
| 1040 | * @brief Constructs a PseudoInertiaTpl object from an InertiaTpl object. | ||
| 1041 | * @param inertia An InertiaTpl object. | ||
| 1042 | * @return A PseudoInertiaTpl object. | ||
| 1043 | */ | ||
| 1044 | 4 | static PseudoInertiaTpl FromInertia(const InertiaTpl<Scalar, Options> & inertia) | |
| 1045 | { | ||
| 1046 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | Vector10 dynamic_params = inertia.toDynamicParameters(); |
| 1047 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | return FromDynamicParameters(dynamic_params); |
| 1048 | } | ||
| 1049 | |||
| 1050 | /** | ||
| 1051 | * @brief Converts the PseudoInertiaTpl object to an InertiaTpl object. | ||
| 1052 | * @return An InertiaTpl object. | ||
| 1053 | */ | ||
| 1054 | 2 | InertiaTpl<Scalar, Options> toInertia() const | |
| 1055 | { | ||
| 1056 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Vector10 dynamic_params = toDynamicParameters(); |
| 1057 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return InertiaTpl<Scalar, Options>::FromDynamicParameters(dynamic_params); |
| 1058 | } | ||
| 1059 | |||
| 1060 | /** | ||
| 1061 | * @brief Constructs a PseudoInertiaTpl object from log Cholesky parameters. | ||
| 1062 | * @param log_cholesky A 10-dimensional vector of log Cholesky parameters. | ||
| 1063 | * @return A PseudoInertiaTpl object. | ||
| 1064 | */ | ||
| 1065 | static PseudoInertiaTpl FromLogCholeskyParameters(const LogCholeskyParameters & log_cholesky) | ||
| 1066 | { | ||
| 1067 | Vector10 dynamic_params = log_cholesky.toDynamicParameters(); | ||
| 1068 | return FromDynamicParameters(dynamic_params); | ||
| 1069 | } | ||
| 1070 | |||
| 1071 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 1072 | template<typename NewScalar> | ||
| 1073 | 1 | PseudoInertiaTpl<NewScalar, Options> cast() const | |
| 1074 | { | ||
| 1075 | return PseudoInertiaTpl<NewScalar, Options>( | ||
| 1076 | 2 | pinocchio::cast<NewScalar>(mass), h.template cast<NewScalar>(), | |
| 1077 |
0/10✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
1 | sigma.template cast<NewScalar>()); |
| 1078 | } | ||
| 1079 | |||
| 1080 | 1 | void disp_impl(std::ostream & os) const | |
| 1081 | { | ||
| 1082 | 1 | os << " m = " << mass << "\n" | |
| 1083 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | << " h = " << h.transpose() << "\n" |
| 1084 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | << " sigma = \n" |
| 1085 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | << sigma << ""; |
| 1086 | 1 | } | |
| 1087 | |||
| 1088 | 1 | friend std::ostream & operator<<(std::ostream & os, const PseudoInertiaTpl & pi) | |
| 1089 | { | ||
| 1090 | 1 | pi.disp_impl(os); | |
| 1091 | 1 | return os; | |
| 1092 | } | ||
| 1093 | }; | ||
| 1094 | |||
| 1095 | /** | ||
| 1096 | * @brief A structure representing log Cholesky parameters. | ||
| 1097 | * | ||
| 1098 | * References: | ||
| 1099 | * - Rucker, Caleb, and Patrick M. Wensing. "Smooth parameterization of rigid-body inertia." | ||
| 1100 | * IEEE Robotics and Automation Letters 7.2 (2022): 2771-2778. | ||
| 1101 | */ | ||
| 1102 | template<typename _Scalar, int _Options> | ||
| 1103 | struct LogCholeskyParametersTpl | ||
| 1104 | { | ||
| 1105 | typedef _Scalar Scalar; | ||
| 1106 | enum | ||
| 1107 | { | ||
| 1108 | Options = _Options, | ||
| 1109 | }; | ||
| 1110 | typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10; | ||
| 1111 | typedef Eigen::Matrix<Scalar, 10, 10, Options> Matrix10; | ||
| 1112 | typedef PseudoInertiaTpl<Scalar, Options> PseudoInertia; | ||
| 1113 | |||
| 1114 | Vector10 parameters; ///< 10-dimensional vector of log Cholesky parameters | ||
| 1115 | /** | ||
| 1116 | * @brief Constructor for LogCholeskyParametersTpl. | ||
| 1117 | * @param log_cholesky A 10-dimensional vector of log Cholesky parameters. | ||
| 1118 | */ | ||
| 1119 | 2 | explicit LogCholeskyParametersTpl(const Vector10 & log_cholesky) | |
| 1120 | 2 | : parameters(log_cholesky) | |
| 1121 | { | ||
| 1122 | 2 | } | |
| 1123 | /** | ||
| 1124 | * @brief Converts the LogCholeskyParametersTpl object to dynamic parameters. | ||
| 1125 | * @return A 10-dimensional vector of dynamic parameters. | ||
| 1126 | */ | ||
| 1127 | 7 | Vector10 toDynamicParameters() const | |
| 1128 | { | ||
| 1129 | using math::exp; | ||
| 1130 | using math::pow; | ||
| 1131 | |||
| 1132 | // clang-format off | ||
| 1133 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | Vector10 dynamic_params; |
| 1134 | |||
| 1135 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar alpha = parameters[0]; |
| 1136 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar d1 = parameters[1]; |
| 1137 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar d2 = parameters[2]; |
| 1138 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar d3 = parameters[3]; |
| 1139 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar s12 = parameters[4]; |
| 1140 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar s23 = parameters[5]; |
| 1141 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar s13 = parameters[6]; |
| 1142 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar t1 = parameters[7]; |
| 1143 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar t2 = parameters[8]; |
| 1144 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | const Scalar t3 = parameters[9]; |
| 1145 | |||
| 1146 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
7 | const Scalar exp_d1 = exp(d1); |
| 1147 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
7 | const Scalar exp_d2 = exp(d2); |
| 1148 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
7 | const Scalar exp_d3 = exp(d3); |
| 1149 | |||
| 1150 |
1/6✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
7 | dynamic_params[0] = 1; |
| 1151 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | dynamic_params[1] = t1; |
| 1152 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | dynamic_params[2] = t2; |
| 1153 |
1/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
7 | dynamic_params[3] = t3; |
| 1154 |
6/22✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
|
7 | dynamic_params[4] = pow(s23, 2) + pow(t2, 2) + pow(t3, 2) + pow(exp_d2, 2) + pow(exp_d3, 2); |
| 1155 |
1/16✓ Branch 1 taken 7 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.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
|
7 | dynamic_params[5] = -s12 * exp_d2 - s13 * s23 - t1 * t2; |
| 1156 |
7/26✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
|
7 | dynamic_params[6] = pow(s12, 2) + pow(s13, 2) + pow(t1, 2) + pow(t3, 2) + pow(exp_d1, 2) + pow(exp_d3, 2); |
| 1157 |
1/12✓ Branch 1 taken 7 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.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
7 | dynamic_params[7] = -s13 * exp_d3 - t1 * t3; |
| 1158 |
1/12✓ Branch 1 taken 7 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.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
7 | dynamic_params[8] = -s23 * exp_d3 - t2 * t3; |
| 1159 |
8/30✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ 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.
|
7 | dynamic_params[9] = pow(s12, 2) + pow(s13, 2) + pow(s23, 2) + pow(t1, 2) + pow(t2, 2) + pow(exp_d1, 2) + pow(exp_d2, 2); |
| 1160 | |||
| 1161 |
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.
|
7 | const Scalar exp_2_alpha = exp(2 * alpha); |
| 1162 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | dynamic_params *= exp_2_alpha; |
| 1163 | // clang-format on | ||
| 1164 | |||
| 1165 | 14 | return dynamic_params; | |
| 1166 | } | ||
| 1167 | |||
| 1168 | /** | ||
| 1169 | * @brief Converts the LogCholeskyParametersTpl object to a PseudoInertiaTpl object. | ||
| 1170 | * @return A PseudoInertiaTpl object. | ||
| 1171 | */ | ||
| 1172 | 2 | PseudoInertia toPseudoInertia() const | |
| 1173 | { | ||
| 1174 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Vector10 dynamic_params = toDynamicParameters(); |
| 1175 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return PseudoInertia::FromDynamicParameters(dynamic_params); |
| 1176 | } | ||
| 1177 | |||
| 1178 | /** | ||
| 1179 | * @brief Converts the LogCholeskyParametersTpl object to an InertiaTpl object. | ||
| 1180 | * @return An InertiaTpl object. | ||
| 1181 | */ | ||
| 1182 | 1 | InertiaTpl<Scalar, Options> toInertia() const | |
| 1183 | { | ||
| 1184 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Vector10 dynamic_params = toDynamicParameters(); |
| 1185 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | return InertiaTpl<Scalar, Options>::FromDynamicParameters(dynamic_params); |
| 1186 | } | ||
| 1187 | |||
| 1188 | /** | ||
| 1189 | * @brief Calculates the Jacobian of the log Cholesky parameters. | ||
| 1190 | * @return A 10x10 matrix representing the Jacobian. | ||
| 1191 | */ | ||
| 1192 | 2 | Matrix10 calculateJacobian() const | |
| 1193 | { | ||
| 1194 | using math::exp; | ||
| 1195 | using math::pow; | ||
| 1196 | |||
| 1197 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | Matrix10 jacobian = Matrix10::Zero(); |
| 1198 | |||
| 1199 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar alpha = parameters[0]; |
| 1200 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar d1 = parameters[1]; |
| 1201 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar d2 = parameters[2]; |
| 1202 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar d3 = parameters[3]; |
| 1203 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar s12 = parameters[4]; |
| 1204 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar s23 = parameters[5]; |
| 1205 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar s13 = parameters[6]; |
| 1206 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar t1 = parameters[7]; |
| 1207 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar t2 = parameters[8]; |
| 1208 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar t3 = parameters[9]; |
| 1209 | |||
| 1210 |
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.
|
2 | const Scalar exp_2alpha = exp(2 * alpha); |
| 1211 |
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.
|
2 | const Scalar exp_2d1 = exp(2 * d1); |
| 1212 |
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.
|
2 | const Scalar exp_2d2 = exp(2 * d2); |
| 1213 |
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.
|
2 | const Scalar exp_2d3 = exp(2 * d3); |
| 1214 | // const Scalar exp_d1 = exp(d1); | ||
| 1215 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
2 | const Scalar exp_d2 = exp(d2); |
| 1216 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
2 | const Scalar exp_d3 = exp(d3); |
| 1217 | |||
| 1218 | // clang-format off | ||
| 1219 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(0, 0) = 2 * exp_2alpha; |
| 1220 | |||
| 1221 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(1, 0) = 2 * t1 * exp_2alpha; |
| 1222 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | jacobian(1, 7) = exp_2alpha; |
| 1223 | |||
| 1224 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(2, 0) = 2 * t2 * exp_2alpha; |
| 1225 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | jacobian(2, 8) = exp_2alpha; |
| 1226 | |||
| 1227 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(3, 0) = 2 * t3 * exp_2alpha; |
| 1228 |
1/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | jacobian(3, 9) = exp_2alpha; |
| 1229 | |||
| 1230 |
4/24✓ 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 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
|
2 | jacobian(4, 0) = 2 * (pow(s23, 2) + pow(t2, 2) + pow(t3, 2) + exp_2d2 + exp_2d3) * exp_2alpha; |
| 1231 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(4, 2) = 2 * exp_2alpha * exp_2d2; |
| 1232 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(4, 3) = 2 * exp_2alpha * exp_2d3; |
| 1233 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(4, 5) = 2 * s23 * exp_2alpha; |
| 1234 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(4, 8) = 2 * t2 * exp_2alpha; |
| 1235 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(4, 9) = 2 * t3 * exp_2alpha; |
| 1236 | |||
| 1237 |
1/20✓ Branch 1 taken 2 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.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
|
2 | jacobian(5, 0) = -2 * (s12 * exp_d2 + s13 * s23 + t1 * t2) * exp_2alpha; |
| 1238 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(5, 2) = -s12 * exp_2alpha * exp_d2; |
| 1239 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(5, 4) = -exp_2alpha * exp_d2; |
| 1240 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(5, 5) = -s13 * exp_2alpha; |
| 1241 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(5, 6) = -s23 * exp_2alpha; |
| 1242 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(5, 7) = -t2 * exp_2alpha; |
| 1243 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(5, 8) = -t1 * exp_2alpha; |
| 1244 | |||
| 1245 |
5/28✓ 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 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ 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.
|
2 | jacobian(6, 0) = 2 * (pow(s12, 2) + pow(s13, 2) + pow(t1, 2) + pow(t3, 2) + exp_2d1 + exp_2d3) * exp_2alpha; |
| 1246 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(6, 1) = 2 * exp_2alpha * exp_2d1; |
| 1247 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(6, 3) = 2 * exp_2alpha * exp_2d3; |
| 1248 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(6, 4) = 2 * s12 * exp_2alpha; |
| 1249 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(6, 6) = 2 * s13 * exp_2alpha; |
| 1250 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(6, 7) = 2 * t1 * exp_2alpha; |
| 1251 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(6, 9) = 2 * t3 * exp_2alpha; |
| 1252 | |||
| 1253 |
1/16✓ Branch 1 taken 2 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.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
|
2 | jacobian(7, 0) = -2 * (s13 * exp_d3 + t1 * t3) * exp_2alpha; |
| 1254 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(7, 3) = -s13 * exp_2alpha * exp_d3; |
| 1255 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(7, 6) = -exp_2alpha * exp_d3; |
| 1256 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(7, 7) = -t3 * exp_2alpha; |
| 1257 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(7, 9) = -t1 * exp_2alpha; |
| 1258 | |||
| 1259 |
1/16✓ Branch 1 taken 2 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.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
|
2 | jacobian(8, 0) = -2 * (s23 * exp_d3 + t2 * t3) * exp_2alpha; |
| 1260 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(8, 3) = -s23 * exp_2alpha * exp_d3; |
| 1261 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(8, 5) = -exp_2alpha * exp_d3; |
| 1262 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(8, 8) = -t3 * exp_2alpha; |
| 1263 |
1/8✓ Branch 1 taken 2 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.
|
2 | jacobian(8, 9) = -t2 * exp_2alpha; |
| 1264 | |||
| 1265 |
6/32✓ 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.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ 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.
|
2 | jacobian(9, 0) = 2 * (pow(s12, 2) + pow(s13, 2) + pow(s23, 2) + pow(t1, 2) + pow(t2, 2) + exp_2d1 + exp_2d2) * exp_2alpha; |
| 1266 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(9, 1) = 2 * exp_2alpha * exp_2d1; |
| 1267 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(9, 2) = 2 * exp_2alpha * exp_2d2; |
| 1268 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(9, 4) = 2 * s12 * exp_2alpha; |
| 1269 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(9, 5) = 2 * s23 * exp_2alpha; |
| 1270 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(9, 6) = 2 * s13 * exp_2alpha; |
| 1271 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(9, 7) = 2 * t1 * exp_2alpha; |
| 1272 |
1/10✓ Branch 1 taken 2 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.
|
2 | jacobian(9, 8) = 2 * t2 * exp_2alpha; |
| 1273 | // clang-format on | ||
| 1274 | |||
| 1275 | 4 | return jacobian; | |
| 1276 | } | ||
| 1277 | |||
| 1278 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 1279 | template<typename NewScalar> | ||
| 1280 | ✗ | LogCholeskyParametersTpl<NewScalar, Options> cast() const | |
| 1281 | { | ||
| 1282 | ✗ | return LogCholeskyParametersTpl<NewScalar, Options>(parameters.template cast<NewScalar>()); | |
| 1283 | } | ||
| 1284 | |||
| 1285 | 1 | void disp_impl(std::ostream & os) const | |
| 1286 | { | ||
| 1287 | 1 | os << " alpha: " << parameters[0] << "\n" | |
| 1288 | 1 | << " d1: " << parameters[1] << "\n" | |
| 1289 | 1 | << " d2: " << parameters[2] << "\n" | |
| 1290 | 1 | << " d3: " << parameters[3] << "\n" | |
| 1291 | 1 | << " s12: " << parameters[4] << "\n" | |
| 1292 | 1 | << " s23: " << parameters[5] << "\n" | |
| 1293 | 1 | << " s13: " << parameters[6] << "\n" | |
| 1294 | 1 | << " t1: " << parameters[7] << "\n" | |
| 1295 | 1 | << " t2: " << parameters[8] << "\n" | |
| 1296 | 1 | << " t3: " << parameters[9] << ""; | |
| 1297 | 1 | } | |
| 1298 | |||
| 1299 | 1 | friend std::ostream & operator<<(std::ostream & os, const LogCholeskyParametersTpl & lcp) | |
| 1300 | { | ||
| 1301 | 1 | lcp.disp_impl(os); | |
| 1302 | 1 | return os; | |
| 1303 | } | ||
| 1304 | }; | ||
| 1305 | |||
| 1306 | } // namespace pinocchio | ||
| 1307 | |||
| 1308 | #endif // ifndef __pinocchio_spatial_inertia_hpp__ | ||
| 1309 |