| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint/joint-free-flyer.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 81 | 81 | 100.0% |
| Branches: | 45 | 83 | 54.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
| 3 | // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
| 4 | // | ||
| 5 | |||
| 6 | #ifndef __pinocchio_multibody_joint_free_flyer_hpp__ | ||
| 7 | #define __pinocchio_multibody_joint_free_flyer_hpp__ | ||
| 8 | |||
| 9 | #include "pinocchio/macros.hpp" | ||
| 10 | #include "pinocchio/spatial/inertia.hpp" | ||
| 11 | #include "pinocchio/spatial/explog.hpp" | ||
| 12 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
| 13 | #include "pinocchio/multibody/joint-motion-subspace.hpp" | ||
| 14 | #include "pinocchio/math/fwd.hpp" | ||
| 15 | #include "pinocchio/math/quaternion.hpp" | ||
| 16 | |||
| 17 | namespace pinocchio | ||
| 18 | { | ||
| 19 | |||
| 20 | template<typename Scalar, int Options> | ||
| 21 | struct JointMotionSubspaceIdentityTpl; | ||
| 22 | |||
| 23 | template<typename _Scalar, int _Options> | ||
| 24 | struct traits<JointMotionSubspaceIdentityTpl<_Scalar, _Options>> | ||
| 25 | { | ||
| 26 | typedef _Scalar Scalar; | ||
| 27 | enum | ||
| 28 | { | ||
| 29 | Options = _Options | ||
| 30 | }; | ||
| 31 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
| 32 | enum | ||
| 33 | { | ||
| 34 | LINEAR = 0, | ||
| 35 | ANGULAR = 3 | ||
| 36 | }; | ||
| 37 | typedef MotionTpl<Scalar, Options> JointMotion; | ||
| 38 | typedef Eigen::Matrix<Scalar, 6, 1, Options> JointForce; | ||
| 39 | typedef Eigen::Matrix<Scalar, 6, 6, Options> DenseBase; | ||
| 40 | typedef Eigen::Matrix<Scalar, 6, 6, Options> ReducedSquaredMatrix; | ||
| 41 | |||
| 42 | typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType; | ||
| 43 | typedef typename Matrix6::IdentityReturnType MatrixReturnType; | ||
| 44 | typedef typename Matrix6::IdentityReturnType StDiagonalMatrixSOperationReturnType; | ||
| 45 | }; // traits ConstraintRevolute | ||
| 46 | |||
| 47 | template<typename _Scalar, int _Options> | ||
| 48 | struct JointMotionSubspaceIdentityTpl | ||
| 49 | : JointMotionSubspaceBase<JointMotionSubspaceIdentityTpl<_Scalar, _Options>> | ||
| 50 | { | ||
| 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 52 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceIdentityTpl) | ||
| 53 | |||
| 54 | enum | ||
| 55 | { | ||
| 56 | NV = 6 | ||
| 57 | }; | ||
| 58 | |||
| 59 | template<typename Vector6Like> | ||
| 60 | JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const | ||
| 61 | { | ||
| 62 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6); | ||
| 63 | return JointMotion(vj); | ||
| 64 | } | ||
| 65 | |||
| 66 | template<typename S1, int O1> | ||
| 67 | 3006 | typename SE3Tpl<S1, O1>::ActionMatrixType se3Action(const SE3Tpl<S1, O1> & m) const | |
| 68 | { | ||
| 69 | 3006 | return m.toActionMatrix(); | |
| 70 | } | ||
| 71 | |||
| 72 | template<typename S1, int O1> | ||
| 73 | 25 | typename SE3Tpl<S1, O1>::ActionMatrixType se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
| 74 | { | ||
| 75 | 25 | return m.toActionMatrixInverse(); | |
| 76 | } | ||
| 77 | |||
| 78 | 33 | int nv_impl() const | |
| 79 | { | ||
| 80 | 33 | return NV; | |
| 81 | } | ||
| 82 | |||
| 83 | struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceIdentityTpl> | ||
| 84 | { | ||
| 85 | template<typename Derived> | ||
| 86 | typename ForceDense<Derived>::ToVectorConstReturnType | ||
| 87 | 1105 | operator*(const ForceDense<Derived> & phi) | |
| 88 | { | ||
| 89 | 1105 | return phi.toVector(); | |
| 90 | } | ||
| 91 | |||
| 92 | /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ | ||
| 93 | template<typename MatrixDerived> | ||
| 94 | typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived) | ||
| 95 | 77 | operator*(const Eigen::MatrixBase<MatrixDerived> & F) | |
| 96 | { | ||
| 97 | 77 | return F.derived(); | |
| 98 | } | ||
| 99 | }; | ||
| 100 | |||
| 101 | 1183 | TransposeConst transpose() const | |
| 102 | { | ||
| 103 | 1183 | return TransposeConst(); | |
| 104 | } | ||
| 105 | 38 | MatrixReturnType matrix_impl() const | |
| 106 | { | ||
| 107 | 38 | return DenseBase::Identity(); | |
| 108 | } | ||
| 109 | |||
| 110 | template<typename MotionDerived> | ||
| 111 | 5 | typename MotionDerived::ActionMatrixType motionAction(const MotionBase<MotionDerived> & v) const | |
| 112 | { | ||
| 113 | 5 | return v.toActionMatrix(); | |
| 114 | } | ||
| 115 | |||
| 116 | 28 | bool isEqual(const JointMotionSubspaceIdentityTpl &) const | |
| 117 | { | ||
| 118 | 28 | return true; | |
| 119 | } | ||
| 120 | |||
| 121 | }; // struct JointMotionSubspaceIdentityTpl | ||
| 122 | |||
| 123 | template<typename Scalar, int Options, typename Vector6Like> | ||
| 124 | 1980 | MotionRef<const Vector6Like> operator*( | |
| 125 | const JointMotionSubspaceIdentityTpl<Scalar, Options> &, | ||
| 126 | const Eigen::MatrixBase<Vector6Like> & v) | ||
| 127 | { | ||
| 128 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6); | ||
| 129 | // typedef typename JointMotionSubspaceIdentityTpl<Scalar,Options>::Motion Motion; | ||
| 130 | typedef MotionRef<const Vector6Like> Motion; | ||
| 131 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1980 | return Motion(v.derived()); |
| 132 | } | ||
| 133 | |||
| 134 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
| 135 | template<typename S1, int O1, typename S2, int O2> | ||
| 136 | inline typename InertiaTpl<S1, O1>::Matrix6 | ||
| 137 | 24 | operator*(const InertiaTpl<S1, O1> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &) | |
| 138 | { | ||
| 139 | 24 | return Y.matrix(); | |
| 140 | } | ||
| 141 | |||
| 142 | /* [ABA] Y*S operator*/ | ||
| 143 | template<typename Matrix6Like, typename S2, int O2> | ||
| 144 | 2 | inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*( | |
| 145 | const Eigen::MatrixBase<Matrix6Like> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &) | ||
| 146 | { | ||
| 147 | 2 | return Y.derived(); | |
| 148 | } | ||
| 149 | |||
| 150 | template<typename S1, int O1> | ||
| 151 | struct SE3GroupAction<JointMotionSubspaceIdentityTpl<S1, O1>> | ||
| 152 | { | ||
| 153 | typedef typename SE3Tpl<S1, O1>::ActionMatrixType ReturnType; | ||
| 154 | }; | ||
| 155 | |||
| 156 | template<typename S1, int O1, typename MotionDerived> | ||
| 157 | struct MotionAlgebraAction<JointMotionSubspaceIdentityTpl<S1, O1>, MotionDerived> | ||
| 158 | { | ||
| 159 | typedef typename SE3Tpl<S1, O1>::ActionMatrixType ReturnType; | ||
| 160 | }; | ||
| 161 | |||
| 162 | template<typename Scalar, int Options> | ||
| 163 | struct JointFreeFlyerTpl; | ||
| 164 | |||
| 165 | template<typename _Scalar, int _Options> | ||
| 166 | struct traits<JointFreeFlyerTpl<_Scalar, _Options>> | ||
| 167 | { | ||
| 168 | enum | ||
| 169 | { | ||
| 170 | NQ = 7, | ||
| 171 | NV = 6 | ||
| 172 | }; | ||
| 173 | typedef _Scalar Scalar; | ||
| 174 | enum | ||
| 175 | { | ||
| 176 | Options = _Options | ||
| 177 | }; | ||
| 178 | typedef JointDataFreeFlyerTpl<Scalar, Options> JointDataDerived; | ||
| 179 | typedef JointModelFreeFlyerTpl<Scalar, Options> JointModelDerived; | ||
| 180 | typedef JointMotionSubspaceIdentityTpl<Scalar, Options> Constraint_t; | ||
| 181 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
| 182 | typedef MotionTpl<Scalar, Options> Motion_t; | ||
| 183 | typedef MotionZeroTpl<Scalar, Options> Bias_t; | ||
| 184 | |||
| 185 | // [ABA] | ||
| 186 | typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t; | ||
| 187 | typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t; | ||
| 188 | typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t; | ||
| 189 | |||
| 190 | typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; | ||
| 191 | typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; | ||
| 192 | |||
| 193 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
| 194 | }; | ||
| 195 | |||
| 196 | template<typename _Scalar, int _Options> | ||
| 197 | struct traits<JointDataFreeFlyerTpl<_Scalar, _Options>> | ||
| 198 | { | ||
| 199 | typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived; | ||
| 200 | typedef _Scalar Scalar; | ||
| 201 | }; | ||
| 202 | |||
| 203 | template<typename _Scalar, int _Options> | ||
| 204 | struct traits<JointModelFreeFlyerTpl<_Scalar, _Options>> | ||
| 205 | { | ||
| 206 | typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived; | ||
| 207 | typedef _Scalar Scalar; | ||
| 208 | }; | ||
| 209 | |||
| 210 | template<typename _Scalar, int _Options> | ||
| 211 | struct JointDataFreeFlyerTpl : public JointDataBase<JointDataFreeFlyerTpl<_Scalar, _Options>> | ||
| 212 | { | ||
| 213 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 214 | typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived; | ||
| 215 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
| 216 | 87568 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
| 217 | |||
| 218 | ConfigVector_t joint_q; | ||
| 219 | TangentVector_t joint_v; | ||
| 220 | |||
| 221 | Constraint_t S; | ||
| 222 | Transformation_t M; | ||
| 223 | Motion_t v; | ||
| 224 | Bias_t c; | ||
| 225 | |||
| 226 | // [ABA] specific data | ||
| 227 | U_t U; | ||
| 228 | D_t Dinv; | ||
| 229 | UD_t UDinv; | ||
| 230 | D_t StU; | ||
| 231 | |||
| 232 | 3969 | JointDataFreeFlyerTpl() | |
| 233 |
1/2✓ Branch 2 taken 3967 times.
✗ Branch 3 not taken.
|
3969 | : joint_q(ConfigVector_t::Zero()) |
| 234 |
3/5✓ Branch 1 taken 19 times.
✓ Branch 2 taken 3948 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
|
3969 | , joint_v(TangentVector_t::Zero()) |
| 235 |
1/2✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
|
3969 | , M(Transformation_t::Identity()) |
| 236 |
1/2✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
|
3969 | , v(Motion_t::Zero()) |
| 237 |
3/5✓ Branch 1 taken 19 times.
✓ Branch 2 taken 3948 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
|
3969 | , U(U_t::Zero()) |
| 238 |
3/5✓ Branch 1 taken 19 times.
✓ Branch 2 taken 3948 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
|
3969 | , Dinv(D_t::Zero()) |
| 239 |
3/5✓ Branch 1 taken 19 times.
✓ Branch 2 taken 3948 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
|
3969 | , UDinv(UD_t::Identity()) |
| 240 |
3/5✓ Branch 2 taken 19 times.
✓ Branch 3 taken 3948 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 19 times.
✗ Branch 6 not taken.
|
7938 | , StU(D_t::Zero()) |
| 241 | { | ||
| 242 |
3/6✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
|
3969 | joint_q[6] = Scalar(1); |
| 243 | 3969 | } | |
| 244 | |||
| 245 | 144 | static std::string classname() | |
| 246 | { | ||
| 247 |
1/2✓ Branch 2 taken 144 times.
✗ Branch 3 not taken.
|
144 | return std::string("JointDataFreeFlyer"); |
| 248 | } | ||
| 249 | 3 | std::string shortname() const | |
| 250 | { | ||
| 251 | 3 | return classname(); | |
| 252 | } | ||
| 253 | |||
| 254 | }; // struct JointDataFreeFlyerTpl | ||
| 255 | |||
| 256 | /// @brief Free-flyer joint in \f$SE(3)\f$. | ||
| 257 | /// | ||
| 258 | /// A free-flyer joint adds seven coordinates to the configuration space. | ||
| 259 | /// Given a configuration vector `q`: | ||
| 260 | /// | ||
| 261 | /// - `q[idx_q:idx_q + 3]` are the translation coordinates, in meters, | ||
| 262 | /// representing the position of the child frame in the parent frame. | ||
| 263 | /// - `q[idx_q + 3:idx_q + 7]` is a unit quaternion representing the rotation | ||
| 264 | /// from the child frame to the parent frame, with quaternion coordinates | ||
| 265 | /// ordered as (x, y, z, w). | ||
| 266 | /// | ||
| 267 | /// Likewise, a free-flyer joint adds six coordinates to the tangent space. | ||
| 268 | /// Let's consider a tangent vector `v`, say, a velocity vector. Following | ||
| 269 | /// Featherstone's convention, all our tangent vectors are body rather than | ||
| 270 | /// spatial vectors: | ||
| 271 | /// | ||
| 272 | /// - `v[idx_v:idx_v + 3]` is the linear velocity, in meters / second, | ||
| 273 | /// corresponding to the linear velocity of the child frame with respect | ||
| 274 | /// to the parent frame, expressed in the child frame (body linear velocity | ||
| 275 | /// of the child frame). | ||
| 276 | /// - `v[idx_v + 3:idx_v + 6]` is the angular velocity, in radians / second, | ||
| 277 | /// corresponding to the angular velocity from the child frame to the | ||
| 278 | /// parent frame, expressed in the child frame (body angular velocity of the | ||
| 279 | /// child frame). | ||
| 280 | PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl); | ||
| 281 | template<typename _Scalar, int _Options> | ||
| 282 | struct JointModelFreeFlyerTpl : public JointModelBase<JointModelFreeFlyerTpl<_Scalar, _Options>> | ||
| 283 | { | ||
| 284 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 285 | typedef JointFreeFlyerTpl<_Scalar, _Options> JointDerived; | ||
| 286 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
| 287 | |||
| 288 | typedef JointModelBase<JointModelFreeFlyerTpl> Base; | ||
| 289 | using Base::id; | ||
| 290 | using Base::idx_q; | ||
| 291 | using Base::idx_v; | ||
| 292 | using Base::setIndexes; | ||
| 293 | |||
| 294 | 3885 | JointDataDerived createData() const | |
| 295 | { | ||
| 296 | 3885 | return JointDataDerived(); | |
| 297 | } | ||
| 298 | |||
| 299 | 2 | const std::vector<bool> hasConfigurationLimit() const | |
| 300 | { | ||
| 301 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | return {true, true, true, false, false, false, false}; |
| 302 | } | ||
| 303 | |||
| 304 | 2 | const std::vector<bool> hasConfigurationLimitInTangent() const | |
| 305 | { | ||
| 306 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | return {true, true, true, false, false, false}; |
| 307 | } | ||
| 308 | |||
| 309 | template<typename ConfigVectorLike> | ||
| 310 | inline void forwardKinematics( | ||
| 311 | Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const | ||
| 312 | { | ||
| 313 | EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t, ConfigVectorLike); | ||
| 314 | typedef typename Eigen::Quaternion< | ||
| 315 | typename ConfigVectorLike::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> | ||
| 316 | Quaternion; | ||
| 317 | typedef Eigen::Map<const Quaternion> ConstQuaternionMap; | ||
| 318 | |||
| 319 | ConstQuaternionMap quat(q_joint.template tail<4>().data()); | ||
| 320 | // assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename | ||
| 321 | // V::Scalar>::epsilon())); TODO: check validity of the rhs precision | ||
| 322 | assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm() - 1)) <= 1e-4); | ||
| 323 | |||
| 324 | M.rotation(quat.matrix()); | ||
| 325 | M.translation(q_joint.template head<3>()); | ||
| 326 | } | ||
| 327 | |||
| 328 | template<typename Vector3Derived, typename QuaternionDerived> | ||
| 329 | 29882 | EIGEN_DONT_INLINE void calc( | |
| 330 | JointDataDerived & data, | ||
| 331 | const typename Eigen::MatrixBase<Vector3Derived> & trans, | ||
| 332 | const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const | ||
| 333 | { | ||
| 334 |
1/2✓ Branch 2 taken 29868 times.
✗ Branch 3 not taken.
|
29882 | data.M.translation(trans); |
| 335 |
1/2✓ Branch 2 taken 29868 times.
✗ Branch 3 not taken.
|
29882 | data.M.rotation(quat.matrix()); |
| 336 | 29882 | } | |
| 337 | |||
| 338 | template<typename ConfigVector> | ||
| 339 | EIGEN_DONT_INLINE void | ||
| 340 | 30257 | calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
| 341 | { | ||
| 342 | typedef typename Eigen::Quaternion<Scalar, Options> Quaternion; | ||
| 343 | typedef Eigen::Map<const Quaternion> ConstQuaternionMap; | ||
| 344 | |||
| 345 |
3/6✓ Branch 1 taken 29868 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29868 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29868 times.
✗ Branch 8 not taken.
|
30257 | data.joint_q = qs.template segment<NQ>(idx_q()); |
| 346 |
2/4✓ Branch 1 taken 29868 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 29868 times.
✗ Branch 6 not taken.
|
30257 | ConstQuaternionMap quat(data.joint_q.template tail<4>().data()); |
| 347 | |||
| 348 |
2/4✓ Branch 1 taken 29868 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29868 times.
✗ Branch 5 not taken.
|
30257 | calc(data, data.joint_q.template head<3>(), quat); |
| 349 | 30257 | } | |
| 350 | |||
| 351 | template<typename TangentVector> | ||
| 352 | EIGEN_DONT_INLINE void | ||
| 353 | 1 | calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs) | |
| 354 | const | ||
| 355 | { | ||
| 356 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | data.joint_v = vs.template segment<NV>(idx_v()); |
| 357 | 1 | data.v = data.joint_v; | |
| 358 | 1 | } | |
| 359 | |||
| 360 | template<typename ConfigVector, typename TangentVector> | ||
| 361 | 5596 | EIGEN_DONT_INLINE void calc( | |
| 362 | JointDataDerived & data, | ||
| 363 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
| 364 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
| 365 | { | ||
| 366 | 5596 | calc(data, qs.derived()); | |
| 367 | |||
| 368 |
1/2✓ Branch 3 taken 5568 times.
✗ Branch 4 not taken.
|
5596 | data.joint_v = vs.template segment<NV>(idx_v()); |
| 369 | 5596 | data.v = data.joint_v; | |
| 370 | 5596 | } | |
| 371 | |||
| 372 | template<typename VectorLike, typename Matrix6Like> | ||
| 373 | 393 | void calc_aba( | |
| 374 | JointDataDerived & data, | ||
| 375 | const Eigen::MatrixBase<VectorLike> & armature, | ||
| 376 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
| 377 | const bool update_I) const | ||
| 378 | { | ||
| 379 | 393 | data.U = I; | |
| 380 | 393 | data.StU = I; | |
| 381 |
1/2✓ Branch 2 taken 384 times.
✗ Branch 3 not taken.
|
393 | data.StU.diagonal() += armature; |
| 382 | |||
| 383 | 393 | internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv); | |
| 384 |
2/4✓ Branch 2 taken 384 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 384 times.
✗ Branch 6 not taken.
|
393 | data.UDinv.noalias() = I * data.Dinv; |
| 385 | |||
| 386 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 382 times.
|
393 | if (update_I) |
| 387 |
3/6✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
|
3 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); |
| 388 | 393 | } | |
| 389 | |||
| 390 | 17512 | static std::string classname() | |
| 391 | { | ||
| 392 |
1/2✓ Branch 2 taken 17512 times.
✗ Branch 3 not taken.
|
17512 | return std::string("JointModelFreeFlyer"); |
| 393 | } | ||
| 394 | 17370 | std::string shortname() const | |
| 395 | { | ||
| 396 | 17370 | return classname(); | |
| 397 | } | ||
| 398 | |||
| 399 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 400 | template<typename NewScalar> | ||
| 401 | 28 | JointModelFreeFlyerTpl<NewScalar, Options> cast() const | |
| 402 | { | ||
| 403 | typedef JointModelFreeFlyerTpl<NewScalar, Options> ReturnType; | ||
| 404 | 28 | ReturnType res; | |
| 405 | 28 | res.setIndexes(id(), idx_q(), idx_v()); | |
| 406 | 28 | return res; | |
| 407 | } | ||
| 408 | |||
| 409 | }; // struct JointModelFreeFlyerTpl | ||
| 410 | |||
| 411 | } // namespace pinocchio | ||
| 412 | |||
| 413 | #include <boost/type_traits.hpp> | ||
| 414 | |||
| 415 | namespace boost | ||
| 416 | { | ||
| 417 | template<typename Scalar, int Options> | ||
| 418 | struct has_nothrow_constructor<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>> | ||
| 419 | : public integral_constant<bool, true> | ||
| 420 | { | ||
| 421 | }; | ||
| 422 | |||
| 423 | template<typename Scalar, int Options> | ||
| 424 | struct has_nothrow_copy<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>> | ||
| 425 | : public integral_constant<bool, true> | ||
| 426 | { | ||
| 427 | }; | ||
| 428 | |||
| 429 | template<typename Scalar, int Options> | ||
| 430 | struct has_nothrow_constructor<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>> | ||
| 431 | : public integral_constant<bool, true> | ||
| 432 | { | ||
| 433 | }; | ||
| 434 | |||
| 435 | template<typename Scalar, int Options> | ||
| 436 | struct has_nothrow_copy<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>> | ||
| 437 | : public integral_constant<bool, true> | ||
| 438 | { | ||
| 439 | }; | ||
| 440 | } // namespace boost | ||
| 441 | |||
| 442 | #endif // ifndef __pinocchio_multibody_joint_free_flyer_hpp__ | ||
| 443 |