| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 32 | 68 | 47.1% |
| Branches: | 19 | 76 | 25.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2019-2020 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__ | ||
| 6 | #define __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/fwd.hpp" | ||
| 9 | #include "pinocchio/spatial/inertia.hpp" | ||
| 10 | #include "pinocchio/math/rotation.hpp" | ||
| 11 | #include "pinocchio/math/matrix.hpp" | ||
| 12 | |||
| 13 | #include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp" | ||
| 14 | |||
| 15 | namespace pinocchio | ||
| 16 | { | ||
| 17 | |||
| 18 | template<typename Scalar, int Options = 0> | ||
| 19 | struct JointRevoluteUnboundedUnalignedTpl; | ||
| 20 | |||
| 21 | template<typename _Scalar, int _Options> | ||
| 22 | struct traits<JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options>> | ||
| 23 | { | ||
| 24 | enum | ||
| 25 | { | ||
| 26 | NQ = 2, | ||
| 27 | NV = 1 | ||
| 28 | }; | ||
| 29 | typedef _Scalar Scalar; | ||
| 30 | enum | ||
| 31 | { | ||
| 32 | Options = _Options | ||
| 33 | }; | ||
| 34 | |||
| 35 | typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; | ||
| 36 | typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; | ||
| 37 | |||
| 38 | typedef JointDataRevoluteUnboundedUnalignedTpl<Scalar, Options> JointDataDerived; | ||
| 39 | typedef JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> JointModelDerived; | ||
| 40 | typedef JointMotionSubspaceRevoluteUnalignedTpl<Scalar, Options> Constraint_t; | ||
| 41 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
| 42 | typedef MotionRevoluteUnalignedTpl<Scalar, Options> Motion_t; | ||
| 43 | typedef MotionZeroTpl<Scalar, Options> Bias_t; | ||
| 44 | typedef Eigen::Matrix<Scalar, 6, NV, Options> F_t; | ||
| 45 | |||
| 46 | // [ABA] | ||
| 47 | typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t; | ||
| 48 | typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t; | ||
| 49 | typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t; | ||
| 50 | |||
| 51 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
| 52 | }; | ||
| 53 | |||
| 54 | template<typename _Scalar, int _Options> | ||
| 55 | struct traits<JointDataRevoluteUnboundedUnalignedTpl<_Scalar, _Options>> | ||
| 56 | { | ||
| 57 | typedef JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options> JointDerived; | ||
| 58 | typedef _Scalar Scalar; | ||
| 59 | }; | ||
| 60 | |||
| 61 | template<typename _Scalar, int _Options> | ||
| 62 | struct traits<JointModelRevoluteUnboundedUnalignedTpl<_Scalar, _Options>> | ||
| 63 | { | ||
| 64 | typedef JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options> JointDerived; | ||
| 65 | typedef _Scalar Scalar; | ||
| 66 | }; | ||
| 67 | |||
| 68 | template<typename _Scalar, int _Options> | ||
| 69 | struct JointDataRevoluteUnboundedUnalignedTpl | ||
| 70 | : public JointDataBase<JointDataRevoluteUnboundedUnalignedTpl<_Scalar, _Options>> | ||
| 71 | { | ||
| 72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 73 | typedef JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options> JointDerived; | ||
| 74 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
| 75 | 1 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
| 76 | |||
| 77 | ConfigVector_t joint_q; | ||
| 78 | TangentVector_t joint_v; | ||
| 79 | |||
| 80 | Transformation_t M; | ||
| 81 | Constraint_t S; | ||
| 82 | Motion_t v; | ||
| 83 | Bias_t c; | ||
| 84 | |||
| 85 | // [ABA] specific data | ||
| 86 | U_t U; | ||
| 87 | D_t Dinv; | ||
| 88 | UD_t UDinv; | ||
| 89 | D_t StU; | ||
| 90 | |||
| 91 | 20 | JointDataRevoluteUnboundedUnalignedTpl() | |
| 92 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | : joint_q(Scalar(1), Scalar(0)) |
| 93 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | , joint_v(TangentVector_t::Zero()) |
| 94 | 20 | , M(Transformation_t::Identity()) | |
| 95 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | , S(Constraint_t::Vector3::Zero()) |
| 96 |
2/4✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
|
20 | , v(Constraint_t::Vector3::Zero(), (Scalar)0) |
| 97 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | , U(U_t::Zero()) |
| 98 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | , Dinv(D_t::Zero()) |
| 99 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | , UDinv(UD_t::Zero()) |
| 100 |
1/2✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
|
40 | , StU(D_t::Zero()) |
| 101 | { | ||
| 102 | 20 | } | |
| 103 | |||
| 104 | template<typename Vector3Like> | ||
| 105 | 3 | JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis) | |
| 106 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | : joint_q(Scalar(1), Scalar(0)) |
| 107 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | , joint_v(TangentVector_t::Zero()) |
| 108 | 3 | , M(Transformation_t::Identity()) | |
| 109 | 3 | , S(axis) | |
| 110 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | , v(axis, (Scalar)0) |
| 111 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | , U(U_t::Zero()) |
| 112 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | , Dinv(D_t::Zero()) |
| 113 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | , UDinv(UD_t::Zero()) |
| 114 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
6 | , StU(D_t::Zero()) |
| 115 | { | ||
| 116 | 3 | } | |
| 117 | |||
| 118 | 40 | static std::string classname() | |
| 119 | { | ||
| 120 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | return std::string("JointDataRevoluteUnboundedUnalignedTpl"); |
| 121 | } | ||
| 122 | ✗ | std::string shortname() const | |
| 123 | { | ||
| 124 | ✗ | return classname(); | |
| 125 | } | ||
| 126 | |||
| 127 | }; // struct JointDataRevoluteUnboundedUnalignedTpl | ||
| 128 | |||
| 129 | PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl); | ||
| 130 | |||
| 131 | template<typename _Scalar, int _Options> | ||
| 132 | struct JointModelRevoluteUnboundedUnalignedTpl | ||
| 133 | : public JointModelBase<JointModelRevoluteUnboundedUnalignedTpl<_Scalar, _Options>> | ||
| 134 | { | ||
| 135 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 136 | typedef JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options> JointDerived; | ||
| 137 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
| 138 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
| 139 | |||
| 140 | typedef JointModelBase<JointModelRevoluteUnboundedUnalignedTpl> Base; | ||
| 141 | using Base::id; | ||
| 142 | using Base::idx_q; | ||
| 143 | using Base::idx_v; | ||
| 144 | using Base::setIndexes; | ||
| 145 | |||
| 146 | 20 | JointModelRevoluteUnboundedUnalignedTpl() | |
| 147 |
1/2✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
|
20 | : axis(Vector3::UnitX()) |
| 148 | { | ||
| 149 | 20 | } | |
| 150 | |||
| 151 | JointModelRevoluteUnboundedUnalignedTpl(const Scalar & x, const Scalar & y, const Scalar & z) | ||
| 152 | : axis(x, y, z) | ||
| 153 | { | ||
| 154 | normalize(axis); | ||
| 155 | assert(isUnitary(axis) && "Rotation axis is not unitary"); | ||
| 156 | } | ||
| 157 | |||
| 158 | template<typename Vector3Like> | ||
| 159 | ✗ | JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis) | |
| 160 | ✗ | : axis(axis) | |
| 161 | { | ||
| 162 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like); | ||
| 163 | ✗ | assert(isUnitary(axis) && "Rotation axis is not unitary"); | |
| 164 | } | ||
| 165 | |||
| 166 | 3 | JointDataDerived createData() const | |
| 167 | { | ||
| 168 | 3 | return JointDataDerived(axis); | |
| 169 | } | ||
| 170 | |||
| 171 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
| 172 | { | ||
| 173 | ✗ | return {false, false}; | |
| 174 | } | ||
| 175 | |||
| 176 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
| 177 | { | ||
| 178 | ✗ | return {false}; | |
| 179 | } | ||
| 180 | |||
| 181 | using Base::isEqual; | ||
| 182 | ✗ | bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const | |
| 183 | { | ||
| 184 | ✗ | return Base::isEqual(other) && internal::comparison_eq(axis, other.axis); | |
| 185 | } | ||
| 186 | |||
| 187 | template<typename ConfigVector> | ||
| 188 | ✗ | void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
| 189 | { | ||
| 190 | ✗ | data.joint_q = qs.template segment<NQ>(idx_q()); | |
| 191 | |||
| 192 | ✗ | const Scalar & ca = data.joint_q(0); | |
| 193 | ✗ | const Scalar & sa = data.joint_q(1); | |
| 194 | |||
| 195 | ✗ | toRotationMatrix(axis, ca, sa, data.M.rotation()); | |
| 196 | } | ||
| 197 | |||
| 198 | template<typename TangentVector> | ||
| 199 | void | ||
| 200 | ✗ | calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs) | |
| 201 | const | ||
| 202 | { | ||
| 203 | ✗ | data.joint_v[0] = vs[idx_v()]; | |
| 204 | ✗ | data.v.angularRate() = data.joint_v[0]; | |
| 205 | } | ||
| 206 | |||
| 207 | template<typename ConfigVector, typename TangentVector> | ||
| 208 | ✗ | void calc( | |
| 209 | JointDataDerived & data, | ||
| 210 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
| 211 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
| 212 | { | ||
| 213 | ✗ | calc(data, qs.derived()); | |
| 214 | ✗ | data.joint_v[0] = vs[idx_v()]; | |
| 215 | ✗ | data.v.angularRate() = data.joint_v[0]; | |
| 216 | } | ||
| 217 | |||
| 218 | template<typename VectorLike, typename Matrix6Like> | ||
| 219 | ✗ | void calc_aba( | |
| 220 | JointDataDerived & data, | ||
| 221 | const Eigen::MatrixBase<VectorLike> & armature, | ||
| 222 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
| 223 | const bool update_I) const | ||
| 224 | { | ||
| 225 | ✗ | data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis; | |
| 226 | ✗ | data.Dinv[0] = | |
| 227 | ✗ | Scalar(1) / (axis.dot(data.U.template segment<3>(Motion::ANGULAR)) + armature[0]); | |
| 228 | ✗ | data.UDinv.noalias() = data.U * data.Dinv; | |
| 229 | |||
| 230 | ✗ | if (update_I) | |
| 231 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); | |
| 232 | } | ||
| 233 | |||
| 234 | 40 | static std::string classname() | |
| 235 | { | ||
| 236 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
40 | return std::string("JointModelRevoluteUnboundedUnaligned"); |
| 237 | } | ||
| 238 | ✗ | std::string shortname() const | |
| 239 | { | ||
| 240 | ✗ | return classname(); | |
| 241 | } | ||
| 242 | |||
| 243 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 244 | template<typename NewScalar> | ||
| 245 | ✗ | JointModelRevoluteUnboundedUnalignedTpl<NewScalar, Options> cast() const | |
| 246 | { | ||
| 247 | typedef JointModelRevoluteUnboundedUnalignedTpl<NewScalar, Options> ReturnType; | ||
| 248 | ✗ | ReturnType res(axis.template cast<NewScalar>()); | |
| 249 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
| 250 | ✗ | return res; | |
| 251 | } | ||
| 252 | |||
| 253 | // data | ||
| 254 | /// | ||
| 255 | /// \brief axis of rotation of the joint. | ||
| 256 | /// | ||
| 257 | Vector3 axis; | ||
| 258 | }; // struct JointModelRevoluteUnboundedUnalignedTpl | ||
| 259 | |||
| 260 | } // namespace pinocchio | ||
| 261 | |||
| 262 | #include <boost/type_traits.hpp> | ||
| 263 | |||
| 264 | namespace boost | ||
| 265 | { | ||
| 266 | template<typename Scalar, int Options> | ||
| 267 | struct has_nothrow_constructor< | ||
| 268 | ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options>> | ||
| 269 | : public integral_constant<bool, true> | ||
| 270 | { | ||
| 271 | }; | ||
| 272 | |||
| 273 | template<typename Scalar, int Options> | ||
| 274 | struct has_nothrow_copy<::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options>> | ||
| 275 | : public integral_constant<bool, true> | ||
| 276 | { | ||
| 277 | }; | ||
| 278 | |||
| 279 | template<typename Scalar, int Options> | ||
| 280 | struct has_nothrow_constructor< | ||
| 281 | ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar, Options>> | ||
| 282 | : public integral_constant<bool, true> | ||
| 283 | { | ||
| 284 | }; | ||
| 285 | |||
| 286 | template<typename Scalar, int Options> | ||
| 287 | struct has_nothrow_copy<::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar, Options>> | ||
| 288 | : public integral_constant<bool, true> | ||
| 289 | { | ||
| 290 | }; | ||
| 291 | } // namespace boost | ||
| 292 | |||
| 293 | #endif // ifndef __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__ | ||
| 294 |