| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint/joint-generic.hpp |
| Date: | 2025-04-30 16:14:33 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 132 | 134 | 98.5% |
| Branches: | 15 | 26 | 57.7% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2016-2021 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_multibody_joint_generic_hpp__ | ||
| 6 | #define __pinocchio_multibody_joint_generic_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/multibody/joint/joint-collection.hpp" | ||
| 9 | #include "pinocchio/multibody/joint/joint-composite.hpp" | ||
| 10 | #include "pinocchio/multibody/joint/joint-basic-visitors.hxx" | ||
| 11 | #include "pinocchio/container/aligned-vector.hpp" | ||
| 12 | |||
| 13 | #include <boost/mpl/contains.hpp> | ||
| 14 | |||
| 15 | namespace pinocchio | ||
| 16 | { | ||
| 17 | |||
| 18 | template< | ||
| 19 | typename Scalar, | ||
| 20 | int Options = context::Options, | ||
| 21 | template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl> | ||
| 22 | struct JointTpl; | ||
| 23 | typedef JointTpl<context::Scalar> Joint; | ||
| 24 | |||
| 25 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
| 26 | struct traits<JointTpl<_Scalar, _Options, JointCollectionTpl>> | ||
| 27 | { | ||
| 28 | enum | ||
| 29 | { | ||
| 30 | Options = _Options, | ||
| 31 | NQ = Eigen::Dynamic, // Dynamic because unknown at compile time | ||
| 32 | NV = Eigen::Dynamic, | ||
| 33 | NVExtended = Eigen::Dynamic | ||
| 34 | }; | ||
| 35 | |||
| 36 | typedef _Scalar Scalar; | ||
| 37 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
| 38 | typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointDataDerived; | ||
| 39 | typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModelDerived; | ||
| 40 | |||
| 41 | typedef JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options> Constraint_t; | ||
| 42 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
| 43 | typedef MotionTpl<Scalar, Options> Motion_t; | ||
| 44 | typedef MotionTpl<Scalar, Options> Bias_t; | ||
| 45 | |||
| 46 | // [ABA] | ||
| 47 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> U_t; | ||
| 48 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> D_t; | ||
| 49 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> UD_t; | ||
| 50 | |||
| 51 | typedef Constraint_t ConstraintTypeConstRef; | ||
| 52 | typedef Constraint_t ConstraintTypeRef; | ||
| 53 | typedef Transformation_t TansformTypeConstRef; | ||
| 54 | typedef Transformation_t TansformTypeRef; | ||
| 55 | typedef Motion_t MotionTypeConstRef; | ||
| 56 | typedef Motion_t MotionTypeRef; | ||
| 57 | typedef Bias_t BiasTypeConstRef; | ||
| 58 | typedef Bias_t BiasTypeRef; | ||
| 59 | typedef U_t UTypeConstRef; | ||
| 60 | typedef U_t UTypeRef; | ||
| 61 | typedef D_t DTypeConstRef; | ||
| 62 | typedef D_t DTypeRef; | ||
| 63 | typedef UD_t UDTypeConstRef; | ||
| 64 | typedef UD_t UDTypeRef; | ||
| 65 | |||
| 66 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ConfigVector_t; | ||
| 67 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> TangentVector_t; | ||
| 68 | |||
| 69 | typedef ConfigVector_t ConfigVectorTypeConstRef; | ||
| 70 | typedef ConfigVector_t ConfigVectorTypeRef; | ||
| 71 | typedef TangentVector_t TangentVectorTypeConstRef; | ||
| 72 | typedef TangentVector_t TangentVectorTypeRef; | ||
| 73 | |||
| 74 | typedef boost::mpl::false_ is_mimicable_t; | ||
| 75 | }; | ||
| 76 | |||
| 77 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
| 78 | struct traits<JointDataTpl<_Scalar, _Options, JointCollectionTpl>> | ||
| 79 | { | ||
| 80 | typedef JointTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
| 81 | typedef typename traits<JointDerived>::Scalar Scalar; | ||
| 82 | }; | ||
| 83 | |||
| 84 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
| 85 | struct traits<JointModelTpl<_Scalar, _Options, JointCollectionTpl>> | ||
| 86 | { | ||
| 87 | typedef JointTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
| 88 | typedef typename traits<JointDerived>::Scalar Scalar; | ||
| 89 | }; | ||
| 90 | |||
| 91 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
| 92 | struct JointDataTpl | ||
| 93 | : public JointDataBase<JointDataTpl<_Scalar, _Options, JointCollectionTpl>> | ||
| 94 | , JointCollectionTpl<_Scalar, _Options>::JointDataVariant | ||
| 95 | { | ||
| 96 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 97 | |||
| 98 | typedef JointTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
| 99 | typedef JointDataBase<JointDataTpl> Base; | ||
| 100 | |||
| 101 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
| 102 | |||
| 103 | typedef JointCollectionTpl<_Scalar, _Options> JointCollection; | ||
| 104 | typedef typename JointCollection::JointDataVariant JointDataVariant; | ||
| 105 | |||
| 106 | using Base::operator==; | ||
| 107 | using Base::operator!=; | ||
| 108 | |||
| 109 | JointDataVariant & toVariant() | ||
| 110 | { | ||
| 111 | return *static_cast<JointDataVariant *>(this); | ||
| 112 | } | ||
| 113 | 678 | const JointDataVariant & toVariant() const | |
| 114 | { | ||
| 115 | 678 | return *static_cast<const JointDataVariant *>(this); | |
| 116 | } | ||
| 117 | |||
| 118 | 854 | ConfigVector_t joint_q() const | |
| 119 | { | ||
| 120 | 854 | return pinocchio::joint_q(*this); | |
| 121 | } | ||
| 122 | 678 | TangentVector_t joint_v() const | |
| 123 | { | ||
| 124 | 678 | return pinocchio::joint_v(*this); | |
| 125 | } | ||
| 126 | 2409 | Constraint_t S() const | |
| 127 | { | ||
| 128 | 2409 | return joint_motin_subspace_xd(*this); | |
| 129 | } | ||
| 130 | 2558 | Transformation_t M() const | |
| 131 | { | ||
| 132 | 2560 | return joint_transform(*this); | |
| 133 | } | ||
| 134 | 1959 | Motion_t v() const | |
| 135 | { | ||
| 136 | 1961 | return motion(*this); | |
| 137 | } | ||
| 138 | 984 | Bias_t c() const | |
| 139 | { | ||
| 140 | 984 | return bias(*this); | |
| 141 | } | ||
| 142 | |||
| 143 | // [ABA CCRBA] | ||
| 144 | 855 | U_t U() const | |
| 145 | { | ||
| 146 | 855 | return u_inertia(*this); | |
| 147 | } | ||
| 148 | 774 | D_t Dinv() const | |
| 149 | { | ||
| 150 | 774 | return dinv_inertia(*this); | |
| 151 | } | ||
| 152 | 774 | UD_t UDinv() const | |
| 153 | { | ||
| 154 | 774 | return udinv_inertia(*this); | |
| 155 | } | ||
| 156 | 83 | D_t StU() const | |
| 157 | { | ||
| 158 | 83 | return stu_inertia(*this); | |
| 159 | } | ||
| 160 | |||
| 161 | 5635 | JointDataTpl() | |
| 162 | 5635 | : JointDataVariant() | |
| 163 | { | ||
| 164 | 5635 | } | |
| 165 | |||
| 166 | ✗ | JointDataTpl(const JointDataVariant & jdata_variant) | |
| 167 | ✗ | : JointDataVariant(jdata_variant) | |
| 168 | { | ||
| 169 | } | ||
| 170 | |||
| 171 | template<typename JointDataDerived> | ||
| 172 | 132724 | JointDataTpl(const JointDataBase<JointDataDerived> & jdata) | |
| 173 | 132724 | : JointCollection::JointDataVariant((JointDataVariant)jdata.derived()) | |
| 174 | { | ||
| 175 | BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types, JointDataDerived>)); | ||
| 176 | 132724 | } | |
| 177 | |||
| 178 | // Define all the standard accessors | ||
| 179 | 854 | ConfigVector_t joint_q_accessor() const | |
| 180 | { | ||
| 181 | 854 | return joint_q(); | |
| 182 | } | ||
| 183 | 678 | TangentVector_t joint_v_accessor() const | |
| 184 | { | ||
| 185 | 678 | return joint_v(); | |
| 186 | } | ||
| 187 | 679 | Constraint_t S_accessor() const | |
| 188 | { | ||
| 189 | 679 | return S(); | |
| 190 | } | ||
| 191 | 684 | Transformation_t M_accessor() const | |
| 192 | { | ||
| 193 | 684 | return M(); | |
| 194 | } | ||
| 195 | 680 | Motion_t v_accessor() const | |
| 196 | { | ||
| 197 | 682 | return v(); | |
| 198 | } | ||
| 199 | 678 | Bias_t c_accessor() const | |
| 200 | { | ||
| 201 | 678 | return c(); | |
| 202 | } | ||
| 203 | 678 | U_t U_accessor() const | |
| 204 | { | ||
| 205 | 678 | return U(); | |
| 206 | } | ||
| 207 | 678 | D_t Dinv_accessor() const | |
| 208 | { | ||
| 209 | 678 | return Dinv(); | |
| 210 | } | ||
| 211 | 678 | UD_t UDinv_accessor() const | |
| 212 | { | ||
| 213 | 678 | return UDinv(); | |
| 214 | } | ||
| 215 | D_t StU_accessor() const | ||
| 216 | { | ||
| 217 | return StU(); | ||
| 218 | } | ||
| 219 | |||
| 220 | 1 | static std::string classname() | |
| 221 | { | ||
| 222 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | return "JointData"; |
| 223 | } | ||
| 224 | 28 | std::string shortname() const | |
| 225 | { | ||
| 226 | 28 | return ::pinocchio::shortname(*this); | |
| 227 | } | ||
| 228 | |||
| 229 | template<typename JointDataDerived> | ||
| 230 | 88 | bool isEqual(const JointDataBase<JointDataDerived> & other) const | |
| 231 | { | ||
| 232 | 88 | return ::pinocchio::isEqual(*this, other.derived()); | |
| 233 | } | ||
| 234 | |||
| 235 | 427 | bool isEqual(const JointDataTpl & other) const | |
| 236 | { | ||
| 237 |
3/4✓ Branch 1 taken 339 times.
✓ Branch 2 taken 88 times.
✓ Branch 6 taken 339 times.
✗ Branch 7 not taken.
|
427 | return Base::isEqual(other) && toVariant() == other.toVariant(); |
| 238 | } | ||
| 239 | |||
| 240 | 427 | bool operator==(const JointDataTpl & other) const | |
| 241 | { | ||
| 242 | 427 | return isEqual(other); | |
| 243 | } | ||
| 244 | |||
| 245 | 88 | bool operator!=(const JointDataTpl & other) const | |
| 246 | { | ||
| 247 | 88 | return !(*this == other); | |
| 248 | } | ||
| 249 | }; | ||
| 250 | |||
| 251 | template< | ||
| 252 | typename NewScalar, | ||
| 253 | typename Scalar, | ||
| 254 | int Options, | ||
| 255 | template<typename S, int O> class JointCollectionTpl> | ||
| 256 | struct CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>> | ||
| 257 | { | ||
| 258 | typedef JointModelTpl<NewScalar, Options, JointCollectionTpl> type; | ||
| 259 | }; | ||
| 260 | |||
| 261 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
| 262 | struct JointModelTpl | ||
| 263 | : JointModelBase<JointModelTpl<_Scalar, _Options, JointCollectionTpl>> | ||
| 264 | , JointCollectionTpl<_Scalar, _Options>::JointModelVariant | ||
| 265 | { | ||
| 266 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 267 | |||
| 268 | typedef JointTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
| 269 | |||
| 270 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
| 271 | PINOCCHIO_JOINT_USE_INDEXES(JointModelTpl); | ||
| 272 | |||
| 273 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
| 274 | typedef typename JointCollection::JointDataVariant JointDataVariant; | ||
| 275 | typedef typename JointCollection::JointModelVariant JointModelVariant; | ||
| 276 | |||
| 277 | using Base::id; | ||
| 278 | using Base::setIndexes; | ||
| 279 | using Base::operator==; | ||
| 280 | using Base::operator!=; | ||
| 281 | |||
| 282 | 2866 | JointModelTpl() | |
| 283 | 2866 | : JointModelVariant() | |
| 284 | { | ||
| 285 | 2866 | } | |
| 286 | |||
| 287 | JointModelTpl(const JointModelVariant & jmodel_variant) | ||
| 288 | : JointCollection::JointModelVariant(jmodel_variant) | ||
| 289 | { | ||
| 290 | } | ||
| 291 | |||
| 292 | 3 | const std::vector<bool> hasConfigurationLimit() const | |
| 293 | { | ||
| 294 | 3 | return ::pinocchio::hasConfigurationLimit(*this); | |
| 295 | } | ||
| 296 | |||
| 297 | 3 | const std::vector<bool> hasConfigurationLimitInTangent() const | |
| 298 | { | ||
| 299 | 3 | return ::pinocchio::hasConfigurationLimitInTangent(*this); | |
| 300 | } | ||
| 301 | |||
| 302 | template<typename JointModelDerived> | ||
| 303 | 33771 | JointModelTpl(const JointModelBase<JointModelDerived> & jmodel) | |
| 304 | 33771 | : JointModelVariant((JointModelVariant)jmodel.derived()) | |
| 305 | { | ||
| 306 | BOOST_MPL_ASSERT( | ||
| 307 | (boost::mpl::contains<typename JointModelVariant::types, JointModelDerived>)); | ||
| 308 | 33771 | } | |
| 309 | |||
| 310 | 1 | JointModelVariant & toVariant() | |
| 311 | { | ||
| 312 | 1 | return *static_cast<JointModelVariant *>(this); | |
| 313 | } | ||
| 314 | |||
| 315 | 6141 | const JointModelVariant & toVariant() const | |
| 316 | { | ||
| 317 | 6141 | return *static_cast<const JointModelVariant *>(this); | |
| 318 | } | ||
| 319 | |||
| 320 | 1714 | JointDataDerived createData() const | |
| 321 | { | ||
| 322 | 1714 | return ::pinocchio::createData<Scalar, Options, JointCollectionTpl>(*this); | |
| 323 | } | ||
| 324 | |||
| 325 | template<typename JointModelDerived> | ||
| 326 | 92 | bool isEqual(const JointModelBase<JointModelDerived> & other) const | |
| 327 | { | ||
| 328 | 92 | return ::pinocchio::isEqual(*this, other.derived()); | |
| 329 | } | ||
| 330 | |||
| 331 | template<typename JointModelDerived> | ||
| 332 | 3119 | bool hasSameIndexes(const JointModelBase<JointModelDerived> & other) const | |
| 333 | { | ||
| 334 | 3119 | return ::pinocchio::hasSameIndexes(*this, other.derived()); | |
| 335 | } | ||
| 336 | |||
| 337 | 3070 | bool isEqual(const JointModelTpl & other) const | |
| 338 | { | ||
| 339 |
3/4✓ Branch 1 taken 1918 times.
✓ Branch 2 taken 4 times.
✓ Branch 6 taken 1918 times.
✗ Branch 7 not taken.
|
3070 | return Base::isEqual(other) && toVariant() == other.toVariant(); |
| 340 | } | ||
| 341 | |||
| 342 | 3066 | bool operator==(const JointModelTpl & other) const | |
| 343 | { | ||
| 344 | 3066 | return isEqual(other); | |
| 345 | } | ||
| 346 | |||
| 347 | 3 | bool operator!=(const JointModelTpl & other) const | |
| 348 | { | ||
| 349 | 3 | return !(*this == other); | |
| 350 | } | ||
| 351 | |||
| 352 | template<typename ConfigVector> | ||
| 353 | 909 | void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVector> & q) const | |
| 354 | { | ||
| 355 | 909 | calc_zero_order(*this, data, q.derived()); | |
| 356 | 909 | } | |
| 357 | |||
| 358 | template<typename TangentVector> | ||
| 359 | void calc( | ||
| 360 | JointDataDerived & data, const Blank blank, const Eigen::MatrixBase<TangentVector> & v) const | ||
| 361 | { | ||
| 362 | calc_first_order(*this, data, blank, v.derived()); | ||
| 363 | } | ||
| 364 | |||
| 365 | template<typename ConfigVector, typename TangentVector> | ||
| 366 | 978 | void calc( | |
| 367 | JointDataDerived & data, | ||
| 368 | const Eigen::MatrixBase<ConfigVector> & q, | ||
| 369 | const Eigen::MatrixBase<TangentVector> & v) const | ||
| 370 | { | ||
| 371 | 978 | calc_first_order(*this, data, q.derived(), v.derived()); | |
| 372 | 978 | } | |
| 373 | |||
| 374 | template<typename VectorLike, typename Matrix6Like> | ||
| 375 | 44 | void calc_aba( | |
| 376 | JointDataDerived & data, | ||
| 377 | const Eigen::MatrixBase<VectorLike> & armature, | ||
| 378 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
| 379 | const bool update_I) const | ||
| 380 | { | ||
| 381 | 44 | ::pinocchio::calc_aba( | |
| 382 | 44 | *this, data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); | |
| 383 | 44 | } | |
| 384 | |||
| 385 | /* Acces to dedicated segment in robot config space. */ | ||
| 386 | // Const access | ||
| 387 | template<typename D> | ||
| 388 | typename SizeDepType<NV>::template SegmentReturn<D>::ConstType | ||
| 389 | 1025 | JointMappedConfigSelector_impl(const Eigen::MatrixBase<D> & a) const | |
| 390 | { | ||
| 391 | typedef const Eigen::MatrixBase<D> & InputType; | ||
| 392 | typedef typename SizeDepType<NV>::template SegmentReturn<D>::ConstType ReturnType; | ||
| 393 | typedef JointMappedConfigSelectorVisitor<InputType, ReturnType> Visitor; | ||
| 394 |
1/2✓ Branch 1 taken 1025 times.
✗ Branch 2 not taken.
|
1025 | typename Visitor::ArgsType arg(a); |
| 395 |
2/4✓ Branch 1 taken 1025 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1025 times.
✗ Branch 5 not taken.
|
1025 | return Visitor::run(*this, arg); |
| 396 | } | ||
| 397 | |||
| 398 | // Non-const access | ||
| 399 | template<typename D> | ||
| 400 | typename SizeDepType<NV>::template SegmentReturn<D>::Type | ||
| 401 | 799 | JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a) const | |
| 402 | { | ||
| 403 | typedef Eigen::MatrixBase<D> & InputType; | ||
| 404 | typedef typename SizeDepType<NV>::template SegmentReturn<D>::Type ReturnType; | ||
| 405 | typedef JointMappedConfigSelectorVisitor<InputType, ReturnType> Visitor; | ||
| 406 |
1/2✓ Branch 1 taken 799 times.
✗ Branch 2 not taken.
|
799 | typename Visitor::ArgsType arg(a); |
| 407 |
2/4✓ Branch 1 taken 799 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 799 times.
✗ Branch 5 not taken.
|
799 | return Visitor::run(*this, arg); |
| 408 | } | ||
| 409 | |||
| 410 | 83 | std::string shortname() const | |
| 411 | { | ||
| 412 | 83 | return ::pinocchio::shortname(*this); | |
| 413 | } | ||
| 414 | 1 | static std::string classname() | |
| 415 | { | ||
| 416 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | return "JointModel"; |
| 417 | } | ||
| 418 | |||
| 419 | 1748704 | int nq_impl() const | |
| 420 | { | ||
| 421 | 1748704 | return ::pinocchio::nq(*this); | |
| 422 | } | ||
| 423 | 10156434 | int nv_impl() const | |
| 424 | { | ||
| 425 | 10160165 | return ::pinocchio::nv(*this); | |
| 426 | } | ||
| 427 | 3399433 | int nvExtended_impl() const | |
| 428 | { | ||
| 429 | 3401237 | return ::pinocchio::nvExtended(*this); | |
| 430 | } | ||
| 431 | |||
| 432 | 1685399 | int idx_q_impl() const | |
| 433 | { | ||
| 434 | 1685399 | return ::pinocchio::idx_q(*this); | |
| 435 | } | ||
| 436 | 1975497 | int idx_v_impl() const | |
| 437 | { | ||
| 438 | 1977966 | return ::pinocchio::idx_v(*this); | |
| 439 | } | ||
| 440 | 6626819 | int idx_vExtended_impl() const | |
| 441 | { | ||
| 442 | 6630907 | return ::pinocchio::idx_vExtended(*this); | |
| 443 | } | ||
| 444 | |||
| 445 | 5211 | JointIndex id_impl() const | |
| 446 | { | ||
| 447 | 5211 | return ::pinocchio::id(*this); | |
| 448 | } | ||
| 449 | |||
| 450 | 8 | void setIndexes(JointIndex id, int nq, int nv) | |
| 451 | { | ||
| 452 | 8 | ::pinocchio::setIndexes(*this, id, nq, nv, nv); | |
| 453 | 8 | } | |
| 454 | |||
| 455 | 17720 | void setIndexes(JointIndex id, int nq, int nv, int nvExtended) | |
| 456 | { | ||
| 457 | 17720 | ::pinocchio::setIndexes(*this, id, nq, nv, nvExtended); | |
| 458 | 17720 | } | |
| 459 | |||
| 460 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
| 461 | template<typename NewScalar> | ||
| 462 | 757 | JointModelTpl<NewScalar, Options, JointCollectionTpl> cast() const | |
| 463 | { | ||
| 464 | 757 | return cast_joint<NewScalar, Scalar, Options, JointCollectionTpl>(*this); | |
| 465 | } | ||
| 466 | }; | ||
| 467 | |||
| 468 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector; | ||
| 469 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector; | ||
| 470 | |||
| 471 | template< | ||
| 472 | typename Scalar, | ||
| 473 | int Options, | ||
| 474 | template<typename S, int O> class JointCollectionTpl, | ||
| 475 | typename JointDataDerived> | ||
| 476 | 44 | bool operator==( | |
| 477 | const JointDataBase<JointDataDerived> & joint_data, | ||
| 478 | const JointDataTpl<Scalar, Options, JointCollectionTpl> & joint_data_generic) | ||
| 479 | { | ||
| 480 | 44 | return joint_data_generic == joint_data.derived(); | |
| 481 | } | ||
| 482 | |||
| 483 | template< | ||
| 484 | typename Scalar, | ||
| 485 | int Options, | ||
| 486 | template<typename S, int O> class JointCollectionTpl, | ||
| 487 | typename JointDataDerived> | ||
| 488 | 44 | bool operator!=( | |
| 489 | const JointDataBase<JointDataDerived> & joint_data, | ||
| 490 | const JointDataTpl<Scalar, Options, JointCollectionTpl> & joint_data_generic) | ||
| 491 | { | ||
| 492 |
1/2✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
|
44 | return joint_data_generic != joint_data.derived(); |
| 493 | } | ||
| 494 | |||
| 495 | template< | ||
| 496 | typename Scalar, | ||
| 497 | int Options, | ||
| 498 | template<typename S, int O> class JointCollectionTpl, | ||
| 499 | typename JointModelDerived> | ||
| 500 | 23 | bool operator==( | |
| 501 | const JointModelBase<JointModelDerived> & joint_model, | ||
| 502 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & joint_model_generic) | ||
| 503 | { | ||
| 504 | 23 | return joint_model_generic == joint_model.derived(); | |
| 505 | } | ||
| 506 | |||
| 507 | template< | ||
| 508 | typename Scalar, | ||
| 509 | int Options, | ||
| 510 | template<typename S, int O> class JointCollectionTpl, | ||
| 511 | typename JointModelDerived> | ||
| 512 | bool operator!=( | ||
| 513 | const JointModelBase<JointModelDerived> & joint_model, | ||
| 514 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & joint_model_generic) | ||
| 515 | { | ||
| 516 | return joint_model_generic != joint_model.derived(); | ||
| 517 | } | ||
| 518 | |||
| 519 | } // namespace pinocchio | ||
| 520 | |||
| 521 | #endif // ifndef __pinocchio_multibody_joint_generic_hpp__ | ||
| 522 |