| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint/joint-model-base.hpp |
| Date: | 2025-04-30 16:14:33 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 167 | 167 | 100.0% |
| Branches: | 47 | 92 | 51.1% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2019 CNRS INRIA | ||
| 3 | // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
| 4 | // | ||
| 5 | |||
| 6 | #ifndef __pinocchio_multibody_joint_model_base_hpp__ | ||
| 7 | #define __pinocchio_multibody_joint_model_base_hpp__ | ||
| 8 | |||
| 9 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
| 10 | #include "pinocchio/multibody/joint/joint-common-operations.hpp" | ||
| 11 | |||
| 12 | #include "pinocchio/math/matrix-block.hpp" | ||
| 13 | |||
| 14 | #include <limits> | ||
| 15 | |||
| 16 | #define PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, TYPENAME) \ | ||
| 17 | typedef Eigen::DenseIndex Index; \ | ||
| 18 | typedef TYPENAME traits<Joint>::Scalar Scalar; \ | ||
| 19 | typedef TYPENAME traits<Joint>::JointDataDerived JointDataDerived; \ | ||
| 20 | typedef TYPENAME traits<Joint>::JointModelDerived JointModelDerived; \ | ||
| 21 | typedef TYPENAME traits<Joint>::Constraint_t Constraint_t; \ | ||
| 22 | typedef TYPENAME traits<Joint>::Transformation_t Transformation_t; \ | ||
| 23 | typedef TYPENAME traits<Joint>::Motion_t Motion_t; \ | ||
| 24 | typedef TYPENAME traits<Joint>::Bias_t Bias_t; \ | ||
| 25 | typedef TYPENAME traits<Joint>::U_t U_t; \ | ||
| 26 | typedef TYPENAME traits<Joint>::D_t D_t; \ | ||
| 27 | typedef TYPENAME traits<Joint>::UD_t UD_t; \ | ||
| 28 | typedef TYPENAME traits<Joint>::is_mimicable_t is_mimicable_t; \ | ||
| 29 | enum \ | ||
| 30 | { \ | ||
| 31 | Options = traits<Joint>::Options, \ | ||
| 32 | NQ = traits<Joint>::NQ, \ | ||
| 33 | NV = traits<Joint>::NV, \ | ||
| 34 | NVExtended = traits<Joint>::NVExtended \ | ||
| 35 | }; \ | ||
| 36 | typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t; \ | ||
| 37 | typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t | ||
| 38 | |||
| 39 | #ifdef __clang__ | ||
| 40 | |||
| 41 | #define PINOCCHIO_JOINT_TYPEDEF(Joint) \ | ||
| 42 | PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG) | ||
| 43 | #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \ | ||
| 44 | PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename) | ||
| 45 | |||
| 46 | #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2) | ||
| 47 | |||
| 48 | #define PINOCCHIO_JOINT_TYPEDEF(Joint) \ | ||
| 49 | PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG) | ||
| 50 | #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \ | ||
| 51 | PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename) | ||
| 52 | |||
| 53 | #else | ||
| 54 | |||
| 55 | #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename) | ||
| 56 | #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \ | ||
| 57 | PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename) | ||
| 58 | |||
| 59 | #endif | ||
| 60 | |||
| 61 | #define PINOCCHIO_JOINT_USE_INDEXES(Joint) \ | ||
| 62 | typedef JointModelBase<Joint> Base; \ | ||
| 63 | using Base::idx_q; \ | ||
| 64 | using Base::idx_v; \ | ||
| 65 | using Base::idx_vExtended | ||
| 66 | |||
| 67 | #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \ | ||
| 68 | template<typename Scalar, int Options, typename NewScalar> \ | ||
| 69 | struct CastType<NewScalar, JointModelTpl<Scalar, Options>> \ | ||
| 70 | { \ | ||
| 71 | typedef JointModelTpl<NewScalar, Options> type; \ | ||
| 72 | } | ||
| 73 | |||
| 74 | namespace pinocchio | ||
| 75 | { | ||
| 76 | |||
| 77 | template<typename Derived> | ||
| 78 | struct JointModelBase : NumericalBase<Derived> | ||
| 79 | { | ||
| 80 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 81 | |||
| 82 | typedef typename traits<Derived>::JointDerived JointDerived; | ||
| 83 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
| 84 | |||
| 85 | 41903 | JointModelDerived & derived() | |
| 86 | { | ||
| 87 | 41903 | return *static_cast<Derived *>(this); | |
| 88 | } | ||
| 89 | 117427365 | const JointModelDerived & derived() const | |
| 90 | { | ||
| 91 | 117894831 | return *static_cast<const Derived *>(this); | |
| 92 | } | ||
| 93 | |||
| 94 | 129354 | JointDataDerived createData() const | |
| 95 | { | ||
| 96 | 129354 | return derived().createData(); | |
| 97 | } | ||
| 98 | |||
| 99 | 6 | const std::vector<bool> hasConfigurationLimit() const | |
| 100 | { | ||
| 101 | 6 | return derived().hasConfigurationLimit(); | |
| 102 | } | ||
| 103 | |||
| 104 | 6 | const std::vector<bool> hasConfigurationLimitInTangent() const | |
| 105 | { | ||
| 106 | 6 | return derived().hasConfigurationLimitInTangent(); | |
| 107 | } | ||
| 108 | |||
| 109 | template<typename ConfigVectorType> | ||
| 110 | 1128044 | void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const | |
| 111 | { | ||
| 112 | 1128044 | derived().calc(data, qs.derived()); | |
| 113 | } | ||
| 114 | |||
| 115 | template<typename TangentVectorType> | ||
| 116 | 44 | void calc( | |
| 117 | JointDataDerived & data, | ||
| 118 | const Blank not_used, | ||
| 119 | const Eigen::MatrixBase<TangentVectorType> & vs) const | ||
| 120 | { | ||
| 121 | 44 | derived().calc(data, not_used, vs.derived()); | |
| 122 | 44 | } | |
| 123 | |||
| 124 | template<typename ConfigVectorType, typename TangentVectorType> | ||
| 125 | 261464 | void calc( | |
| 126 | JointDataDerived & data, | ||
| 127 | const Eigen::MatrixBase<ConfigVectorType> & qs, | ||
| 128 | const Eigen::MatrixBase<TangentVectorType> & vs) const | ||
| 129 | { | ||
| 130 | 261464 | derived().calc(data, qs.derived(), vs.derived()); | |
| 131 | } | ||
| 132 | |||
| 133 | template<typename VectorLike, typename Matrix6Like> | ||
| 134 | 12248 | void calc_aba( | |
| 135 | JointDataDerived & data, | ||
| 136 | const Eigen::MatrixBase<VectorLike> & armature, | ||
| 137 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
| 138 | const bool update_I = false) const | ||
| 139 | { | ||
| 140 | 24496 | derived().calc_aba( | |
| 141 | 12248 | data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); | |
| 142 | 12248 | } | |
| 143 | |||
| 144 | 42350278 | int nv() const | |
| 145 | { | ||
| 146 | 42350278 | return derived().nv_impl(); | |
| 147 | } | ||
| 148 | 4101584 | int nq() const | |
| 149 | { | ||
| 150 | 8203164 | return derived().nq_impl(); | |
| 151 | } | ||
| 152 | 6895100 | int nvExtended() const | |
| 153 | { | ||
| 154 | 13790200 | return derived().nvExtended_impl(); | |
| 155 | } | ||
| 156 | |||
| 157 | // Default _impl methods are reimplemented by dynamic-size joints. | ||
| 158 | 21970726 | int nv_impl() const | |
| 159 | { | ||
| 160 | 21970726 | return NV; | |
| 161 | } | ||
| 162 | 2343058 | int nq_impl() const | |
| 163 | { | ||
| 164 | 4686112 | return NQ; | |
| 165 | } | ||
| 166 | 3467091 | int nvExtended_impl() const | |
| 167 | { | ||
| 168 | 6934182 | return NVExtended; | |
| 169 | } | ||
| 170 | |||
| 171 | 9179086 | int idx_q() const | |
| 172 | { | ||
| 173 | 9179086 | return derived().idx_q_impl(); | |
| 174 | } | ||
| 175 | 5040656 | int idx_v() const | |
| 176 | { | ||
| 177 | 10081312 | return derived().idx_v_impl(); | |
| 178 | } | ||
| 179 | 13294104 | int idx_vExtended() const | |
| 180 | { | ||
| 181 | 26588192 | return derived().idx_vExtended_impl(); | |
| 182 | } | ||
| 183 | 842190 | JointIndex id() const | |
| 184 | { | ||
| 185 | 1684380 | return derived().id_impl(); | |
| 186 | } | ||
| 187 | |||
| 188 | 5812936 | int idx_q_impl() const | |
| 189 | { | ||
| 190 | 5812936 | return i_q; | |
| 191 | } | ||
| 192 | 3065159 | int idx_v_impl() const | |
| 193 | { | ||
| 194 | 6130318 | return i_v; | |
| 195 | } | ||
| 196 | 304537 | int idx_vExtended_impl() const | |
| 197 | { | ||
| 198 | 6971806 | return i_vExtended; | |
| 199 | } | ||
| 200 | 838201 | JointIndex id_impl() const | |
| 201 | { | ||
| 202 | 1676402 | return i_id; | |
| 203 | } | ||
| 204 | |||
| 205 | 1361 | void setIndexes(JointIndex id, int q, int v) | |
| 206 | { | ||
| 207 | 1361 | derived().setIndexes_impl(id, q, v, v); | |
| 208 | 1361 | } | |
| 209 | |||
| 210 | 38830 | void setIndexes(JointIndex id, int q, int v, int vExtended) | |
| 211 | { | ||
| 212 | 38830 | derived().setIndexes_impl(id, q, v, vExtended); | |
| 213 | 38830 | } | |
| 214 | |||
| 215 | 39893 | void setIndexes_impl(JointIndex id, int q, int v, int vExtended) | |
| 216 | { | ||
| 217 | 39893 | i_id = id, i_q = q; | |
| 218 | 39893 | i_v = v; | |
| 219 | 39893 | i_vExtended = vExtended; | |
| 220 | 39893 | } | |
| 221 | |||
| 222 | 76 | void disp(std::ostream & os) const | |
| 223 | { | ||
| 224 | using namespace std; | ||
| 225 |
2/4✓ Branch 2 taken 38 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38 times.
✗ Branch 6 not taken.
|
152 | os << shortname() << endl |
| 226 |
4/8✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38 times.
✗ Branch 11 not taken.
|
76 | << " index: " << id() << endl |
| 227 |
4/8✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38 times.
✗ Branch 11 not taken.
|
76 | << " index q: " << idx_q() << endl |
| 228 |
4/8✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38 times.
✗ Branch 11 not taken.
|
76 | << " index v: " << idx_v() << endl |
| 229 |
4/8✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38 times.
✗ Branch 11 not taken.
|
76 | << " index vExtended: " << idx_vExtended() << endl |
| 230 |
4/8✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38 times.
✗ Branch 11 not taken.
|
76 | << " nq: " << nq() << endl |
| 231 |
4/8✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38 times.
✗ Branch 11 not taken.
|
76 | << " nv: " << nv() << endl |
| 232 |
4/8✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38 times.
✗ Branch 11 not taken.
|
76 | << " nvExtended: " << nvExtended() << endl; |
| 233 | 76 | } | |
| 234 | |||
| 235 | 96 | friend std::ostream & operator<<(std::ostream & os, const JointModelBase<Derived> & jmodel) | |
| 236 | { | ||
| 237 | 96 | jmodel.derived().disp(os); | |
| 238 | 96 | return os; | |
| 239 | } | ||
| 240 | |||
| 241 | 1434 | std::string shortname() const | |
| 242 | { | ||
| 243 | 1434 | return derived().shortname(); | |
| 244 | } | ||
| 245 | 25 | static std::string classname() | |
| 246 | { | ||
| 247 | 50 | return Derived::classname(); | |
| 248 | } | ||
| 249 | |||
| 250 | template<typename NewScalar> | ||
| 251 | 1674 | typename CastType<NewScalar, Derived>::type cast() const | |
| 252 | { | ||
| 253 | 1674 | return derived().template cast<NewScalar>(); | |
| 254 | } | ||
| 255 | |||
| 256 | template<class OtherDerived> | ||
| 257 | 4638 | bool operator==(const JointModelBase<OtherDerived> & other) const | |
| 258 | { | ||
| 259 | 4638 | return derived().isEqual(other.derived()); | |
| 260 | } | ||
| 261 | |||
| 262 | template<class OtherDerived> | ||
| 263 | 51 | bool operator!=(const JointModelBase<OtherDerived> & other) const | |
| 264 | { | ||
| 265 | 51 | return !(internal::comparison_eq(derived(), other.derived())); | |
| 266 | } | ||
| 267 | |||
| 268 | template<class OtherDerived> | ||
| 269 | 4 | bool isEqual(const JointModelBase<OtherDerived> &) const | |
| 270 | { | ||
| 271 | 4 | return false; | |
| 272 | } | ||
| 273 | |||
| 274 | 8478 | bool isEqual(const JointModelBase<Derived> & other) const | |
| 275 | { | ||
| 276 | 8478 | return derived().hasSameIndexes(other.derived()); | |
| 277 | } | ||
| 278 | |||
| 279 | template<class OtherDerived> | ||
| 280 | 8522 | bool hasSameIndexes(const JointModelBase<OtherDerived> & other) const | |
| 281 | { | ||
| 282 |
3/6✓ Branch 1 taken 4261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4261 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4261 times.
✗ Branch 8 not taken.
|
8522 | return internal::comparison_eq(other.id(), id()) |
| 283 |
4/8✓ Branch 1 taken 4209 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4209 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4209 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 4209 times.
✗ Branch 10 not taken.
|
8418 | && internal::comparison_eq(other.idx_q(), idx_q()) |
| 284 |
4/8✓ Branch 1 taken 4209 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4209 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4209 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 4209 times.
✗ Branch 10 not taken.
|
8418 | && internal::comparison_eq(other.idx_v(), idx_v()) |
| 285 |
6/10✓ Branch 0 taken 4209 times.
✓ Branch 1 taken 52 times.
✓ Branch 3 taken 4209 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4209 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4209 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 4209 times.
✗ Branch 12 not taken.
|
16940 | && internal::comparison_eq(other.idx_vExtended(), idx_vExtended()); |
| 286 | } | ||
| 287 | |||
| 288 | /* Acces to dedicated segment in robot config space. */ | ||
| 289 | // Const access | ||
| 290 | template<typename D> | ||
| 291 | typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType | ||
| 292 | 4136 | JointMappedConfigSelector(const Eigen::MatrixBase<D> & a) const | |
| 293 | { | ||
| 294 | 4136 | return derived().JointMappedConfigSelector_impl(a); | |
| 295 | } | ||
| 296 | |||
| 297 | template<typename D> | ||
| 298 | typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType | ||
| 299 | 2002 | JointMappedConfigSelector_impl(const Eigen::MatrixBase<D> & a) const | |
| 300 | { | ||
| 301 | 2002 | return SizeDepType<NQ>::segment(a.derived(), idx_q(), nq()); | |
| 302 | } | ||
| 303 | |||
| 304 | // Non-const access | ||
| 305 | template<typename D> | ||
| 306 | typename SizeDepType<NQ>::template SegmentReturn<D>::Type | ||
| 307 | 3232 | JointMappedConfigSelector(Eigen::MatrixBase<D> & a) const | |
| 308 | { | ||
| 309 | 3232 | return derived().JointMappedConfigSelector_impl(a); | |
| 310 | } | ||
| 311 | |||
| 312 | template<typename D> | ||
| 313 | typename SizeDepType<NQ>::template SegmentReturn<D>::Type | ||
| 314 | 1630 | JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a) const | |
| 315 | { | ||
| 316 | 1630 | return SizeDepType<NQ>::segment(a, idx_q(), nq()); | |
| 317 | } | ||
| 318 | |||
| 319 | // Const access | ||
| 320 | template<typename D> | ||
| 321 | typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType | ||
| 322 | 525588 | jointConfigSelector(const Eigen::MatrixBase<D> & a) const | |
| 323 | { | ||
| 324 | 525588 | return derived().jointConfigSelector_impl(a); | |
| 325 | } | ||
| 326 | |||
| 327 | template<typename D> | ||
| 328 | typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType | ||
| 329 | 525588 | jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const | |
| 330 | { | ||
| 331 | 525588 | return SizeDepType<NQ>::segment(a.derived(), idx_q(), nq()); | |
| 332 | } | ||
| 333 | |||
| 334 | // Non-const access | ||
| 335 | template<typename D> | ||
| 336 | typename SizeDepType<NQ>::template SegmentReturn<D>::Type | ||
| 337 | 373938 | jointConfigSelector(Eigen::MatrixBase<D> & a) const | |
| 338 | { | ||
| 339 | 373938 | return derived().jointConfigSelector_impl(a); | |
| 340 | } | ||
| 341 | |||
| 342 | template<typename D> | ||
| 343 | typename SizeDepType<NQ>::template SegmentReturn<D>::Type | ||
| 344 | 373938 | jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const | |
| 345 | { | ||
| 346 | 373938 | return SizeDepType<NQ>::segment(a, idx_q(), nq()); | |
| 347 | } | ||
| 348 | |||
| 349 | /* Acces to dedicated segment in robot config velocity space. */ | ||
| 350 | // Const access | ||
| 351 | template<typename D> | ||
| 352 | typename SizeDepType<NV>::template SegmentReturn<D>::ConstType | ||
| 353 | 85112 | JointMappedVelocitySelector(const Eigen::MatrixBase<D> & a) const | |
| 354 | { | ||
| 355 | 85112 | return derived().JointMappedVelocitySelector_impl(a.derived()); | |
| 356 | } | ||
| 357 | |||
| 358 | template<typename D> | ||
| 359 | typename SizeDepType<NV>::template SegmentReturn<D>::ConstType | ||
| 360 | 84536 | JointMappedVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const | |
| 361 | { | ||
| 362 | 84536 | return SizeDepType<NV>::segment(a.derived(), idx_v(), nvExtended()); | |
| 363 | } | ||
| 364 | |||
| 365 | // Non-const access | ||
| 366 | template<typename D> | ||
| 367 | typename SizeDepType<NV>::template SegmentReturn<D>::Type | ||
| 368 | 38800 | JointMappedVelocitySelector(Eigen::MatrixBase<D> & a) const | |
| 369 | { | ||
| 370 | 38800 | return derived().JointMappedVelocitySelector_impl(a.derived()); | |
| 371 | } | ||
| 372 | |||
| 373 | template<typename D> | ||
| 374 | typename SizeDepType<NV>::template SegmentReturn<D>::Type | ||
| 375 | 38198 | JointMappedVelocitySelector_impl(Eigen::MatrixBase<D> & a) const | |
| 376 | { | ||
| 377 | 38198 | return SizeDepType<NV>::segment(a.derived(), idx_v(), nvExtended()); | |
| 378 | } | ||
| 379 | |||
| 380 | // Const access | ||
| 381 | template<typename D> | ||
| 382 | typename SizeDepType<NV>::template SegmentReturn<D>::ConstType | ||
| 383 | 232285 | jointVelocitySelector(const Eigen::MatrixBase<D> & a) const | |
| 384 | { | ||
| 385 | 232285 | return derived().jointVelocitySelector_impl(a.derived()); | |
| 386 | } | ||
| 387 | |||
| 388 | template<typename D> | ||
| 389 | typename SizeDepType<NV>::template SegmentReturn<D>::ConstType | ||
| 390 | 232277 | jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const | |
| 391 | { | ||
| 392 | 232277 | return SizeDepType<NV>::segment(a.derived(), idx_v(), nv()); | |
| 393 | } | ||
| 394 | |||
| 395 | // Non-const access | ||
| 396 | template<typename D> | ||
| 397 | typename SizeDepType<NV>::template SegmentReturn<D>::Type | ||
| 398 | 307955 | jointVelocitySelector(Eigen::MatrixBase<D> & a) const | |
| 399 | { | ||
| 400 | 307955 | return derived().jointVelocitySelector_impl(a.derived()); | |
| 401 | } | ||
| 402 | |||
| 403 | template<typename D> | ||
| 404 | typename SizeDepType<NV>::template SegmentReturn<D>::Type | ||
| 405 | 307935 | jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const | |
| 406 | { | ||
| 407 | 307935 | return SizeDepType<NV>::segment(a.derived(), idx_v(), nv()); | |
| 408 | } | ||
| 409 | |||
| 410 | /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ | ||
| 411 | // Const access | ||
| 412 | template<typename D> | ||
| 413 | typename SizeDepType<NV>::template ColsReturn<D>::ConstType | ||
| 414 | 2422 | jointCols(const Eigen::MatrixBase<D> & A) const | |
| 415 | { | ||
| 416 | 2422 | return derived().jointCols_impl(A.derived()); | |
| 417 | } | ||
| 418 | |||
| 419 | template<typename D> | ||
| 420 | typename SizeDepType<NVExtended>::template ColsReturn<D>::ConstType | ||
| 421 | 36 | jointExtendedModelCols(const Eigen::MatrixBase<D> & A) const | |
| 422 | { | ||
| 423 | 36 | return derived().jointExtendedModelCols_impl(A.derived()); | |
| 424 | } | ||
| 425 | |||
| 426 | template<typename D> | ||
| 427 | typename SizeDepType<NV>::template ColsReturn<D>::ConstType | ||
| 428 | 2422 | jointCols_impl(const Eigen::MatrixBase<D> & A) const | |
| 429 | { | ||
| 430 | 2422 | return SizeDepType<NV>::middleCols(A.derived(), idx_v(), nv()); | |
| 431 | } | ||
| 432 | |||
| 433 | template<typename D> | ||
| 434 | typename SizeDepType<NVExtended>::template ColsReturn<D>::ConstType | ||
| 435 | 36 | jointExtendedModelCols_impl(const Eigen::MatrixBase<D> & A) const | |
| 436 | { | ||
| 437 | 36 | return SizeDepType<NVExtended>::middleCols(A.derived(), idx_vExtended(), nvExtended()); | |
| 438 | } | ||
| 439 | |||
| 440 | // Non-const access | ||
| 441 | // TODO rename Jac/Vel into Full/Red (for full system and reduced system ?) | ||
| 442 | template<typename D> | ||
| 443 | 587042 | typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A) const | |
| 444 | { | ||
| 445 | 587042 | return derived().jointCols_impl(A.derived()); | |
| 446 | } | ||
| 447 | |||
| 448 | template<typename D> | ||
| 449 | typename SizeDepType<NVExtended>::template ColsReturn<D>::Type | ||
| 450 | 64720 | jointExtendedModelCols(Eigen::MatrixBase<D> & A) const | |
| 451 | { | ||
| 452 | 64720 | return derived().jointExtendedModelCols_impl(A.derived()); | |
| 453 | } | ||
| 454 | |||
| 455 | template<typename D> | ||
| 456 | typename SizeDepType<NV>::template ColsReturn<D>::Type | ||
| 457 | 309780 | jointCols_impl(Eigen::MatrixBase<D> & A) const | |
| 458 | { | ||
| 459 | 309780 | return SizeDepType<NV>::middleCols(A.derived(), idx_v(), nv()); | |
| 460 | } | ||
| 461 | |||
| 462 | template<typename D> | ||
| 463 | typename SizeDepType<NVExtended>::template ColsReturn<D>::Type | ||
| 464 | 64600 | jointExtendedModelCols_impl(Eigen::MatrixBase<D> & A) const | |
| 465 | { | ||
| 466 | 64600 | return SizeDepType<NVExtended>::middleCols(A.derived(), idx_vExtended(), nvExtended()); | |
| 467 | } | ||
| 468 | |||
| 469 | /* Acces to dedicated rows in a matrix.*/ | ||
| 470 | // Const access | ||
| 471 | template<typename D> | ||
| 472 | typename SizeDepType<NV>::template RowsReturn<D>::ConstType | ||
| 473 | 152 | jointRows(const Eigen::MatrixBase<D> & A) const | |
| 474 | { | ||
| 475 | 152 | return derived().jointRows_impl(A.derived()); | |
| 476 | } | ||
| 477 | |||
| 478 | template<typename D> | ||
| 479 | typename SizeDepType<NVExtended>::template RowsReturn<D>::ConstType | ||
| 480 | jointExtendedModelRows(const Eigen::MatrixBase<D> & A) const | ||
| 481 | { | ||
| 482 | return derived().jointExtendedModelRows_impl(A.derived()); | ||
| 483 | } | ||
| 484 | |||
| 485 | template<typename D> | ||
| 486 | typename SizeDepType<NV>::template RowsReturn<D>::ConstType | ||
| 487 | 152 | jointRows_impl(const Eigen::MatrixBase<D> & A) const | |
| 488 | { | ||
| 489 | 152 | return SizeDepType<NV>::middleRows(A.derived(), idx_v(), nv()); | |
| 490 | } | ||
| 491 | |||
| 492 | template<typename D> | ||
| 493 | typename SizeDepType<NVExtended>::template RowsReturn<D>::ConstType | ||
| 494 | jointExtendedModelRows_impl(const Eigen::MatrixBase<D> & A) const | ||
| 495 | { | ||
| 496 | return SizeDepType<NVExtended>::middleRows(A.derived(), idx_vExtended(), nvExtended()); | ||
| 497 | } | ||
| 498 | |||
| 499 | // Non-const access | ||
| 500 | template<typename D> | ||
| 501 | 226 | typename SizeDepType<NV>::template RowsReturn<D>::Type jointRows(Eigen::MatrixBase<D> & A) const | |
| 502 | { | ||
| 503 | 226 | return derived().jointRows_impl(A.derived()); | |
| 504 | } | ||
| 505 | |||
| 506 | template<typename D> | ||
| 507 | typename SizeDepType<NVExtended>::template RowsReturn<D>::Type | ||
| 508 | jointExtendedModelRows(Eigen::MatrixBase<D> & A) const | ||
| 509 | { | ||
| 510 | return derived().jointExtendedModelRows_impl(A.derived()); | ||
| 511 | } | ||
| 512 | |||
| 513 | template<typename D> | ||
| 514 | typename SizeDepType<NV>::template RowsReturn<D>::Type | ||
| 515 | 196 | jointRows_impl(Eigen::MatrixBase<D> & A) const | |
| 516 | { | ||
| 517 | 196 | return SizeDepType<NV>::middleRows(A.derived(), idx_v(), nv()); | |
| 518 | } | ||
| 519 | |||
| 520 | template<typename D> | ||
| 521 | typename SizeDepType<NVExtended>::template RowsReturn<D>::Type | ||
| 522 | jointExtendedModelRows_impl(Eigen::MatrixBase<D> & A) const | ||
| 523 | { | ||
| 524 | return SizeDepType<NVExtended>::middleRows(A.derived(), idx_vExtended(), nvExtended()); | ||
| 525 | } | ||
| 526 | |||
| 527 | /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the | ||
| 528 | /// matrix Mat | ||
| 529 | // Const access | ||
| 530 | template<typename D> | ||
| 531 | typename SizeDepType<NV>::template BlockReturn<D>::ConstType | ||
| 532 | jointBlock(const Eigen::MatrixBase<D> & Mat) const | ||
| 533 | { | ||
| 534 | return derived().jointBlock_impl(Mat.derived()); | ||
| 535 | } | ||
| 536 | |||
| 537 | template<typename D> | ||
| 538 | typename SizeDepType<NVExtended>::template BlockReturn<D>::ConstType | ||
| 539 | jointExtendedModelBlock(const Eigen::MatrixBase<D> & Mat) const | ||
| 540 | { | ||
| 541 | return derived().jointExtendedModelBlock_impl(Mat.derived()); | ||
| 542 | } | ||
| 543 | |||
| 544 | template<typename D> | ||
| 545 | typename SizeDepType<NV>::template BlockReturn<D>::ConstType | ||
| 546 | jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const | ||
| 547 | { | ||
| 548 | return SizeDepType<NV>::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); | ||
| 549 | } | ||
| 550 | |||
| 551 | template<typename D> | ||
| 552 | typename SizeDepType<NVExtended>::template BlockReturn<D>::ConstType | ||
| 553 | jointExtendedModelBlock_impl(const Eigen::MatrixBase<D> & Mat) const | ||
| 554 | { | ||
| 555 | return SizeDepType<NVExtended>::block( | ||
| 556 | Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended()); | ||
| 557 | } | ||
| 558 | |||
| 559 | // Non-const access | ||
| 560 | template<typename D> | ||
| 561 | typename SizeDepType<NV>::template BlockReturn<D>::Type | ||
| 562 | 1605 | jointBlock(Eigen::MatrixBase<D> & Mat) const | |
| 563 | { | ||
| 564 | 1605 | return derived().jointBlock_impl(Mat.derived()); | |
| 565 | } | ||
| 566 | |||
| 567 | template<typename D> | ||
| 568 | typename SizeDepType<NVExtended>::template BlockReturn<D>::Type | ||
| 569 | jointExtendedModelBlock(Eigen::MatrixBase<D> & Mat) const | ||
| 570 | { | ||
| 571 | return derived().jointExtendedModelBlock_impl(Mat.derived()); | ||
| 572 | } | ||
| 573 | |||
| 574 | template<typename D> | ||
| 575 | typename SizeDepType<NV>::template BlockReturn<D>::Type | ||
| 576 | 794 | jointBlock_impl(Eigen::MatrixBase<D> & Mat) const | |
| 577 | { | ||
| 578 | 794 | return SizeDepType<NV>::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); | |
| 579 | } | ||
| 580 | |||
| 581 | template<typename D> | ||
| 582 | typename SizeDepType<NVExtended>::template BlockReturn<D>::Type | ||
| 583 | jointExtendedModelBlock_impl(Eigen::MatrixBase<D> & Mat) const | ||
| 584 | { | ||
| 585 | return SizeDepType<NVExtended>::block( | ||
| 586 | Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended()); | ||
| 587 | } | ||
| 588 | |||
| 589 | protected: | ||
| 590 | /// Default constructor: protected. | ||
| 591 | /// | ||
| 592 | /// Prevent the construction of stand-alone JointModelBase. | ||
| 593 | 83028 | inline JointModelBase() | |
| 594 | 83028 | : i_id(std::numeric_limits<JointIndex>::max()) | |
| 595 | 83028 | , i_q(-1) | |
| 596 | 83028 | , i_v(-1) | |
| 597 | 83028 | , i_vExtended(-1) | |
| 598 | { | ||
| 599 | 83028 | } | |
| 600 | |||
| 601 | /// Copy constructor: protected. | ||
| 602 | /// | ||
| 603 | /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting | ||
| 604 | /// objects. Copy is done by calling copy operator. | ||
| 605 | 355543 | inline JointModelBase(const JointModelBase & clone) | |
| 606 | { | ||
| 607 | 355543 | *this = clone; | |
| 608 | 355543 | } | |
| 609 | |||
| 610 | /// Copy operator: protected. | ||
| 611 | /// | ||
| 612 | /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting | ||
| 613 | /// objects. | ||
| 614 | 184097 | inline JointModelBase & operator=(const JointModelBase & clone) | |
| 615 | { | ||
| 616 | 184097 | i_id = clone.i_id; | |
| 617 | 184097 | i_q = clone.i_q; | |
| 618 | 184097 | i_v = clone.i_v; | |
| 619 | 184097 | i_vExtended = clone.i_vExtended; | |
| 620 | 184097 | return *this; | |
| 621 | } | ||
| 622 | |||
| 623 | // data | ||
| 624 | JointIndex i_id; // ID of the joint in the multibody list. | ||
| 625 | int i_q; // Index of the joint configuration in the joint configuration vector. | ||
| 626 | int i_v; // Index of the joint velocity in the joint velocity vector. | ||
| 627 | int i_vExtended; // Index of the joint jacobian in the joint jacobian matrix. | ||
| 628 | |||
| 629 | }; // struct JointModelBase | ||
| 630 | |||
| 631 | } // namespace pinocchio | ||
| 632 | |||
| 633 | #endif // ifndef __pinocchio_multibody_joint_model_base_hpp__ | ||
| 634 |