| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint/joint-helical.hpp |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 41 | 243 | 16.9% |
| Branches: | 12 | 352 | 3.4% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2022-2023 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_multibody_joint_helical_hpp__ | ||
| 6 | #define __pinocchio_multibody_joint_helical_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/math/sincos.hpp" | ||
| 9 | #include "pinocchio/spatial/inertia.hpp" | ||
| 10 | #include "pinocchio/multibody/joint-motion-subspace.hpp" | ||
| 11 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
| 12 | #include "pinocchio/spatial/spatial-axis.hpp" | ||
| 13 | #include "pinocchio/utils/axis-label.hpp" | ||
| 14 | |||
| 15 | namespace pinocchio | ||
| 16 | { | ||
| 17 | |||
| 18 | template<typename Scalar, int Options, int axis> | ||
| 19 | struct MotionHelicalTpl; | ||
| 20 | |||
| 21 | template<typename Scalar, int Options, int axis> | ||
| 22 | struct SE3GroupAction<MotionHelicalTpl<Scalar, Options, axis>> | ||
| 23 | { | ||
| 24 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
| 25 | }; | ||
| 26 | |||
| 27 | template<typename Scalar, int Options, int axis, typename MotionDerived> | ||
| 28 | struct MotionAlgebraAction<MotionHelicalTpl<Scalar, Options, axis>, MotionDerived> | ||
| 29 | { | ||
| 30 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
| 31 | }; | ||
| 32 | |||
| 33 | template<typename _Scalar, int _Options, int axis> | ||
| 34 | struct traits<MotionHelicalTpl<_Scalar, _Options, axis>> | ||
| 35 | { | ||
| 36 | typedef _Scalar Scalar; | ||
| 37 | enum | ||
| 38 | { | ||
| 39 | Options = _Options | ||
| 40 | }; | ||
| 41 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
| 42 | typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6; | ||
| 43 | typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; | ||
| 44 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
| 45 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; | ||
| 46 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; | ||
| 47 | typedef Vector3 AngularType; | ||
| 48 | typedef Vector3 LinearType; | ||
| 49 | typedef const Vector3 ConstAngularType; | ||
| 50 | typedef const Vector3 ConstLinearType; | ||
| 51 | typedef Matrix6 ActionMatrixType; | ||
| 52 | typedef MotionTpl<Scalar, Options> MotionPlain; | ||
| 53 | typedef MotionPlain PlainReturnType; | ||
| 54 | typedef Matrix4 HomogeneousMatrixType; | ||
| 55 | enum | ||
| 56 | { | ||
| 57 | LINEAR = 0, | ||
| 58 | ANGULAR = 3 | ||
| 59 | }; | ||
| 60 | }; // traits MotionHelicalTpl | ||
| 61 | |||
| 62 | template<typename Scalar, int Options, int axis> | ||
| 63 | struct TransformHelicalTpl; | ||
| 64 | |||
| 65 | template<typename _Scalar, int _Options, int _axis> | ||
| 66 | struct traits<TransformHelicalTpl<_Scalar, _Options, _axis>> | ||
| 67 | { | ||
| 68 | enum | ||
| 69 | { | ||
| 70 | axis = _axis, | ||
| 71 | Options = _Options, | ||
| 72 | LINEAR = 0, | ||
| 73 | ANGULAR = 3 | ||
| 74 | }; | ||
| 75 | typedef _Scalar Scalar; | ||
| 76 | typedef SE3Tpl<Scalar, Options> PlainType; | ||
| 77 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
| 78 | typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3; | ||
| 79 | typedef Matrix3 AngularType; | ||
| 80 | typedef Matrix3 AngularRef; | ||
| 81 | typedef Matrix3 ConstAngularRef; | ||
| 82 | typedef Vector3 LinearType; | ||
| 83 | typedef typename Vector3::ConstantReturnType LinearRef; | ||
| 84 | typedef const typename Vector3::ConstantReturnType ConstLinearRef; | ||
| 85 | typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType; | ||
| 86 | typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType; | ||
| 87 | }; // traits TransformHelicalTpl | ||
| 88 | |||
| 89 | template<typename Scalar, int Options, int axis> | ||
| 90 | struct SE3GroupAction<TransformHelicalTpl<Scalar, Options, axis>> | ||
| 91 | { | ||
| 92 | typedef typename traits<TransformHelicalTpl<Scalar, Options, axis>>::PlainType ReturnType; | ||
| 93 | }; | ||
| 94 | |||
| 95 | template<typename _Scalar, int _Options, int axis> | ||
| 96 | struct TransformHelicalTpl : SE3Base<TransformHelicalTpl<_Scalar, _Options, axis>> | ||
| 97 | { | ||
| 98 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 99 | PINOCCHIO_SE3_TYPEDEF_TPL(TransformHelicalTpl); | ||
| 100 | |||
| 101 | typedef SpatialAxis<axis + LINEAR> AxisLinear; | ||
| 102 | typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear; | ||
| 103 | |||
| 104 | ✗ | TransformHelicalTpl() | |
| 105 | { | ||
| 106 | } | ||
| 107 | 132 | TransformHelicalTpl(const Scalar & sin, const Scalar & cos, const Scalar & displacement) | |
| 108 | 132 | : m_sin(sin) | |
| 109 | 132 | , m_cos(cos) | |
| 110 | 132 | , m_displacement(displacement) | |
| 111 | { | ||
| 112 | 132 | } | |
| 113 | |||
| 114 | ✗ | PlainType plain() const | |
| 115 | { | ||
| 116 | ✗ | PlainType res(PlainType::Identity()); | |
| 117 | ✗ | _setRotation(res.rotation()); | |
| 118 | ✗ | res.translation()[axis] = m_displacement; | |
| 119 | ✗ | return res; | |
| 120 | } | ||
| 121 | |||
| 122 | ✗ | operator PlainType() const | |
| 123 | { | ||
| 124 | ✗ | return plain(); | |
| 125 | } | ||
| 126 | |||
| 127 | template<typename S2, int O2> | ||
| 128 | typename SE3GroupAction<TransformHelicalTpl>::ReturnType | ||
| 129 | se3action(const SE3Tpl<S2, O2> & m) const | ||
| 130 | { | ||
| 131 | typedef typename SE3GroupAction<TransformHelicalTpl>::ReturnType ReturnType; | ||
| 132 | ReturnType res; | ||
| 133 | switch (axis) | ||
| 134 | { | ||
| 135 | case 0: { | ||
| 136 | res.rotation().col(0) = m.rotation().col(0); | ||
| 137 | res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2); | ||
| 138 | res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1)); | ||
| 139 | break; | ||
| 140 | } | ||
| 141 | case 1: { | ||
| 142 | res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0); | ||
| 143 | res.rotation().col(1) = m.rotation().col(1); | ||
| 144 | res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2)); | ||
| 145 | break; | ||
| 146 | } | ||
| 147 | case 2: { | ||
| 148 | res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1); | ||
| 149 | res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0)); | ||
| 150 | res.rotation().col(2) = m.rotation().col(2); | ||
| 151 | break; | ||
| 152 | } | ||
| 153 | default: { | ||
| 154 | assert(false && "must never happen"); | ||
| 155 | break; | ||
| 156 | } | ||
| 157 | } | ||
| 158 | res.translation() = m.translation(); | ||
| 159 | res.translation()[axis] += m_displacement; | ||
| 160 | return res; | ||
| 161 | } | ||
| 162 | |||
| 163 | const Scalar & sin() const | ||
| 164 | { | ||
| 165 | return m_sin; | ||
| 166 | } | ||
| 167 | ✗ | Scalar & sin() | |
| 168 | { | ||
| 169 | ✗ | return m_sin; | |
| 170 | } | ||
| 171 | |||
| 172 | const Scalar & cos() const | ||
| 173 | { | ||
| 174 | return m_cos; | ||
| 175 | } | ||
| 176 | ✗ | Scalar & cos() | |
| 177 | { | ||
| 178 | ✗ | return m_cos; | |
| 179 | } | ||
| 180 | |||
| 181 | ✗ | const Scalar & displacement() const | |
| 182 | { | ||
| 183 | ✗ | return m_displacement; | |
| 184 | } | ||
| 185 | ✗ | Scalar & displacement() | |
| 186 | { | ||
| 187 | ✗ | return m_displacement; | |
| 188 | } | ||
| 189 | |||
| 190 | template<typename Scalar1, typename Scalar2, typename Scalar3> | ||
| 191 | ✗ | void setValues(const Scalar1 & sin, const Scalar2 & cos, const Scalar3 & displacement) | |
| 192 | { | ||
| 193 | ✗ | m_sin = sin; | |
| 194 | ✗ | m_cos = cos; | |
| 195 | ✗ | m_displacement = displacement; | |
| 196 | } | ||
| 197 | |||
| 198 | ✗ | LinearType translation() const | |
| 199 | { | ||
| 200 | ✗ | return CartesianAxis3Linear() * displacement(); | |
| 201 | } | ||
| 202 | ✗ | AngularType rotation() const | |
| 203 | { | ||
| 204 | ✗ | AngularType m(AngularType::Identity()); | |
| 205 | ✗ | _setRotation(m); | |
| 206 | ✗ | return m; | |
| 207 | } | ||
| 208 | |||
| 209 | ✗ | bool isEqual(const TransformHelicalTpl & other) const | |
| 210 | { | ||
| 211 | ✗ | return internal::comparison_eq(m_cos, other.m_cos) | |
| 212 | ✗ | && internal::comparison_eq(m_sin, other.m_sin) | |
| 213 | ✗ | && internal::comparison_eq(m_displacement, other.m_displacement); | |
| 214 | } | ||
| 215 | |||
| 216 | protected: | ||
| 217 | Scalar m_sin, m_cos, m_displacement; | ||
| 218 | ✗ | inline void _setRotation(typename PlainType::AngularRef & rot) const | |
| 219 | { | ||
| 220 | switch (axis) | ||
| 221 | { | ||
| 222 | case 0: { | ||
| 223 | ✗ | rot.coeffRef(1, 1) = m_cos; | |
| 224 | ✗ | rot.coeffRef(1, 2) = -m_sin; | |
| 225 | ✗ | rot.coeffRef(2, 1) = m_sin; | |
| 226 | ✗ | rot.coeffRef(2, 2) = m_cos; | |
| 227 | ✗ | break; | |
| 228 | } | ||
| 229 | case 1: { | ||
| 230 | ✗ | rot.coeffRef(0, 0) = m_cos; | |
| 231 | ✗ | rot.coeffRef(0, 2) = m_sin; | |
| 232 | ✗ | rot.coeffRef(2, 0) = -m_sin; | |
| 233 | ✗ | rot.coeffRef(2, 2) = m_cos; | |
| 234 | ✗ | break; | |
| 235 | } | ||
| 236 | case 2: { | ||
| 237 | ✗ | rot.coeffRef(0, 0) = m_cos; | |
| 238 | ✗ | rot.coeffRef(0, 1) = -m_sin; | |
| 239 | ✗ | rot.coeffRef(1, 0) = m_sin; | |
| 240 | ✗ | rot.coeffRef(1, 1) = m_cos; | |
| 241 | ✗ | break; | |
| 242 | } | ||
| 243 | default: { | ||
| 244 | assert(false && "must never happen"); | ||
| 245 | break; | ||
| 246 | } | ||
| 247 | } | ||
| 248 | } | ||
| 249 | }; // struct TransformHelicalTpl | ||
| 250 | |||
| 251 | template<typename _Scalar, int _Options, int axis> | ||
| 252 | struct MotionHelicalTpl : MotionBase<MotionHelicalTpl<_Scalar, _Options, axis>> | ||
| 253 | { | ||
| 254 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 255 | |||
| 256 | MOTION_TYPEDEF_TPL(MotionHelicalTpl); | ||
| 257 | typedef SpatialAxis<axis + ANGULAR> AxisAngular; | ||
| 258 | typedef typename AxisAngular::CartesianAxis3 CartesianAxis3Angular; | ||
| 259 | typedef SpatialAxis<axis + LINEAR> AxisLinear; | ||
| 260 | typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear; | ||
| 261 | |||
| 262 | ✗ | MotionHelicalTpl() | |
| 263 | { | ||
| 264 | } | ||
| 265 | |||
| 266 | 132 | MotionHelicalTpl(const Scalar & w, const Scalar & v) | |
| 267 | 132 | : m_w(w) | |
| 268 | 132 | , m_v(v) | |
| 269 | { | ||
| 270 | 132 | } | |
| 271 | |||
| 272 | ✗ | inline PlainReturnType plain() const | |
| 273 | { | ||
| 274 | ✗ | return PlainReturnType(CartesianAxis3Linear() * m_v, CartesianAxis3Angular() * m_w); | |
| 275 | } | ||
| 276 | |||
| 277 | template<typename OtherScalar> | ||
| 278 | MotionHelicalTpl __mult__(const OtherScalar & alpha) const | ||
| 279 | { | ||
| 280 | return MotionHelicalTpl(alpha * m_w, alpha * m_v); | ||
| 281 | } | ||
| 282 | |||
| 283 | template<typename MotionDerived> | ||
| 284 | ✗ | void setTo(MotionDense<MotionDerived> & m) const | |
| 285 | { | ||
| 286 | ✗ | for (Eigen::DenseIndex k = 0; k < 3; ++k) | |
| 287 | { | ||
| 288 | ✗ | m.angular()[k] = k == axis ? m_w : (Scalar)0; | |
| 289 | ✗ | m.linear()[k] = k == axis ? m_v : (Scalar)0; | |
| 290 | } | ||
| 291 | } | ||
| 292 | |||
| 293 | template<typename MotionDerived> | ||
| 294 | ✗ | inline void addTo(MotionDense<MotionDerived> & v) const | |
| 295 | { | ||
| 296 | typedef typename MotionDense<MotionDerived>::Scalar OtherScalar; | ||
| 297 | ✗ | v.angular()[axis] += (OtherScalar)m_w; | |
| 298 | ✗ | v.linear()[axis] += (OtherScalar)m_v; | |
| 299 | } | ||
| 300 | |||
| 301 | template<typename S2, int O2, typename D2> | ||
| 302 | ✗ | inline void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
| 303 | { | ||
| 304 | ✗ | v.angular().noalias() = m.rotation().col(axis) * m_w; | |
| 305 | ✗ | v.linear().noalias() = m.translation().cross(v.angular()) + m_v * (m.rotation().col(axis)); | |
| 306 | } | ||
| 307 | |||
| 308 | template<typename S2, int O2> | ||
| 309 | ✗ | MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const | |
| 310 | { | ||
| 311 | |||
| 312 | ✗ | MotionPlain res; | |
| 313 | ✗ | se3Action_impl(m, res); | |
| 314 | ✗ | return res; | |
| 315 | } | ||
| 316 | |||
| 317 | template<typename S2, int O2, typename D2> | ||
| 318 | ✗ | void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
| 319 | { | ||
| 320 | // Linear | ||
| 321 | ✗ | CartesianAxis3Linear::alphaCross(m_w, m.translation(), v.angular()); | |
| 322 | ✗ | v.linear().noalias() = | |
| 323 | ✗ | m.rotation().transpose() * v.angular() + m_v * (m.rotation().transpose().col(axis)); | |
| 324 | |||
| 325 | // Angular | ||
| 326 | ✗ | v.angular().noalias() = m.rotation().transpose().col(axis) * m_w; | |
| 327 | } | ||
| 328 | |||
| 329 | template<typename S2, int O2> | ||
| 330 | ✗ | MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const | |
| 331 | { | ||
| 332 | ✗ | MotionPlain res; | |
| 333 | ✗ | se3ActionInverse_impl(m, res); | |
| 334 | ✗ | return res; | |
| 335 | } | ||
| 336 | |||
| 337 | template<typename M1, typename M2> | ||
| 338 | ✗ | EIGEN_STRONG_INLINE void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const | |
| 339 | { | ||
| 340 | // Linear | ||
| 341 | ✗ | CartesianAxis3Linear::alphaCross(-m_w, v.linear(), mout.linear()); | |
| 342 | ✗ | CartesianAxis3Linear::alphaCross(-m_v, v.angular(), mout.angular()); | |
| 343 | ✗ | mout.linear() += mout.angular(); | |
| 344 | // Angular | ||
| 345 | ✗ | CartesianAxis3Angular::alphaCross(-m_w, v.angular(), mout.angular()); | |
| 346 | } | ||
| 347 | |||
| 348 | template<typename M1> | ||
| 349 | ✗ | MotionPlain motionAction(const MotionDense<M1> & v) const | |
| 350 | { | ||
| 351 | ✗ | MotionPlain res; | |
| 352 | ✗ | motionAction(v, res); | |
| 353 | ✗ | return res; | |
| 354 | } | ||
| 355 | |||
| 356 | ✗ | Scalar & angularRate() | |
| 357 | { | ||
| 358 | ✗ | return m_w; | |
| 359 | } | ||
| 360 | const Scalar & angularRate() const | ||
| 361 | { | ||
| 362 | return m_w; | ||
| 363 | } | ||
| 364 | |||
| 365 | ✗ | Scalar & linearRate() | |
| 366 | { | ||
| 367 | ✗ | return m_v; | |
| 368 | } | ||
| 369 | const Scalar & linearRate() const | ||
| 370 | { | ||
| 371 | return m_v; | ||
| 372 | } | ||
| 373 | |||
| 374 | ✗ | bool isEqual_impl(const MotionHelicalTpl & other) const | |
| 375 | { | ||
| 376 | ✗ | return internal::comparison_eq(m_w, other.m_w) && internal::comparison_eq(m_v, other.m_v); | |
| 377 | } | ||
| 378 | |||
| 379 | protected: | ||
| 380 | Scalar m_w, m_v; | ||
| 381 | }; // struct MotionHelicalTpl | ||
| 382 | template<typename S1, int O1, int axis, typename MotionDerived> | ||
| 383 | typename MotionDerived::MotionPlain | ||
| 384 | ✗ | operator+(const MotionHelicalTpl<S1, O1, axis> & m1, const MotionDense<MotionDerived> & m2) | |
| 385 | { | ||
| 386 | ✗ | typename MotionDerived::MotionPlain res(m2); | |
| 387 | ✗ | res += m1; | |
| 388 | ✗ | return res; | |
| 389 | } | ||
| 390 | |||
| 391 | template<typename MotionDerived, typename S2, int O2, int axis> | ||
| 392 | EIGEN_STRONG_INLINE typename MotionDerived::MotionPlain | ||
| 393 | ✗ | operator^(const MotionDense<MotionDerived> & m1, const MotionHelicalTpl<S2, O2, axis> & m2) | |
| 394 | { | ||
| 395 | ✗ | return m2.motionAction(m1); | |
| 396 | } | ||
| 397 | |||
| 398 | template<typename Scalar, int Options, int axis> | ||
| 399 | struct JointMotionSubspaceHelicalTpl; | ||
| 400 | |||
| 401 | template<typename Scalar, int Options, int axis> | ||
| 402 | struct SE3GroupAction<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>> | ||
| 403 | { | ||
| 404 | typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType; | ||
| 405 | }; | ||
| 406 | |||
| 407 | template<typename Scalar, int Options, int axis, typename MotionDerived> | ||
| 408 | struct MotionAlgebraAction<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>, MotionDerived> | ||
| 409 | { | ||
| 410 | typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType; | ||
| 411 | }; | ||
| 412 | |||
| 413 | template<typename Scalar, int Options, int axis, typename ForceDerived> | ||
| 414 | struct ConstraintForceOp<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>, ForceDerived> | ||
| 415 | { | ||
| 416 | typedef typename Eigen::Matrix<Scalar, 1, 1> ReturnType; | ||
| 417 | }; | ||
| 418 | |||
| 419 | template<typename Scalar, int Options, int axis, typename ForceSet> | ||
| 420 | struct ConstraintForceSetOp<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>, ForceSet> | ||
| 421 | { | ||
| 422 | typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> ReturnType; | ||
| 423 | }; | ||
| 424 | |||
| 425 | template<typename _Scalar, int _Options, int axis> | ||
| 426 | struct traits<JointMotionSubspaceHelicalTpl<_Scalar, _Options, axis>> | ||
| 427 | { | ||
| 428 | typedef _Scalar Scalar; | ||
| 429 | enum | ||
| 430 | { | ||
| 431 | Options = _Options | ||
| 432 | }; | ||
| 433 | enum | ||
| 434 | { | ||
| 435 | LINEAR = 0, | ||
| 436 | ANGULAR = 3 | ||
| 437 | }; | ||
| 438 | |||
| 439 | typedef MotionHelicalTpl<Scalar, Options, axis> JointMotion; | ||
| 440 | typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce; | ||
| 441 | typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase; | ||
| 442 | typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix; | ||
| 443 | |||
| 444 | typedef DenseBase MatrixReturnType; | ||
| 445 | typedef const DenseBase ConstMatrixReturnType; | ||
| 446 | |||
| 447 | typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType; | ||
| 448 | }; // traits JointMotionSubspaceHelicalTpl | ||
| 449 | |||
| 450 | template<class ConstraintDerived> | ||
| 451 | struct TransposeConstraintActionConstraint | ||
| 452 | { | ||
| 453 | typedef | ||
| 454 | typename Eigen::Matrix<typename ConstraintDerived::Scalar, 1, 1, ConstraintDerived::Options> | ||
| 455 | ReturnType; | ||
| 456 | }; | ||
| 457 | |||
| 458 | template<typename _Scalar, int _Options, int axis> | ||
| 459 | struct JointMotionSubspaceHelicalTpl | ||
| 460 | : JointMotionSubspaceBase<JointMotionSubspaceHelicalTpl<_Scalar, _Options, axis>> | ||
| 461 | { | ||
| 462 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 463 | |||
| 464 | PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceHelicalTpl) | ||
| 465 | enum | ||
| 466 | { | ||
| 467 | NV = 1 | ||
| 468 | }; | ||
| 469 | |||
| 470 | typedef SpatialAxis<ANGULAR + axis> AxisAngular; | ||
| 471 | typedef SpatialAxis<ANGULAR + axis> AxisLinear; | ||
| 472 | |||
| 473 | typedef typename AxisAngular::CartesianAxis3 CartesianAxis3Angular; | ||
| 474 | typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear; | ||
| 475 | |||
| 476 | ✗ | JointMotionSubspaceHelicalTpl() | |
| 477 | { | ||
| 478 | } | ||
| 479 | |||
| 480 | 136 | JointMotionSubspaceHelicalTpl(const Scalar & h) | |
| 481 | 136 | : m_pitch(h) | |
| 482 | { | ||
| 483 | 136 | } | |
| 484 | |||
| 485 | template<typename Vector1Like> | ||
| 486 | 4 | JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const | |
| 487 | { | ||
| 488 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1); | ||
| 489 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
|
4 | assert(v.size() == 1); |
| 490 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | return JointMotion(v[0], v[0] * m_pitch); |
| 491 | } | ||
| 492 | |||
| 493 | template<typename S1, int O1> | ||
| 494 | typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType | ||
| 495 | ✗ | se3Action(const SE3Tpl<S1, O1> & m) const | |
| 496 | { | ||
| 497 | typedef typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType ReturnType; | ||
| 498 | ✗ | ReturnType res; | |
| 499 | ✗ | res.template segment<3>(LINEAR) = | |
| 500 | ✗ | m.translation().cross(m.rotation().col(axis)) + m_pitch * (m.rotation().col(axis)); | |
| 501 | ✗ | res.template segment<3>(ANGULAR) = m.rotation().col(axis); | |
| 502 | ✗ | return res; | |
| 503 | } | ||
| 504 | |||
| 505 | template<typename S1, int O1> | ||
| 506 | typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType | ||
| 507 | ✗ | se3ActionInverse(const SE3Tpl<S1, O1> & m) const | |
| 508 | { | ||
| 509 | typedef typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType ReturnType; | ||
| 510 | typedef typename AxisAngular::CartesianAxis3 CartesianAxis3; | ||
| 511 | ✗ | ReturnType res; | |
| 512 | ✗ | res.template segment<3>(LINEAR).noalias() = | |
| 513 | ✗ | m.rotation().transpose() * CartesianAxis3::cross(m.translation()) | |
| 514 | ✗ | + m.rotation().transpose().col(axis) * m_pitch; | |
| 515 | ✗ | res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis); | |
| 516 | ✗ | return res; | |
| 517 | } | ||
| 518 | |||
| 519 | ✗ | int nv_impl() const | |
| 520 | { | ||
| 521 | ✗ | return NV; | |
| 522 | } | ||
| 523 | |||
| 524 | // For force T | ||
| 525 | struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceHelicalTpl> | ||
| 526 | { | ||
| 527 | const JointMotionSubspaceHelicalTpl & ref; | ||
| 528 | ✗ | TransposeConst(const JointMotionSubspaceHelicalTpl & ref) | |
| 529 | ✗ | : ref(ref) | |
| 530 | { | ||
| 531 | } | ||
| 532 | |||
| 533 | template<typename ForceDerived> | ||
| 534 | typename ConstraintForceOp<JointMotionSubspaceHelicalTpl, ForceDerived>::ReturnType | ||
| 535 | ✗ | operator*(const ForceDense<ForceDerived> & f) const | |
| 536 | { | ||
| 537 | ✗ | return Eigen::Matrix<Scalar, 1, 1>(f.angular()(axis) + f.linear()(axis) * ref.m_pitch); | |
| 538 | } | ||
| 539 | |||
| 540 | /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
| 541 | template<typename Derived> | ||
| 542 | typename ConstraintForceSetOp<JointMotionSubspaceHelicalTpl, Derived>::ReturnType | ||
| 543 | ✗ | operator*(const Eigen::MatrixBase<Derived> & F) const | |
| 544 | { | ||
| 545 | ✗ | assert(F.rows() == 6); | |
| 546 | ✗ | return F.row(ANGULAR + axis) + F.row(LINEAR + axis) * ref.m_pitch; | |
| 547 | } | ||
| 548 | }; // struct TransposeConst | ||
| 549 | |||
| 550 | ✗ | TransposeConst transpose() const | |
| 551 | { | ||
| 552 | ✗ | return TransposeConst(*this); | |
| 553 | } | ||
| 554 | |||
| 555 | /* CRBA joint operators | ||
| 556 | * - ForceSet::Block = ForceSet | ||
| 557 | * - ForceSet operator* (Inertia Y,Constraint S) | ||
| 558 | * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) | ||
| 559 | * - SE3::act(ForceSet::Block) | ||
| 560 | */ | ||
| 561 | ✗ | DenseBase matrix_impl() const | |
| 562 | { | ||
| 563 | ✗ | DenseBase S; | |
| 564 | ✗ | MotionRef<DenseBase> v(S); | |
| 565 | ✗ | v << AxisAngular(); | |
| 566 | ✗ | S(LINEAR + axis) = m_pitch; | |
| 567 | ✗ | return S; | |
| 568 | } | ||
| 569 | |||
| 570 | template<typename MotionDerived> | ||
| 571 | typename MotionAlgebraAction<JointMotionSubspaceHelicalTpl, MotionDerived>::ReturnType | ||
| 572 | ✗ | motionAction(const MotionDense<MotionDerived> & m) const | |
| 573 | { | ||
| 574 | typedef typename MotionAlgebraAction<JointMotionSubspaceHelicalTpl, MotionDerived>::ReturnType | ||
| 575 | ReturnType; | ||
| 576 | ✗ | ReturnType res; | |
| 577 | // Linear | ||
| 578 | ✗ | CartesianAxis3Linear::cross(-m.linear(), res.template segment<3>(LINEAR)); | |
| 579 | ✗ | CartesianAxis3Linear::alphaCross(-m_pitch, m.angular(), res.template segment<3>(ANGULAR)); | |
| 580 | ✗ | res.template segment<3>(LINEAR) += res.template segment<3>(ANGULAR); | |
| 581 | |||
| 582 | // Angular | ||
| 583 | ✗ | CartesianAxis3Angular::cross(-m.angular(), res.template segment<3>(ANGULAR)); | |
| 584 | ✗ | return res; | |
| 585 | } | ||
| 586 | |||
| 587 | ✗ | bool isEqual(const JointMotionSubspaceHelicalTpl &) const | |
| 588 | { | ||
| 589 | ✗ | return true; | |
| 590 | } | ||
| 591 | |||
| 592 | ✗ | Scalar & h() | |
| 593 | { | ||
| 594 | ✗ | return m_pitch; | |
| 595 | } | ||
| 596 | ✗ | const Scalar & h() const | |
| 597 | { | ||
| 598 | ✗ | return m_pitch; | |
| 599 | } | ||
| 600 | |||
| 601 | protected: | ||
| 602 | Scalar m_pitch; | ||
| 603 | }; // struct JointMotionSubspaceHelicalTpl | ||
| 604 | |||
| 605 | template<typename _Scalar, int _Options, int _axis> | ||
| 606 | ✗ | Eigen::Matrix<_Scalar, 1, 1, _Options> operator*( | |
| 607 | const typename JointMotionSubspaceHelicalTpl<_Scalar, _Options, _axis>::TransposeConst & | ||
| 608 | S_transpose, | ||
| 609 | const JointMotionSubspaceHelicalTpl<_Scalar, _Options, _axis> & S) | ||
| 610 | { | ||
| 611 | ✗ | Eigen::Matrix<_Scalar, 1, 1, _Options> res; | |
| 612 | ✗ | res(0) = 1.0 + S_transpose.ref.h() * S.h(); | |
| 613 | ✗ | return res; | |
| 614 | } | ||
| 615 | |||
| 616 | template<typename _Scalar, int _Options, int _axis> | ||
| 617 | struct JointHelicalTpl | ||
| 618 | { | ||
| 619 | typedef _Scalar Scalar; | ||
| 620 | |||
| 621 | enum | ||
| 622 | { | ||
| 623 | Options = _Options, | ||
| 624 | axis = _axis | ||
| 625 | }; | ||
| 626 | }; | ||
| 627 | |||
| 628 | template<typename S1, int O1, typename S2, int O2, int axis> | ||
| 629 | struct MultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, axis>> | ||
| 630 | { | ||
| 631 | typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType; | ||
| 632 | }; | ||
| 633 | |||
| 634 | /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ | ||
| 635 | namespace impl | ||
| 636 | { | ||
| 637 | template<typename S1, int O1, typename S2, int O2> | ||
| 638 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, 0>> | ||
| 639 | { | ||
| 640 | typedef InertiaTpl<S1, O1> Inertia; | ||
| 641 | typedef JointMotionSubspaceHelicalTpl<S2, O2, 0> Constraint; | ||
| 642 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
| 643 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & constraint) | |
| 644 | { | ||
| 645 | ✗ | ReturnType res; | |
| 646 | ✗ | const S2 & m_pitch = constraint.h(); | |
| 647 | |||
| 648 | /* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */ | ||
| 649 | /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */ | ||
| 650 | ✗ | const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; | |
| 651 | ✗ | const typename Inertia::Symmetric3 & I = Y.inertia(); | |
| 652 | |||
| 653 | ✗ | res << m * m_pitch, -m * z, m * y, I(0, 0) + m * (y * y + z * z), | |
| 654 | ✗ | I(0, 1) - m * x * y + m * z * m_pitch, I(0, 2) - m * x * z - m * y * m_pitch; | |
| 655 | |||
| 656 | ✗ | return res; | |
| 657 | } | ||
| 658 | }; | ||
| 659 | |||
| 660 | template<typename S1, int O1, typename S2, int O2> | ||
| 661 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, 1>> | ||
| 662 | { | ||
| 663 | typedef InertiaTpl<S1, O1> Inertia; | ||
| 664 | typedef JointMotionSubspaceHelicalTpl<S2, O2, 1> Constraint; | ||
| 665 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
| 666 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & constraint) | |
| 667 | { | ||
| 668 | ✗ | ReturnType res; | |
| 669 | ✗ | const S2 & m_pitch = constraint.h(); | |
| 670 | |||
| 671 | /* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */ | ||
| 672 | /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */ | ||
| 673 | ✗ | const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; | |
| 674 | ✗ | const typename Inertia::Symmetric3 & I = Y.inertia(); | |
| 675 | |||
| 676 | ✗ | res << m * z, m * m_pitch, -m * x, I(1, 0) - m * x * y - m * z * m_pitch, | |
| 677 | ✗ | I(1, 1) + m * (x * x + z * z), I(1, 2) - m * y * z + m * x * m_pitch; | |
| 678 | |||
| 679 | ✗ | return res; | |
| 680 | } | ||
| 681 | }; | ||
| 682 | |||
| 683 | template<typename S1, int O1, typename S2, int O2> | ||
| 684 | struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalTpl<S2, O2, 2>> | ||
| 685 | { | ||
| 686 | typedef InertiaTpl<S1, O1> Inertia; | ||
| 687 | typedef JointMotionSubspaceHelicalTpl<S2, O2, 2> Constraint; | ||
| 688 | typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType; | ||
| 689 | ✗ | static inline ReturnType run(const Inertia & Y, const Constraint & constraint) | |
| 690 | { | ||
| 691 | ✗ | ReturnType res; | |
| 692 | ✗ | const S2 & m_pitch = constraint.h(); | |
| 693 | |||
| 694 | /* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */ | ||
| 695 | /* Y(:,2) = ( 0,0, 1, y , -x , 0) */ | ||
| 696 | ✗ | const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; | |
| 697 | ✗ | const typename Inertia::Symmetric3 & I = Y.inertia(); | |
| 698 | |||
| 699 | ✗ | res << -m * y, m * x, m * m_pitch, I(2, 0) - m * x * z + m * y * m_pitch, | |
| 700 | ✗ | I(2, 1) - m * y * z - m * x * m_pitch, I(2, 2) + m * (x * x + y * y); | |
| 701 | |||
| 702 | ✗ | return res; | |
| 703 | } | ||
| 704 | }; | ||
| 705 | } // namespace impl | ||
| 706 | |||
| 707 | template<typename M6Like, typename S2, int O2, int axis> | ||
| 708 | struct MultiplicationOp<Eigen::MatrixBase<M6Like>, JointMotionSubspaceHelicalTpl<S2, O2, axis>> | ||
| 709 | { | ||
| 710 | typedef Eigen::Matrix<S2, 6, 1> ReturnType; | ||
| 711 | }; | ||
| 712 | |||
| 713 | /* [ABA] operator* (Inertia Y,Constraint S) */ | ||
| 714 | namespace impl | ||
| 715 | { | ||
| 716 | template<typename M6Like, typename Scalar, int Options, int axis> | ||
| 717 | struct LhsMultiplicationOp< | ||
| 718 | Eigen::MatrixBase<M6Like>, | ||
| 719 | JointMotionSubspaceHelicalTpl<Scalar, Options, axis>> | ||
| 720 | { | ||
| 721 | typedef JointMotionSubspaceHelicalTpl<Scalar, Options, axis> Constraint; | ||
| 722 | typedef Eigen::Matrix<Scalar, 6, 1> ReturnType; | ||
| 723 | static inline ReturnType | ||
| 724 | ✗ | run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & constraint) | |
| 725 | { | ||
| 726 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6); | ||
| 727 | ✗ | return (Y.col(Inertia::ANGULAR + axis) + Y.col(Inertia::LINEAR + axis) * constraint.h()); | |
| 728 | } | ||
| 729 | }; | ||
| 730 | } // namespace impl | ||
| 731 | |||
| 732 | template<typename _Scalar, int _Options, int axis> | ||
| 733 | struct traits<JointHelicalTpl<_Scalar, _Options, axis>> | ||
| 734 | { | ||
| 735 | enum | ||
| 736 | { | ||
| 737 | NQ = 1, | ||
| 738 | NV = 1 | ||
| 739 | }; | ||
| 740 | typedef _Scalar Scalar; | ||
| 741 | enum | ||
| 742 | { | ||
| 743 | Options = _Options | ||
| 744 | }; | ||
| 745 | typedef JointDataHelicalTpl<Scalar, Options, axis> JointDataDerived; | ||
| 746 | typedef JointModelHelicalTpl<Scalar, Options, axis> JointModelDerived; | ||
| 747 | typedef JointMotionSubspaceHelicalTpl<Scalar, Options, axis> Constraint_t; | ||
| 748 | typedef TransformHelicalTpl<Scalar, Options, axis> Transformation_t; | ||
| 749 | typedef MotionHelicalTpl<Scalar, Options, axis> Motion_t; | ||
| 750 | typedef MotionZeroTpl<Scalar, Options> Bias_t; | ||
| 751 | |||
| 752 | // [ABA] | ||
| 753 | typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t; | ||
| 754 | typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t; | ||
| 755 | typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t; | ||
| 756 | |||
| 757 | typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t; | ||
| 758 | typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t; | ||
| 759 | |||
| 760 | PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE | ||
| 761 | }; | ||
| 762 | |||
| 763 | template<typename _Scalar, int _Options, int axis> | ||
| 764 | struct traits<JointDataHelicalTpl<_Scalar, _Options, axis>> | ||
| 765 | { | ||
| 766 | typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived; | ||
| 767 | typedef _Scalar Scalar; | ||
| 768 | }; | ||
| 769 | |||
| 770 | template<typename _Scalar, int _Options, int axis> | ||
| 771 | struct traits<JointModelHelicalTpl<_Scalar, _Options, axis>> | ||
| 772 | { | ||
| 773 | typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived; | ||
| 774 | typedef _Scalar Scalar; | ||
| 775 | }; | ||
| 776 | |||
| 777 | template<typename _Scalar, int _Options, int axis> | ||
| 778 | struct JointDataHelicalTpl : public JointDataBase<JointDataHelicalTpl<_Scalar, _Options, axis>> | ||
| 779 | { | ||
| 780 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 781 | typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived; | ||
| 782 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
| 783 | 4 | PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR | |
| 784 | |||
| 785 | ConfigVector_t joint_q; | ||
| 786 | TangentVector_t joint_v; | ||
| 787 | |||
| 788 | Constraint_t S; | ||
| 789 | Transformation_t M; | ||
| 790 | Motion_t v; | ||
| 791 | Bias_t c; | ||
| 792 | |||
| 793 | // [ABA] specific data | ||
| 794 | U_t U; | ||
| 795 | D_t Dinv; | ||
| 796 | UD_t UDinv; | ||
| 797 | D_t StU; | ||
| 798 | |||
| 799 | 136 | JointDataHelicalTpl() | |
| 800 |
1/2✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
|
136 | : joint_q(ConfigVector_t::Zero()) |
| 801 |
1/2✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
|
136 | , joint_v(TangentVector_t::Zero()) |
| 802 | 136 | , S((Scalar)0) | |
| 803 | 136 | , M((Scalar)0, (Scalar)1, (Scalar)0) | |
| 804 | 136 | , v((Scalar)0, (Scalar)0) | |
| 805 |
1/2✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
|
136 | , U(U_t::Zero()) |
| 806 |
1/2✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
|
136 | , Dinv(D_t::Zero()) |
| 807 |
1/2✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
|
136 | , UDinv(UD_t::Zero()) |
| 808 |
1/2✓ Branch 3 taken 68 times.
✗ Branch 4 not taken.
|
272 | , StU(D_t::Zero()) |
| 809 | { | ||
| 810 | 136 | } | |
| 811 | |||
| 812 | 240 | static std::string classname() | |
| 813 | { | ||
| 814 |
2/4✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 120 times.
✗ Branch 7 not taken.
|
480 | return std::string("JointDataH") + axisLabel<axis>(); |
| 815 | } | ||
| 816 | ✗ | std::string shortname() const | |
| 817 | { | ||
| 818 | ✗ | return classname(); | |
| 819 | } | ||
| 820 | |||
| 821 | }; // struct JointDataHelicalTpl | ||
| 822 | |||
| 823 | template<typename NewScalar, typename Scalar, int Options, int axis> | ||
| 824 | struct CastType<NewScalar, JointModelHelicalTpl<Scalar, Options, axis>> | ||
| 825 | { | ||
| 826 | typedef JointModelHelicalTpl<NewScalar, Options, axis> type; | ||
| 827 | }; | ||
| 828 | |||
| 829 | template<typename _Scalar, int _Options, int axis> | ||
| 830 | struct JointModelHelicalTpl : public JointModelBase<JointModelHelicalTpl<_Scalar, _Options, axis>> | ||
| 831 | { | ||
| 832 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 833 | typedef JointHelicalTpl<_Scalar, _Options, axis> JointDerived; | ||
| 834 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
| 835 | |||
| 836 | typedef JointModelBase<JointModelHelicalTpl> Base; | ||
| 837 | using Base::id; | ||
| 838 | using Base::idx_q; | ||
| 839 | using Base::idx_v; | ||
| 840 | using Base::setIndexes; | ||
| 841 | |||
| 842 | typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3; | ||
| 843 | |||
| 844 | 16 | JointDataDerived createData() const | |
| 845 | { | ||
| 846 | 16 | return JointDataDerived(); | |
| 847 | } | ||
| 848 | |||
| 849 | 120 | JointModelHelicalTpl() | |
| 850 | 120 | { | |
| 851 | 120 | } | |
| 852 | |||
| 853 | ✗ | explicit JointModelHelicalTpl(const Scalar & h) | |
| 854 | ✗ | : m_pitch(h) | |
| 855 | { | ||
| 856 | } | ||
| 857 | |||
| 858 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
| 859 | { | ||
| 860 | ✗ | return {true, true}; | |
| 861 | } | ||
| 862 | |||
| 863 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
| 864 | { | ||
| 865 | ✗ | return {true, true}; | |
| 866 | } | ||
| 867 | |||
| 868 | template<typename ConfigVector> | ||
| 869 | EIGEN_DONT_INLINE void | ||
| 870 | ✗ | calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const | |
| 871 | { | ||
| 872 | ✗ | data.joint_q[0] = qs[idx_q()]; | |
| 873 | Scalar ca, sa; | ||
| 874 | ✗ | SINCOS(data.joint_q[0], &sa, &ca); | |
| 875 | ✗ | data.M.setValues(sa, ca, data.joint_q[0] * m_pitch); | |
| 876 | ✗ | data.S.h() = m_pitch; | |
| 877 | } | ||
| 878 | |||
| 879 | template<typename TangentVector> | ||
| 880 | EIGEN_DONT_INLINE void | ||
| 881 | ✗ | calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs) | |
| 882 | const | ||
| 883 | { | ||
| 884 | ✗ | data.joint_v[0] = vs[idx_v()]; | |
| 885 | ✗ | data.v.angularRate() = data.joint_v[0]; | |
| 886 | ✗ | data.v.linearRate() = data.joint_v[0] * m_pitch; | |
| 887 | } | ||
| 888 | |||
| 889 | template<typename ConfigVector, typename TangentVector> | ||
| 890 | 12 | EIGEN_DONT_INLINE void calc( | |
| 891 | JointDataDerived & data, | ||
| 892 | const typename Eigen::MatrixBase<ConfigVector> & qs, | ||
| 893 | const typename Eigen::MatrixBase<TangentVector> & vs) const | ||
| 894 | { | ||
| 895 | 12 | calc(data, qs.derived()); | |
| 896 | |||
| 897 | 12 | data.joint_v[0] = vs[idx_v()]; | |
| 898 | 12 | data.v.angularRate() = data.joint_v[0]; | |
| 899 | 12 | data.v.linearRate() = data.joint_v[0] * m_pitch; | |
| 900 | } | ||
| 901 | |||
| 902 | template<typename VectorLike, typename Matrix6Like> | ||
| 903 | ✗ | void calc_aba( | |
| 904 | JointDataDerived & data, | ||
| 905 | const Eigen::MatrixBase<VectorLike> & armature, | ||
| 906 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
| 907 | const bool update_I) const | ||
| 908 | { | ||
| 909 | ✗ | data.U = I.col(Inertia::ANGULAR + axis) + m_pitch * I.col(Inertia::LINEAR + axis); | |
| 910 | ✗ | data.StU[0] = | |
| 911 | ✗ | data.U(Inertia::ANGULAR + axis) + m_pitch * data.U(Inertia::LINEAR + axis) + armature[0]; | |
| 912 | ✗ | data.Dinv[0] = Scalar(1) / data.StU[0]; | |
| 913 | ✗ | data.UDinv.noalias() = data.U * data.Dinv; | |
| 914 | |||
| 915 | ✗ | if (update_I) | |
| 916 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); | |
| 917 | } | ||
| 918 | |||
| 919 | 240 | static std::string classname() | |
| 920 | { | ||
| 921 |
2/4✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 120 times.
✗ Branch 7 not taken.
|
480 | return std::string("JointModelH") + axisLabel<axis>(); |
| 922 | } | ||
| 923 | ✗ | std::string shortname() const | |
| 924 | { | ||
| 925 | ✗ | return classname(); | |
| 926 | } | ||
| 927 | |||
| 928 | ✗ | Vector3 getMotionAxis() const | |
| 929 | { | ||
| 930 | switch (axis) | ||
| 931 | { | ||
| 932 | case 0: | ||
| 933 | ✗ | return Vector3::UnitX(); | |
| 934 | case 1: | ||
| 935 | ✗ | return Vector3::UnitY(); | |
| 936 | case 2: | ||
| 937 | ✗ | return Vector3::UnitZ(); | |
| 938 | default: | ||
| 939 | assert(false && "must never happen"); | ||
| 940 | break; | ||
| 941 | } | ||
| 942 | } | ||
| 943 | |||
| 944 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 945 | template<typename NewScalar> | ||
| 946 | ✗ | JointModelHelicalTpl<NewScalar, Options, axis> cast() const | |
| 947 | { | ||
| 948 | typedef JointModelHelicalTpl<NewScalar, Options, axis> ReturnType; | ||
| 949 | ✗ | ReturnType res(ScalarCast<NewScalar, Scalar>::cast(m_pitch)); | |
| 950 | ✗ | res.setIndexes(id(), idx_q(), idx_v()); | |
| 951 | ✗ | return res; | |
| 952 | } | ||
| 953 | |||
| 954 | Scalar m_pitch; | ||
| 955 | |||
| 956 | }; // struct JointModelHelicalTpl | ||
| 957 | |||
| 958 | typedef JointHelicalTpl<context::Scalar, context::Options, 0> JointHX; | ||
| 959 | typedef JointDataHelicalTpl<context::Scalar, context::Options, 0> JointDataHX; | ||
| 960 | typedef JointModelHelicalTpl<context::Scalar, context::Options, 0> JointModelHX; | ||
| 961 | |||
| 962 | typedef JointHelicalTpl<context::Scalar, context::Options, 1> JointHY; | ||
| 963 | typedef JointDataHelicalTpl<context::Scalar, context::Options, 1> JointDataHY; | ||
| 964 | typedef JointModelHelicalTpl<context::Scalar, context::Options, 1> JointModelHY; | ||
| 965 | |||
| 966 | typedef JointHelicalTpl<context::Scalar, context::Options, 2> JointHZ; | ||
| 967 | typedef JointDataHelicalTpl<context::Scalar, context::Options, 2> JointDataHZ; | ||
| 968 | typedef JointModelHelicalTpl<context::Scalar, context::Options, 2> JointModelHZ; | ||
| 969 | |||
| 970 | } // namespace pinocchio | ||
| 971 | |||
| 972 | #include <boost/type_traits.hpp> | ||
| 973 | |||
| 974 | namespace boost | ||
| 975 | { | ||
| 976 | template<typename Scalar, int Options, int axis> | ||
| 977 | struct has_nothrow_constructor<::pinocchio::JointModelHelicalTpl<Scalar, Options, axis>> | ||
| 978 | : public integral_constant<bool, true> | ||
| 979 | { | ||
| 980 | }; | ||
| 981 | |||
| 982 | template<typename Scalar, int Options, int axis> | ||
| 983 | struct has_nothrow_copy<::pinocchio::JointModelHelicalTpl<Scalar, Options, axis>> | ||
| 984 | : public integral_constant<bool, true> | ||
| 985 | { | ||
| 986 | }; | ||
| 987 | |||
| 988 | template<typename Scalar, int Options, int axis> | ||
| 989 | struct has_nothrow_constructor<::pinocchio::JointDataHelicalTpl<Scalar, Options, axis>> | ||
| 990 | : public integral_constant<bool, true> | ||
| 991 | { | ||
| 992 | }; | ||
| 993 | |||
| 994 | template<typename Scalar, int Options, int axis> | ||
| 995 | struct has_nothrow_copy<::pinocchio::JointDataHelicalTpl<Scalar, Options, axis>> | ||
| 996 | : public integral_constant<bool, true> | ||
| 997 | { | ||
| 998 | }; | ||
| 999 | } // namespace boost | ||
| 1000 | |||
| 1001 | #endif // ifndef __pinocchio_multibody_joint_helical_hpp__ | ||
| 1002 |