| Directory: | ./ |
|---|---|
| File: | include/pinocchio/spatial/inertia.hpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 188 | 318 | 59.1% |
| Branches: | 180 | 732 | 24.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2021 CNRS INRIA | ||
| 3 | // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
| 4 | // | ||
| 5 | |||
| 6 | #ifndef __pinocchio_spatial_inertia_hpp__ | ||
| 7 | #define __pinocchio_spatial_inertia_hpp__ | ||
| 8 | |||
| 9 | #include "pinocchio/math/fwd.hpp" | ||
| 10 | #include "pinocchio/spatial/symmetric3.hpp" | ||
| 11 | #include "pinocchio/spatial/force.hpp" | ||
| 12 | #include "pinocchio/spatial/motion.hpp" | ||
| 13 | #include "pinocchio/spatial/skew.hpp" | ||
| 14 | |||
| 15 | namespace pinocchio | ||
| 16 | { | ||
| 17 | |||
| 18 | template<class Derived> | ||
| 19 | struct InertiaBase : NumericalBase<Derived> | ||
| 20 | { | ||
| 21 | SPATIAL_TYPEDEF_TEMPLATE(Derived); | ||
| 22 | |||
| 23 | 77822 | Derived & derived() | |
| 24 | { | ||
| 25 | 77822 | return *static_cast<Derived *>(this); | |
| 26 | } | ||
| 27 | 162666 | const Derived & derived() const | |
| 28 | { | ||
| 29 | 162666 | 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 | 875 | Matrix6 matrix() const | |
| 68 | { | ||
| 69 | 875 | return derived().matrix_impl(); | |
| 70 | } | ||
| 71 | 697 | operator Matrix6() const | |
| 72 | { | ||
| 73 | 697 | 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 | ✗ | Matrix6 inverse() const | |
| 82 | { | ||
| 83 | ✗ | return derived().inverse_impl(); | |
| 84 | } | ||
| 85 | |||
| 86 | Derived & operator=(const Derived & clone) | ||
| 87 | { | ||
| 88 | return derived().__equl__(clone); | ||
| 89 | } | ||
| 90 | 106 | bool operator==(const Derived & other) const | |
| 91 | { | ||
| 92 | 106 | return derived().isEqual(other); | |
| 93 | } | ||
| 94 | 648 | bool operator!=(const Derived & other) const | |
| 95 | { | ||
| 96 | 648 | return !(*this == other); | |
| 97 | } | ||
| 98 | |||
| 99 | 77820 | Derived & operator+=(const Derived & Yb) | |
| 100 | { | ||
| 101 | 77820 | return derived().__pequ__(Yb); | |
| 102 | } | ||
| 103 | ✗ | Derived operator+(const Derived & Yb) const | |
| 104 | { | ||
| 105 | ✗ | return derived().__plus__(Yb); | |
| 106 | } | ||
| 107 | 2 | Derived & operator-=(const Derived & Yb) | |
| 108 | { | ||
| 109 | 2 | return derived().__mequ__(Yb); | |
| 110 | } | ||
| 111 | ✗ | Derived operator-(const Derived & Yb) const | |
| 112 | { | ||
| 113 | ✗ | return derived().__minus__(Yb); | |
| 114 | } | ||
| 115 | |||
| 116 | template<typename MotionDerived> | ||
| 117 | ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options> | ||
| 118 | 119681 | operator*(const MotionDense<MotionDerived> & v) const | |
| 119 | { | ||
| 120 | 119681 | return derived().__mult__(v); | |
| 121 | } | ||
| 122 | |||
| 123 | template<typename MotionDerived> | ||
| 124 | 7222 | Scalar vtiv(const MotionDense<MotionDerived> & v) const | |
| 125 | { | ||
| 126 | 7222 | 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 | ✗ | vxi(const MotionDense<MotionDerived> & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout) | |
| 146 | { | ||
| 147 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6); | ||
| 148 | ✗ | Derived::vxi_impl(v, I, Iout); | |
| 149 | } | ||
| 150 | |||
| 151 | template<typename MotionDerived> | ||
| 152 | ✗ | Matrix6 vxi(const MotionDense<MotionDerived> & v) const | |
| 153 | { | ||
| 154 | ✗ | Matrix6 Iout; | |
| 155 | ✗ | vxi(v, derived(), Iout); | |
| 156 | ✗ | 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 | ✗ | ivx(const MotionDense<MotionDerived> & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout) | |
| 170 | { | ||
| 171 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6); | ||
| 172 | ✗ | Derived::ivx_impl(v, I, Iout); | |
| 173 | } | ||
| 174 | |||
| 175 | template<typename MotionDerived> | ||
| 176 | ✗ | Matrix6 ivx(const MotionDense<MotionDerived> & v) const | |
| 177 | { | ||
| 178 | ✗ | Matrix6 Iout; | |
| 179 | ✗ | ivx(v, derived(), Iout); | |
| 180 | ✗ | 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 | 43 | bool isApprox( | |
| 197 | const Derived & other, | ||
| 198 | const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | ||
| 199 | { | ||
| 200 | 43 | return derived().isApprox_impl(other, prec); | |
| 201 | } | ||
| 202 | |||
| 203 | 341 | bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
| 204 | { | ||
| 205 | 341 | return derived().isZero_impl(prec); | |
| 206 | } | ||
| 207 | |||
| 208 | /// aI = aXb.act(bI) | ||
| 209 | template<typename S2, int O2> | ||
| 210 | 63156 | Derived se3Action(const SE3Tpl<S2, O2> & M) const | |
| 211 | { | ||
| 212 | 63156 | return derived().se3Action_impl(M); | |
| 213 | } | ||
| 214 | |||
| 215 | /// bI = aXb.actInv(aI) | ||
| 216 | template<typename S2, int O2> | ||
| 217 | ✗ | Derived se3ActionInverse(const SE3Tpl<S2, O2> & M) const | |
| 218 | { | ||
| 219 | ✗ | return derived().se3ActionInverse_impl(M); | |
| 220 | } | ||
| 221 | |||
| 222 | ✗ | void disp(std::ostream & os) const | |
| 223 | { | ||
| 224 | ✗ | static_cast<const Derived *>(this)->disp_impl(os); | |
| 225 | } | ||
| 226 | ✗ | friend std::ostream & operator<<(std::ostream & os, const InertiaBase<Derived> & X) | |
| 227 | { | ||
| 228 | ✗ | X.disp(os); | |
| 229 | ✗ | return os; | |
| 230 | } | ||
| 231 | |||
| 232 | }; // class InertiaBase | ||
| 233 | |||
| 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 Matrix6 ActionMatrix_t; | ||
| 245 | typedef Vector3 Angular_t; | ||
| 246 | typedef Vector3 Linear_t; | ||
| 247 | typedef const Vector3 ConstAngular_t; | ||
| 248 | typedef const Vector3 ConstLinear_t; | ||
| 249 | typedef Eigen::Quaternion<T, U> Quaternion_t; | ||
| 250 | typedef SE3Tpl<T, U> SE3; | ||
| 251 | typedef ForceTpl<T, U> Force; | ||
| 252 | typedef MotionTpl<T, U> Motion; | ||
| 253 | typedef Symmetric3Tpl<T, U> Symmetric3; | ||
| 254 | enum | ||
| 255 | { | ||
| 256 | LINEAR = 0, | ||
| 257 | ANGULAR = 3 | ||
| 258 | }; | ||
| 259 | }; // traits InertiaTpl | ||
| 260 | |||
| 261 | template<typename _Scalar, int _Options> | ||
| 262 | struct InertiaTpl : public InertiaBase<InertiaTpl<_Scalar, _Options>> | ||
| 263 | { | ||
| 264 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 265 | |||
| 266 | SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl); | ||
| 267 | enum | ||
| 268 | { | ||
| 269 | Options = _Options | ||
| 270 | }; | ||
| 271 | |||
| 272 | typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare; | ||
| 273 | typedef typename Eigen::Matrix<Scalar, 10, 1, Options> Vector10; | ||
| 274 | |||
| 275 | // Constructors | ||
| 276 | ✗ | InertiaTpl() | |
| 277 | { | ||
| 278 | } | ||
| 279 | |||
| 280 | 1621 | InertiaTpl(const Scalar & mass, const Vector3 & com, const Matrix3 & rotational_inertia) | |
| 281 | 1621 | : m_mass(mass) | |
| 282 | 1621 | , m_com(com) | |
| 283 | 1621 | , m_inertia(rotational_inertia) | |
| 284 | { | ||
| 285 | 1621 | } | |
| 286 | |||
| 287 | ✗ | explicit InertiaTpl(const Matrix6 & I6) | |
| 288 | { | ||
| 289 | ✗ | assert(check_expression_if_real<Scalar>(pinocchio::isZero(I6 - I6.transpose()))); | |
| 290 | ✗ | mass() = I6(LINEAR, LINEAR); | |
| 291 | const typename Matrix6::template ConstFixedBlockXpr<3, 3>::Type mc_cross = | ||
| 292 | ✗ | I6.template block<3, 3>(ANGULAR, LINEAR); | |
| 293 | ✗ | lever() = unSkew(mc_cross); | |
| 294 | ✗ | lever() /= mass(); | |
| 295 | |||
| 296 | ✗ | Matrix3 I3(mc_cross * mc_cross); | |
| 297 | ✗ | I3 /= mass(); | |
| 298 | ✗ | I3 += I6.template block<3, 3>(ANGULAR, ANGULAR); | |
| 299 | ✗ | const Symmetric3 S3(I3); | |
| 300 | ✗ | inertia() = S3; | |
| 301 | } | ||
| 302 | |||
| 303 | 70985 | InertiaTpl(const Scalar & mass, const Vector3 & com, const Symmetric3 & rotational_inertia) | |
| 304 | 70985 | : m_mass(mass) | |
| 305 | 70985 | , m_com(com) | |
| 306 | 70985 | , m_inertia(rotational_inertia) | |
| 307 | { | ||
| 308 | 70985 | } | |
| 309 | |||
| 310 | 72261 | InertiaTpl(const InertiaTpl & clone) // Copy constructor | |
| 311 | 72261 | : m_mass(clone.mass()) | |
| 312 | 72261 | , m_com(clone.lever()) | |
| 313 | 72261 | , m_inertia(clone.inertia()) | |
| 314 | { | ||
| 315 | 72261 | } | |
| 316 | |||
| 317 | 92807 | InertiaTpl & operator=(const InertiaTpl & clone) // Copy assignment operator | |
| 318 | { | ||
| 319 | 92807 | m_mass = clone.mass(); | |
| 320 | 92807 | m_com = clone.lever(); | |
| 321 | 92807 | m_inertia = clone.inertia(); | |
| 322 | 92807 | return *this; | |
| 323 | } | ||
| 324 | |||
| 325 | template<typename S2, int O2> | ||
| 326 | ✗ | explicit InertiaTpl(const InertiaTpl<S2, O2> & clone) | |
| 327 | { | ||
| 328 | ✗ | *this = clone.template cast<Scalar>(); | |
| 329 | } | ||
| 330 | |||
| 331 | // Initializers | ||
| 332 | 23092 | static InertiaTpl Zero() | |
| 333 | { | ||
| 334 |
3/6✓ Branch 2 taken 23092 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23092 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23092 times.
✗ Branch 9 not taken.
|
46184 | return InertiaTpl(Scalar(0), Vector3::Zero(), Symmetric3::Zero()); |
| 335 | } | ||
| 336 | |||
| 337 | 2201 | void setZero() | |
| 338 | { | ||
| 339 | 2201 | mass() = Scalar(0); | |
| 340 | 2201 | lever().setZero(); | |
| 341 | 2201 | inertia().setZero(); | |
| 342 | 2201 | } | |
| 343 | |||
| 344 | 137 | static InertiaTpl Identity() | |
| 345 | { | ||
| 346 |
3/6✓ Branch 2 taken 137 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 137 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 137 times.
✗ Branch 9 not taken.
|
274 | return InertiaTpl(Scalar(1), Vector3::Zero(), Symmetric3::Identity()); |
| 347 | } | ||
| 348 | |||
| 349 | ✗ | void setIdentity() | |
| 350 | { | ||
| 351 | ✗ | mass() = Scalar(1); | |
| 352 | ✗ | lever().setZero(); | |
| 353 | ✗ | inertia().setIdentity(); | |
| 354 | } | ||
| 355 | |||
| 356 | 7539 | static InertiaTpl Random() | |
| 357 | { | ||
| 358 | // We have to shoot "I" definite positive and not only symmetric. | ||
| 359 | return InertiaTpl( | ||
| 360 |
4/8✓ Branch 2 taken 7539 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7539 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7539 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7539 times.
✗ Branch 12 not taken.
|
15078 | Eigen::internal::random<Scalar>() + 1, Vector3::Random(), Symmetric3::RandomPositive()); |
| 361 | } | ||
| 362 | |||
| 363 | /// | ||
| 364 | /// \brief Computes the Inertia of a sphere defined by its mass and its radius. | ||
| 365 | /// | ||
| 366 | /// \param[in] mass of the sphere. | ||
| 367 | /// \param[in] radius of the sphere. | ||
| 368 | /// | ||
| 369 | 6 | static InertiaTpl FromSphere(const Scalar mass, const Scalar radius) | |
| 370 | { | ||
| 371 | 6 | return FromEllipsoid(mass, radius, radius, radius); | |
| 372 | } | ||
| 373 | |||
| 374 | /// | ||
| 375 | /// \brief Computes the Inertia of an ellipsoid defined by its mass and main semi-axis | ||
| 376 | /// dimensions (x,y,z). | ||
| 377 | /// | ||
| 378 | /// \param[in] mass of the ellipsoid. | ||
| 379 | /// \param[in] x semi-axis dimension along the local X axis. | ||
| 380 | /// \param[in] y semi-axis dimension along the local Y axis. | ||
| 381 | /// \param[in] z semi-axis dimension along the local Z axis. | ||
| 382 | /// | ||
| 383 | static InertiaTpl | ||
| 384 | 11 | FromEllipsoid(const Scalar mass, const Scalar x, const Scalar y, const Scalar z) | |
| 385 | { | ||
| 386 | 11 | const Scalar a = mass * (y * y + z * z) / Scalar(5); | |
| 387 | 11 | const Scalar b = mass * (x * x + z * z) / Scalar(5); | |
| 388 | 11 | const Scalar c = mass * (y * y + x * x) / Scalar(5); | |
| 389 | return InertiaTpl( | ||
| 390 |
4/8✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11 times.
✗ Branch 11 not taken.
|
11 | mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, Scalar(0), Scalar(0), c)); |
| 391 | } | ||
| 392 | |||
| 393 | /// | ||
| 394 | /// \brief Computes the Inertia of a cylinder defined by its mass, radius and length along the Z | ||
| 395 | /// axis. | ||
| 396 | /// | ||
| 397 | /// \param[in] mass of the cylinder. | ||
| 398 | /// \param[in] radius of the cylinder. | ||
| 399 | /// \param[in] length of the cylinder. | ||
| 400 | /// | ||
| 401 | 7 | static InertiaTpl FromCylinder(const Scalar mass, const Scalar radius, const Scalar length) | |
| 402 | { | ||
| 403 | 7 | const Scalar radius_square = radius * radius; | |
| 404 | 7 | const Scalar a = mass * (radius_square / Scalar(4) + length * length / Scalar(12)); | |
| 405 | 7 | const Scalar c = mass * (radius_square / Scalar(2)); | |
| 406 | return InertiaTpl( | ||
| 407 |
4/8✓ 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.
|
7 | mass, Vector3::Zero(), Symmetric3(a, Scalar(0), a, Scalar(0), Scalar(0), c)); |
| 408 | } | ||
| 409 | |||
| 410 | /// | ||
| 411 | /// \brief Computes the Inertia of a box defined by its mass and main dimensions (x,y,z). | ||
| 412 | /// | ||
| 413 | /// \param[in] mass of the box. | ||
| 414 | /// \param[in] x dimension along the local X axis. | ||
| 415 | /// \param[in] y dimension along the local Y axis. | ||
| 416 | /// \param[in] z dimension along the local Z axis. | ||
| 417 | /// | ||
| 418 | 10 | static InertiaTpl FromBox(const Scalar mass, const Scalar x, const Scalar y, const Scalar z) | |
| 419 | { | ||
| 420 | 10 | const Scalar a = mass * (y * y + z * z) / Scalar(12); | |
| 421 | 10 | const Scalar b = mass * (x * x + z * z) / Scalar(12); | |
| 422 | 10 | const Scalar c = mass * (y * y + x * x) / Scalar(12); | |
| 423 | return InertiaTpl( | ||
| 424 |
4/8✓ 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.
|
10 | mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, Scalar(0), Scalar(0), c)); |
| 425 | } | ||
| 426 | |||
| 427 | /// | ||
| 428 | /// \brief Computes the Inertia of a capsule defined by its mass, radius and length along the Z | ||
| 429 | /// axis. Assumes a uniform density. | ||
| 430 | /// | ||
| 431 | /// \param[in] mass of the capsule. | ||
| 432 | /// \param[in] radius of the capsule. | ||
| 433 | /// \param[in] height of the capsule. | ||
| 434 | 7 | static InertiaTpl FromCapsule(const Scalar mass, const Scalar radius, const Scalar height) | |
| 435 | { | ||
| 436 | 7 | const Scalar pi = boost::math::constants::pi<Scalar>(); | |
| 437 | |||
| 438 | // first need to compute mass repartition between cylinder and halfsphere | ||
| 439 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | const Scalar v_cyl = pi * math::pow(radius, 2) * height; |
| 440 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | const Scalar v_hs = Scalar(2) / Scalar(3) * math::pow(radius, 3) * pi; |
| 441 | 7 | const Scalar total_v = v_cyl + Scalar(2) * v_hs; | |
| 442 | |||
| 443 | 7 | const Scalar m_cyl = mass * (v_cyl / total_v); | |
| 444 | 7 | const Scalar m_hs = mass * (v_hs / total_v); | |
| 445 | |||
| 446 | // Then Distance between halfSphere center and cylindere center. | ||
| 447 | 7 | const Scalar dist_hc_cyl = height / Scalar(2) + Scalar(0.375) * radius; | |
| 448 | |||
| 449 | // Computes inertia terms | ||
| 450 | 7 | const Scalar ix_c = | |
| 451 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
7 | m_cyl * (math::pow(height, 2) / Scalar(12) + math::pow(radius, 2) / Scalar(4)); |
| 452 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | const Scalar iz_c = m_cyl * math::pow(radius, 2) / Scalar(2); |
| 453 | |||
| 454 | // For halfsphere inertia see, "Dynamics: Theory and Applications" McGraw-Hill, New York, | ||
| 455 | // 1985, by T.R. Kane and D.A. Levinson, Appendix 23. | ||
| 456 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | const Scalar ix_hs = m_hs * math::pow(radius, 2) * Scalar(0.259375); |
| 457 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | const Scalar iz_hs = m_hs * math::pow(radius, 2) * Scalar(0.4); |
| 458 | |||
| 459 | // Put everything together using the parallel axis theorem | ||
| 460 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | const Scalar ix = ix_c + Scalar(2) * (ix_hs + m_hs * math::pow(dist_hc_cyl, 2)); |
| 461 | 7 | const Scalar iz = iz_c + Scalar(2) * iz_hs; | |
| 462 | |||
| 463 | return InertiaTpl( | ||
| 464 |
4/8✓ 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.
|
7 | mass, Vector3::Zero(), Symmetric3(ix, Scalar(0), ix, Scalar(0), Scalar(0), iz)); |
| 465 | } | ||
| 466 | |||
| 467 | ✗ | void setRandom() | |
| 468 | { | ||
| 469 | ✗ | mass() = static_cast<Scalar>(std::rand()) / static_cast<Scalar>(RAND_MAX); | |
| 470 | ✗ | lever().setRandom(); | |
| 471 | ✗ | inertia().setRandom(); | |
| 472 | } | ||
| 473 | |||
| 474 | template<typename Matrix6Like> | ||
| 475 | 875 | void matrix_impl(const Eigen::MatrixBase<Matrix6Like> & M_) const | |
| 476 | { | ||
| 477 | 875 | Matrix6Like & M = M_.const_cast_derived(); | |
| 478 | |||
| 479 |
1/2✓ Branch 2 taken 875 times.
✗ Branch 3 not taken.
|
875 | M.template block<3, 3>(LINEAR, LINEAR).setZero(); |
| 480 |
2/4✓ Branch 2 taken 875 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 875 times.
✗ Branch 7 not taken.
|
875 | M.template block<3, 3>(LINEAR, LINEAR).diagonal().fill(mass()); |
| 481 |
2/4✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
|
875 | M.template block<3, 3>(ANGULAR, LINEAR) = alphaSkew(mass(), lever()); |
| 482 |
3/6✓ Branch 2 taken 875 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 875 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 875 times.
✗ Branch 9 not taken.
|
875 | M.template block<3, 3>(LINEAR, ANGULAR) = -M.template block<3, 3>(ANGULAR, LINEAR); |
| 483 |
4/8✓ Branch 1 taken 875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 875 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 875 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 875 times.
✗ Branch 11 not taken.
|
875 | M.template block<3, 3>(ANGULAR, ANGULAR) = |
| 484 | 875 | (inertia() - AlphaSkewSquare(mass(), lever())).matrix(); | |
| 485 | 875 | } | |
| 486 | |||
| 487 | 875 | Matrix6 matrix_impl() const | |
| 488 | { | ||
| 489 | 875 | Matrix6 M; | |
| 490 | 875 | matrix_impl(M); | |
| 491 | 875 | return M; | |
| 492 | } | ||
| 493 | |||
| 494 | template<typename Matrix6Like> | ||
| 495 | ✗ | void inverse_impl(const Eigen::MatrixBase<Matrix6Like> & M_) const | |
| 496 | { | ||
| 497 | ✗ | Matrix6Like & M = M_.const_cast_derived(); | |
| 498 | ✗ | inertia().inverse(M.template block<3, 3>(ANGULAR, ANGULAR)); | |
| 499 | |||
| 500 | ✗ | M.template block<3, 3>(LINEAR, ANGULAR).noalias() = | |
| 501 | ✗ | -M.template block<3, 3>(ANGULAR, ANGULAR).colwise().cross(lever()); | |
| 502 | ✗ | M.template block<3, 3>(ANGULAR, LINEAR) = M.template block<3, 3>(LINEAR, ANGULAR).transpose(); | |
| 503 | |||
| 504 | ✗ | const Scalar &cx = lever()[0], cy = lever()[1], cz = lever()[2]; | |
| 505 | |||
| 506 | ✗ | M.template block<3, 3>(LINEAR, LINEAR).col(0).noalias() = | |
| 507 | ✗ | cy * M.template block<3, 3>(LINEAR, ANGULAR).col(2) | |
| 508 | ✗ | - cz * M.template block<3, 3>(LINEAR, ANGULAR).col(1); | |
| 509 | |||
| 510 | ✗ | M.template block<3, 3>(LINEAR, LINEAR).col(1).noalias() = | |
| 511 | ✗ | cz * M.template block<3, 3>(LINEAR, ANGULAR).col(0) | |
| 512 | ✗ | - cx * M.template block<3, 3>(LINEAR, ANGULAR).col(2); | |
| 513 | |||
| 514 | ✗ | M.template block<3, 3>(LINEAR, LINEAR).col(2).noalias() = | |
| 515 | ✗ | cx * M.template block<3, 3>(LINEAR, ANGULAR).col(1) | |
| 516 | ✗ | - cy * M.template block<3, 3>(LINEAR, ANGULAR).col(0); | |
| 517 | |||
| 518 | ✗ | const Scalar m_inv = Scalar(1) / mass(); | |
| 519 | ✗ | M.template block<3, 3>(LINEAR, LINEAR).diagonal().array() += m_inv; | |
| 520 | } | ||
| 521 | |||
| 522 | ✗ | Matrix6 inverse_impl() const | |
| 523 | { | ||
| 524 | ✗ | Matrix6 res; | |
| 525 | ✗ | inverse_impl(res); | |
| 526 | ✗ | return res; | |
| 527 | } | ||
| 528 | |||
| 529 | /** Returns the representation of the matrix as a vector of dynamic parameters. | ||
| 530 | * The parameters are given as | ||
| 531 | * \f$ v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T \f$ | ||
| 532 | * 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 | ||
| 533 | * origin at the barycenter and \f$ S(c) \f$ is the the skew matrix representation of the cross | ||
| 534 | * product operator. | ||
| 535 | */ | ||
| 536 | ✗ | Vector10 toDynamicParameters() const | |
| 537 | { | ||
| 538 | ✗ | Vector10 v; | |
| 539 | |||
| 540 | ✗ | v[0] = mass(); | |
| 541 | ✗ | v.template segment<3>(1).noalias() = mass() * lever(); | |
| 542 | ✗ | v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(), lever())).data(); | |
| 543 | |||
| 544 | ✗ | return v; | |
| 545 | } | ||
| 546 | |||
| 547 | /** Builds and inertia matrix from a vector of dynamic parameters. | ||
| 548 | * | ||
| 549 | * @param[in] params The dynamic parameters. | ||
| 550 | * | ||
| 551 | * The parameters are given as | ||
| 552 | * \f$ v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T \f$ | ||
| 553 | * where \f$ I = I_C + mS^T(c)S(c) \f$ and \f$ I_C \f$ has its origin at the barycenter. | ||
| 554 | */ | ||
| 555 | template<typename Vector10Like> | ||
| 556 | ✗ | static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & params) | |
| 557 | { | ||
| 558 | ✗ | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1); | |
| 559 | |||
| 560 | ✗ | const Scalar & mass = params[0]; | |
| 561 | ✗ | Vector3 lever = params.template segment<3>(1); | |
| 562 | ✗ | lever /= mass; | |
| 563 | |||
| 564 | return InertiaTpl( | ||
| 565 | ✗ | mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass, lever)); | |
| 566 | } | ||
| 567 | |||
| 568 | // Arithmetic operators | ||
| 569 | InertiaTpl & __equl__(const InertiaTpl & clone) | ||
| 570 | { | ||
| 571 | mass() = clone.mass(); | ||
| 572 | lever() = clone.lever(); | ||
| 573 | inertia() = clone.inertia(); | ||
| 574 | return *this; | ||
| 575 | } | ||
| 576 | |||
| 577 | // Required by std::vector boost::python bindings. | ||
| 578 | 107 | bool isEqual(const InertiaTpl & Y2) const | |
| 579 | { | ||
| 580 |
4/6✓ Branch 2 taken 106 times.
✓ Branch 3 taken 1 times.
✓ Branch 7 taken 106 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 106 times.
✗ Branch 13 not taken.
|
107 | return (mass() == Y2.mass()) && (lever() == Y2.lever()) && (inertia() == Y2.inertia()); |
| 581 | } | ||
| 582 | |||
| 583 | 43 | bool isApprox_impl( | |
| 584 | const InertiaTpl & other, | ||
| 585 | const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | ||
| 586 | { | ||
| 587 | using math::fabs; | ||
| 588 | 43 | return fabs(static_cast<Scalar>(mass() - other.mass())) <= prec | |
| 589 |
5/10✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 43 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 43 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 43 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 43 times.
✗ Branch 15 not taken.
|
43 | && lever().isApprox(other.lever(), prec) && inertia().isApprox(other.inertia(), prec); |
| 590 | } | ||
| 591 | |||
| 592 | 341 | bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
| 593 | { | ||
| 594 | using math::fabs; | ||
| 595 |
6/10✓ Branch 2 taken 20 times.
✓ Branch 3 taken 321 times.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 20 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 20 times.
✗ Branch 15 not taken.
|
341 | return fabs(mass()) <= prec && lever().isZero(prec) && inertia().isZero(prec); |
| 596 | } | ||
| 597 | |||
| 598 | ✗ | InertiaTpl __plus__(const InertiaTpl & Yb) const | |
| 599 | { | ||
| 600 | /* Y_{a+b} = ( m_a+m_b, | ||
| 601 | * (m_a*c_a + m_b*c_b ) / (m_a + m_b), | ||
| 602 | * I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x ) | ||
| 603 | */ | ||
| 604 | |||
| 605 | ✗ | const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); | |
| 606 | |||
| 607 | ✗ | const Scalar & mab = mass() + Yb.mass(); | |
| 608 | ✗ | const Scalar mab_inv = Scalar(1) / math::max(mab, eps); | |
| 609 | ✗ | const Vector3 & AB = (lever() - Yb.lever()).eval(); | |
| 610 | return InertiaTpl( | ||
| 611 | ✗ | mab, (mass() * lever() + Yb.mass() * Yb.lever()) * mab_inv, | |
| 612 | ✗ | inertia() + Yb.inertia() | |
| 613 | ✗ | - (mass() * Yb.mass() * mab_inv) * typename Symmetric3::SkewSquare(AB)); | |
| 614 | } | ||
| 615 | |||
| 616 | 77820 | InertiaTpl & __pequ__(const InertiaTpl & Yb) | |
| 617 | { | ||
| 618 | static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); | ||
| 619 | |||
| 620 | 77820 | const InertiaTpl & Ya = *this; | |
| 621 | 77820 | const Scalar & mab = mass() + Yb.mass(); | |
| 622 |
1/2✓ Branch 1 taken 77820 times.
✗ Branch 2 not taken.
|
77820 | const Scalar mab_inv = Scalar(1) / math::max(mab, eps); |
| 623 |
2/4✓ Branch 3 taken 77820 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 77820 times.
✗ Branch 7 not taken.
|
77820 | const Vector3 & AB = (Ya.lever() - Yb.lever()).eval(); |
| 624 |
1/2✓ Branch 3 taken 77820 times.
✗ Branch 4 not taken.
|
77820 | lever() *= (mass() * mab_inv); |
| 625 |
2/4✓ Branch 3 taken 77820 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 77820 times.
✗ Branch 8 not taken.
|
77820 | lever() += (Yb.mass() * mab_inv) * Yb.lever(); // c *= mab_inv; |
| 626 |
1/2✓ Branch 3 taken 77820 times.
✗ Branch 4 not taken.
|
77820 | inertia() += Yb.inertia(); |
| 627 |
2/4✓ Branch 4 taken 77820 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 77820 times.
✗ Branch 9 not taken.
|
77820 | inertia() -= (Ya.mass() * Yb.mass() * mab_inv) * typename Symmetric3::SkewSquare(AB); |
| 628 | 77820 | mass() = mab; | |
| 629 | 77820 | return *this; | |
| 630 | } | ||
| 631 | |||
| 632 | ✗ | InertiaTpl __minus__(const InertiaTpl & Yb) const | |
| 633 | { | ||
| 634 | /* Y_{a} = ( m_{a+b}+m_b, | ||
| 635 | * (m_{a+b}*c_{a+b} - m_b*c_b ) / (m_a), | ||
| 636 | * I_{a+b} - I_b + (m_a*m_b)/(m_a+m_b) * AB_x * AB_x ) | ||
| 637 | */ | ||
| 638 | |||
| 639 | ✗ | const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); | |
| 640 | |||
| 641 | ✗ | const Scalar ma = mass() - Yb.mass(); | |
| 642 | ✗ | assert(check_expression_if_real<Scalar>(ma >= Scalar(0))); | |
| 643 | |||
| 644 | ✗ | const Scalar ma_inv = Scalar(1) / math::max(ma, eps); | |
| 645 | ✗ | const Vector3 c_a((mass() * lever() - Yb.mass() * Yb.lever()) * ma_inv); | |
| 646 | |||
| 647 | ✗ | const Vector3 AB = c_a - Yb.lever(); | |
| 648 | |||
| 649 | return InertiaTpl( | ||
| 650 | ma, c_a, | ||
| 651 | ✗ | inertia() - Yb.inertia() + (ma * Yb.mass() / mass()) * typename Symmetric3::SkewSquare(AB)); | |
| 652 | } | ||
| 653 | |||
| 654 | 2 | InertiaTpl & __mequ__(const InertiaTpl & Yb) | |
| 655 | { | ||
| 656 | static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); | ||
| 657 | |||
| 658 | 2 | const Scalar ma = mass() - Yb.mass(); | |
| 659 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
|
2 | assert(check_expression_if_real<Scalar>(ma >= Scalar(0))); |
| 660 | |||
| 661 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | const Scalar ma_inv = Scalar(1) / math::max(ma, eps); |
| 662 | |||
| 663 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | lever() *= (mass() * ma_inv); |
| 664 |
3/6✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2 | lever().noalias() -= (Yb.mass() * ma_inv) * Yb.lever(); |
| 665 | |||
| 666 |
2/4✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
2 | const Vector3 AB = lever() - Yb.lever(); |
| 667 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | inertia() -= Yb.inertia(); |
| 668 |
3/6✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
|
2 | inertia() += (ma * Yb.mass() / mass()) * typename Symmetric3::SkewSquare(AB); |
| 669 | 2 | mass() = ma; | |
| 670 | |||
| 671 | 2 | return *this; | |
| 672 | } | ||
| 673 | |||
| 674 | template<typename MotionDerived> | ||
| 675 | ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options> | ||
| 676 | 119681 | __mult__(const MotionDense<MotionDerived> & v) const | |
| 677 | { | ||
| 678 | typedef ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options> | ||
| 679 | ReturnType; | ||
| 680 | 119681 | ReturnType f; | |
| 681 | 119681 | __mult__(v, f); | |
| 682 | 119681 | return f; | |
| 683 | } | ||
| 684 | |||
| 685 | template<typename MotionDerived, typename ForceDerived> | ||
| 686 | 288350 | void __mult__(const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f) const | |
| 687 | { | ||
| 688 |
7/14✓ Branch 3 taken 200125 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 200125 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 200125 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 200125 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 200125 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 200125 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 200125 times.
✗ Branch 23 not taken.
|
288350 | f.linear().noalias() = mass() * (v.linear() - lever().cross(v.angular())); |
| 689 |
2/4✓ Branch 2 taken 200125 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 200125 times.
✗ Branch 7 not taken.
|
288350 | Symmetric3::rhsMult(inertia(), v.angular(), f.angular()); |
| 690 |
3/6✓ Branch 3 taken 200125 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 200125 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 200125 times.
✗ Branch 10 not taken.
|
288350 | f.angular() += lever().cross(f.linear()); |
| 691 | // f.angular().noalias() = c.cross(f.linear()) + I*v.angular(); | ||
| 692 | 288350 | } | |
| 693 | |||
| 694 | template<typename MotionDerived> | ||
| 695 | 7222 | Scalar vtiv_impl(const MotionDense<MotionDerived> & v) const | |
| 696 | { | ||
| 697 |
2/4✓ Branch 2 taken 7222 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7222 times.
✗ Branch 6 not taken.
|
7222 | const Vector3 cxw(lever().cross(v.angular())); |
| 698 |
4/8✓ Branch 2 taken 7222 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7222 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7222 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7222 times.
✗ Branch 12 not taken.
|
7222 | Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2) * v.linear().dot(cxw)); |
| 699 |
3/6✓ Branch 2 taken 7222 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7222 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7222 times.
✗ Branch 10 not taken.
|
7222 | const Vector3 mcxcxw(-mass() * lever().cross(cxw)); |
| 700 |
2/4✓ Branch 1 taken 7222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7222 times.
✗ Branch 5 not taken.
|
7222 | res += v.angular().dot(mcxcxw); |
| 701 |
3/6✓ Branch 2 taken 7222 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7222 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7222 times.
✗ Branch 9 not taken.
|
7222 | res += inertia().vtiv(v.angular()); |
| 702 | |||
| 703 | 7222 | return res; | |
| 704 | } | ||
| 705 | |||
| 706 | template<typename MotionDerived> | ||
| 707 | 10414 | Matrix6 variation(const MotionDense<MotionDerived> & v) const | |
| 708 | { | ||
| 709 |
1/2✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
|
10414 | Matrix6 res; |
| 710 |
1/2✓ Branch 2 taken 10414 times.
✗ Branch 3 not taken.
|
10414 | const Motion mv(v * mass()); |
| 711 | |||
| 712 |
8/16✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10414 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10414 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10414 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10414 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10414 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10414 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10414 times.
✗ Branch 23 not taken.
|
31242 | res.template block<3, 3>(LINEAR, ANGULAR) = |
| 713 |
3/6✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 10414 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10414 times.
✗ Branch 10 not taken.
|
31242 | -skew(mv.linear()) - skewSquare(mv.angular(), lever()) + skewSquare(lever(), mv.angular()); |
| 714 |
2/4✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10414 times.
✗ Branch 5 not taken.
|
10414 | res.template block<3, 3>(ANGULAR, LINEAR) = |
| 715 |
2/4✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10414 times.
✗ Branch 5 not taken.
|
10414 | res.template block<3, 3>(LINEAR, ANGULAR).transpose(); |
| 716 | |||
| 717 | // res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as | ||
| 718 | // temporary variable res.template block<3,3>(ANGULAR,ANGULAR) = res.template | ||
| 719 | // block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose(); | ||
| 720 |
6/12✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10414 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10414 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10414 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10414 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10414 times.
✗ Branch 17 not taken.
|
20828 | res.template block<3, 3>(ANGULAR, ANGULAR) = |
| 721 |
2/4✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 10414 times.
✗ Branch 7 not taken.
|
20828 | -skewSquare(mv.linear(), lever()) - skewSquare(lever(), mv.linear()); |
| 722 | |||
| 723 |
4/8✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10414 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10414 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10414 times.
✗ Branch 11 not taken.
|
10414 | res.template block<3, 3>(LINEAR, LINEAR) = |
| 724 | 10414 | (inertia() - AlphaSkewSquare(mass(), lever())).matrix(); | |
| 725 | |||
| 726 |
2/4✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10414 times.
✗ Branch 5 not taken.
|
10414 | res.template block<3, 3>(ANGULAR, ANGULAR) -= |
| 727 |
4/8✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10414 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10414 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10414 times.
✗ Branch 11 not taken.
|
10414 | res.template block<3, 3>(LINEAR, LINEAR) * skew(v.angular()); |
| 728 |
3/6✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10414 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10414 times.
✗ Branch 8 not taken.
|
10414 | res.template block<3, 3>(ANGULAR, ANGULAR) += |
| 729 |
2/4✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10414 times.
✗ Branch 5 not taken.
|
10414 | cross(v.angular(), res.template block<3, 3>(LINEAR, LINEAR)); |
| 730 | |||
| 731 |
2/4✓ Branch 1 taken 10414 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10414 times.
✗ Branch 5 not taken.
|
10414 | res.template block<3, 3>(LINEAR, LINEAR).setZero(); |
| 732 | 20828 | return res; | |
| 733 | } | ||
| 734 | |||
| 735 | template<typename MotionDerived, typename M6> | ||
| 736 | ✗ | static void vxi_impl( | |
| 737 | const MotionDense<MotionDerived> & v, | ||
| 738 | const InertiaTpl & I, | ||
| 739 | const Eigen::MatrixBase<M6> & Iout) | ||
| 740 | { | ||
| 741 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6); | ||
| 742 | ✗ | M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout); | |
| 743 | |||
| 744 | // Block 1,1 | ||
| 745 | ✗ | alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR)); | |
| 746 | // Iout_.template block<3,3>(LINEAR,LINEAR) = alphaSkew(I.mass(),v.angular()); | ||
| 747 | ✗ | const Vector3 mc(I.mass() * I.lever()); | |
| 748 | |||
| 749 | // Block 1,2 | ||
| 750 | ✗ | skewSquare(-v.angular(), mc, Iout_.template block<3, 3>(LINEAR, ANGULAR)); | |
| 751 | |||
| 752 | //// Block 2,1 | ||
| 753 | ✗ | alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(ANGULAR, LINEAR)); | |
| 754 | ✗ | Iout_.template block<3, 3>(ANGULAR, LINEAR) -= Iout_.template block<3, 3>(LINEAR, ANGULAR); | |
| 755 | |||
| 756 | //// Block 2,2 | ||
| 757 | ✗ | skewSquare(-v.linear(), mc, Iout_.template block<3, 3>(ANGULAR, ANGULAR)); | |
| 758 | |||
| 759 | // TODO: I do not why, but depending on the CPU, these three lines can give | ||
| 760 | // wrong output. | ||
| 761 | // typename Symmetric3::AlphaSkewSquare mcxcx(I.mass(),I.lever()); | ||
| 762 | // const Symmetric3 I_mcxcx(I.inertia() - mcxcx); | ||
| 763 | // Iout_.template block<3,3>(ANGULAR,ANGULAR) += I_mcxcx.vxs(v.angular()); | ||
| 764 | ✗ | Symmetric3 mcxcx(typename Symmetric3::AlphaSkewSquare(I.mass(), I.lever())); | |
| 765 | ✗ | Iout_.template block<3, 3>(ANGULAR, ANGULAR) += I.inertia().vxs(v.angular()); | |
| 766 | ✗ | Iout_.template block<3, 3>(ANGULAR, ANGULAR) -= mcxcx.vxs(v.angular()); | |
| 767 | } | ||
| 768 | |||
| 769 | template<typename MotionDerived, typename M6> | ||
| 770 | ✗ | static void ivx_impl( | |
| 771 | const MotionDense<MotionDerived> & v, | ||
| 772 | const InertiaTpl & I, | ||
| 773 | const Eigen::MatrixBase<M6> & Iout) | ||
| 774 | { | ||
| 775 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6); | ||
| 776 | ✗ | M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout); | |
| 777 | |||
| 778 | // Block 1,1 | ||
| 779 | ✗ | alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR)); | |
| 780 | |||
| 781 | // Block 2,1 | ||
| 782 | ✗ | const Vector3 mc(I.mass() * I.lever()); | |
| 783 | ✗ | skewSquare(mc, v.angular(), Iout_.template block<3, 3>(ANGULAR, LINEAR)); | |
| 784 | |||
| 785 | // Block 1,2 | ||
| 786 | ✗ | alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(LINEAR, ANGULAR)); | |
| 787 | |||
| 788 | // Block 2,2 | ||
| 789 | ✗ | cross( | |
| 790 | ✗ | -I.lever(), Iout_.template block<3, 3>(ANGULAR, LINEAR), | |
| 791 | ✗ | Iout_.template block<3, 3>(ANGULAR, ANGULAR)); | |
| 792 | ✗ | Iout_.template block<3, 3>(ANGULAR, ANGULAR) += I.inertia().svx(v.angular()); | |
| 793 | ✗ | for (int k = 0; k < 3; ++k) | |
| 794 | ✗ | Iout_.template block<3, 3>(ANGULAR, ANGULAR).col(k) += | |
| 795 | ✗ | I.lever().cross(Iout_.template block<3, 3>(LINEAR, ANGULAR).col(k)); | |
| 796 | |||
| 797 | // Block 1,2 | ||
| 798 | ✗ | Iout_.template block<3, 3>(LINEAR, ANGULAR) -= Iout_.template block<3, 3>(ANGULAR, LINEAR); | |
| 799 | } | ||
| 800 | |||
| 801 | // Getters | ||
| 802 | 620090 | Scalar mass() const | |
| 803 | { | ||
| 804 | 620090 | return m_mass; | |
| 805 | } | ||
| 806 | 743584 | const Vector3 & lever() const | |
| 807 | { | ||
| 808 | 743584 | return m_com; | |
| 809 | } | ||
| 810 | 429881 | const Symmetric3 & inertia() const | |
| 811 | { | ||
| 812 | 429881 | return m_inertia; | |
| 813 | } | ||
| 814 | |||
| 815 | 203821 | Scalar & mass() | |
| 816 | { | ||
| 817 | 203821 | return m_mass; | |
| 818 | } | ||
| 819 | 132247 | Vector3 & lever() | |
| 820 | { | ||
| 821 | 132247 | return m_com; | |
| 822 | } | ||
| 823 | 124197 | Symmetric3 & inertia() | |
| 824 | { | ||
| 825 | 124197 | return m_inertia; | |
| 826 | } | ||
| 827 | |||
| 828 | /// aI = aXb.act(bI) | ||
| 829 | template<typename S2, int O2> | ||
| 830 | 63156 | InertiaTpl se3Action_impl(const SE3Tpl<S2, O2> & M) const | |
| 831 | { | ||
| 832 | /* The multiplication RIR' has a particular form that could be used, however it | ||
| 833 | * does not seems to be more efficient, see http://stackoverflow.m_comom/questions/ | ||
| 834 | * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/ | ||
| 835 | return InertiaTpl( | ||
| 836 |
6/12✓ Branch 5 taken 63156 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 63156 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 63156 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 63156 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 63156 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 63156 times.
✗ Branch 22 not taken.
|
126312 | mass(), M.translation() + M.rotation() * lever(), inertia().rotate(M.rotation())); |
| 837 | } | ||
| 838 | |||
| 839 | /// bI = aXb.actInv(aI) | ||
| 840 | template<typename S2, int O2> | ||
| 841 | ✗ | InertiaTpl se3ActionInverse_impl(const SE3Tpl<S2, O2> & M) const | |
| 842 | { | ||
| 843 | return InertiaTpl( | ||
| 844 | ✗ | mass(), M.rotation().transpose() * (lever() - M.translation()), | |
| 845 | ✗ | inertia().rotate(M.rotation().transpose())); | |
| 846 | } | ||
| 847 | |||
| 848 | template<typename MotionDerived> | ||
| 849 | 513 | Force vxiv(const MotionDense<MotionDerived> & v) const | |
| 850 | { | ||
| 851 |
4/8✓ Branch 2 taken 513 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 513 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 513 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 513 times.
✗ Branch 13 not taken.
|
513 | const Vector3 & mcxw = mass() * lever().cross(v.angular()); |
| 852 |
4/8✓ Branch 1 taken 513 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 513 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 513 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 513 times.
✗ Branch 12 not taken.
|
513 | const Vector3 & mv_mcxw = mass() * v.linear() - mcxw; |
| 853 | return Force( | ||
| 854 |
1/2✓ Branch 1 taken 513 times.
✗ Branch 2 not taken.
|
513 | v.angular().cross(mv_mcxw), |
| 855 |
2/4✓ Branch 1 taken 513 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 513 times.
✗ Branch 5 not taken.
|
1026 | v.angular().cross(lever().cross(mv_mcxw) + inertia() * v.angular()) |
| 856 |
9/18✓ Branch 1 taken 513 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 513 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 513 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 513 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 513 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 513 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 513 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 513 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 513 times.
✗ Branch 26 not taken.
|
2052 | - v.linear().cross(mcxw)); |
| 857 | } | ||
| 858 | |||
| 859 | ✗ | void disp_impl(std::ostream & os) const | |
| 860 | { | ||
| 861 | ✗ | os << " m = " << mass() << "\n" | |
| 862 | ✗ | << " c = " << lever().transpose() << "\n" | |
| 863 | ✗ | << " I = \n" | |
| 864 | ✗ | << inertia().matrix() << ""; | |
| 865 | } | ||
| 866 | |||
| 867 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 868 | template<typename NewScalar> | ||
| 869 | 1146 | InertiaTpl<NewScalar, Options> cast() const | |
| 870 | { | ||
| 871 | return InertiaTpl<NewScalar, Options>( | ||
| 872 |
2/6✓ Branch 2 taken 1146 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1146 times.
✗ Branch 7 not taken.
|
1146 | pinocchio::cast<NewScalar>(mass()), lever().template cast<NewScalar>(), |
| 873 |
1/2✓ Branch 3 taken 1146 times.
✗ Branch 4 not taken.
|
3438 | inertia().template cast<NewScalar>()); |
| 874 | } | ||
| 875 | |||
| 876 | // TODO: adjust code | ||
| 877 | // /// \brief Check whether *this is a valid inertia, resulting from a positive mass | ||
| 878 | // distribution bool isValid() const | ||
| 879 | // { | ||
| 880 | // return | ||
| 881 | // (m_mass > Scalar(0) && m_inertia.isValid()) | ||
| 882 | // || (m_mass == Scalar(0) && (m_inertia.data().array() == Scalar(0)).all()); | ||
| 883 | // } | ||
| 884 | |||
| 885 | protected: | ||
| 886 | Scalar m_mass; | ||
| 887 | Vector3 m_com; | ||
| 888 | Symmetric3 m_inertia; | ||
| 889 | |||
| 890 | }; // class InertiaTpl | ||
| 891 | |||
| 892 | } // namespace pinocchio | ||
| 893 | |||
| 894 | #endif // ifndef __pinocchio_spatial_inertia_hpp__ | ||
| 895 |