| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/joint/joint-data-base.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 66 | 72 | 91.7% |
| Branches: | 70 | 131 | 53.4% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
| 3 | // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
| 4 | // | ||
| 5 | |||
| 6 | #ifndef __pinocchio_multibody_joint_data_base_hpp__ | ||
| 7 | #define __pinocchio_multibody_joint_data_base_hpp__ | ||
| 8 | |||
| 9 | #include "pinocchio/multibody/joint/joint-base.hpp" | ||
| 10 | #include "pinocchio/multibody/joint/joint-model-base.hpp" | ||
| 11 | |||
| 12 | #define PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, TYPENAME) \ | ||
| 13 | PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, TYPENAME); \ | ||
| 14 | typedef TYPENAME traits<Joint>::ConfigVectorTypeConstRef ConfigVectorTypeConstRef; \ | ||
| 15 | typedef TYPENAME traits<Joint>::ConfigVectorTypeRef ConfigVectorTypeRef; \ | ||
| 16 | typedef TYPENAME traits<Joint>::TangentVectorTypeConstRef TangentVectorTypeConstRef; \ | ||
| 17 | typedef TYPENAME traits<Joint>::TangentVectorTypeRef TangentVectorTypeRef; \ | ||
| 18 | typedef TYPENAME traits<Joint>::ConstraintTypeConstRef ConstraintTypeConstRef; \ | ||
| 19 | typedef TYPENAME traits<Joint>::ConstraintTypeRef ConstraintTypeRef; \ | ||
| 20 | typedef TYPENAME traits<Joint>::TansformTypeConstRef TansformTypeConstRef; \ | ||
| 21 | typedef TYPENAME traits<Joint>::TansformTypeRef TansformTypeRef; \ | ||
| 22 | typedef TYPENAME traits<Joint>::MotionTypeConstRef MotionTypeConstRef; \ | ||
| 23 | typedef TYPENAME traits<Joint>::MotionTypeRef MotionTypeRef; \ | ||
| 24 | typedef TYPENAME traits<Joint>::BiasTypeConstRef BiasTypeConstRef; \ | ||
| 25 | typedef TYPENAME traits<Joint>::BiasTypeRef BiasTypeRef; \ | ||
| 26 | typedef TYPENAME traits<Joint>::UTypeConstRef UTypeConstRef; \ | ||
| 27 | typedef TYPENAME traits<Joint>::UTypeRef UTypeRef; \ | ||
| 28 | typedef TYPENAME traits<Joint>::DTypeConstRef DTypeConstRef; \ | ||
| 29 | typedef TYPENAME traits<Joint>::DTypeRef DTypeRef; \ | ||
| 30 | typedef TYPENAME traits<Joint>::UDTypeConstRef UDTypeConstRef; \ | ||
| 31 | typedef TYPENAME traits<Joint>::UDTypeRef UDTypeRef | ||
| 32 | |||
| 33 | #ifdef __clang__ | ||
| 34 | |||
| 35 | #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) \ | ||
| 36 | PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG) | ||
| 37 | #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) \ | ||
| 38 | PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, typename) | ||
| 39 | |||
| 40 | #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2) | ||
| 41 | |||
| 42 | #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) \ | ||
| 43 | PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG) | ||
| 44 | #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) \ | ||
| 45 | PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, typename) | ||
| 46 | |||
| 47 | #else | ||
| 48 | |||
| 49 | #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, typename) | ||
| 50 | #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) \ | ||
| 51 | PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, typename) | ||
| 52 | |||
| 53 | #endif | ||
| 54 | |||
| 55 | #define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR \ | ||
| 56 | ConfigVectorTypeConstRef joint_q_accessor() const \ | ||
| 57 | { \ | ||
| 58 | return joint_q; \ | ||
| 59 | } \ | ||
| 60 | ConfigVectorTypeRef joint_q_accessor() \ | ||
| 61 | { \ | ||
| 62 | return joint_q; \ | ||
| 63 | } \ | ||
| 64 | TangentVectorTypeConstRef joint_v_accessor() const \ | ||
| 65 | { \ | ||
| 66 | return joint_v; \ | ||
| 67 | } \ | ||
| 68 | TangentVectorTypeRef joint_v_accessor() \ | ||
| 69 | { \ | ||
| 70 | return joint_v; \ | ||
| 71 | } \ | ||
| 72 | ConstraintTypeConstRef S_accessor() const \ | ||
| 73 | { \ | ||
| 74 | return S; \ | ||
| 75 | } \ | ||
| 76 | ConstraintTypeRef S_accessor() \ | ||
| 77 | { \ | ||
| 78 | return S; \ | ||
| 79 | } \ | ||
| 80 | TansformTypeConstRef M_accessor() const \ | ||
| 81 | { \ | ||
| 82 | return M; \ | ||
| 83 | } \ | ||
| 84 | TansformTypeRef M_accessor() \ | ||
| 85 | { \ | ||
| 86 | return M; \ | ||
| 87 | } \ | ||
| 88 | MotionTypeConstRef v_accessor() const \ | ||
| 89 | { \ | ||
| 90 | return v; \ | ||
| 91 | } \ | ||
| 92 | MotionTypeRef v_accessor() \ | ||
| 93 | { \ | ||
| 94 | return v; \ | ||
| 95 | } \ | ||
| 96 | BiasTypeConstRef c_accessor() const \ | ||
| 97 | { \ | ||
| 98 | return c; \ | ||
| 99 | } \ | ||
| 100 | BiasTypeRef c_accessor() \ | ||
| 101 | { \ | ||
| 102 | return c; \ | ||
| 103 | } \ | ||
| 104 | UTypeConstRef U_accessor() const \ | ||
| 105 | { \ | ||
| 106 | return U; \ | ||
| 107 | } \ | ||
| 108 | UTypeRef U_accessor() \ | ||
| 109 | { \ | ||
| 110 | return U; \ | ||
| 111 | } \ | ||
| 112 | DTypeConstRef Dinv_accessor() const \ | ||
| 113 | { \ | ||
| 114 | return Dinv; \ | ||
| 115 | } \ | ||
| 116 | DTypeRef Dinv_accessor() \ | ||
| 117 | { \ | ||
| 118 | return Dinv; \ | ||
| 119 | } \ | ||
| 120 | UDTypeConstRef UDinv_accessor() const \ | ||
| 121 | { \ | ||
| 122 | return UDinv; \ | ||
| 123 | } \ | ||
| 124 | UDTypeRef UDinv_accessor() \ | ||
| 125 | { \ | ||
| 126 | return UDinv; \ | ||
| 127 | } \ | ||
| 128 | DTypeConstRef StU_accessor() const \ | ||
| 129 | { \ | ||
| 130 | return StU; \ | ||
| 131 | } \ | ||
| 132 | DTypeRef StU_accessor() \ | ||
| 133 | { \ | ||
| 134 | return StU; \ | ||
| 135 | } | ||
| 136 | |||
| 137 | #define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE \ | ||
| 138 | typedef const ConfigVector_t & ConfigVectorTypeConstRef; \ | ||
| 139 | typedef ConfigVector_t & ConfigVectorTypeRef; \ | ||
| 140 | typedef const TangentVector_t & TangentVectorTypeConstRef; \ | ||
| 141 | typedef TangentVector_t & TangentVectorTypeRef; \ | ||
| 142 | typedef const Constraint_t & ConstraintTypeConstRef; \ | ||
| 143 | typedef Constraint_t & ConstraintTypeRef; \ | ||
| 144 | typedef const Transformation_t & TansformTypeConstRef; \ | ||
| 145 | typedef Transformation_t & TansformTypeRef; \ | ||
| 146 | typedef const Motion_t & MotionTypeConstRef; \ | ||
| 147 | typedef Motion_t & MotionTypeRef; \ | ||
| 148 | typedef const Bias_t & BiasTypeConstRef; \ | ||
| 149 | typedef Bias_t & BiasTypeRef; \ | ||
| 150 | typedef const U_t & UTypeConstRef; \ | ||
| 151 | typedef U_t & UTypeRef; \ | ||
| 152 | typedef const D_t & DTypeConstRef; \ | ||
| 153 | typedef D_t & DTypeRef; \ | ||
| 154 | typedef const UD_t & UDTypeConstRef; \ | ||
| 155 | typedef UD_t & UDTypeRef; | ||
| 156 | |||
| 157 | namespace pinocchio | ||
| 158 | { | ||
| 159 | |||
| 160 | template<typename Derived> | ||
| 161 | struct JointDataBase : NumericalBase<Derived> | ||
| 162 | { | ||
| 163 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 164 | |||
| 165 | typedef typename traits<Derived>::JointDerived JointDerived; | ||
| 166 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
| 167 | |||
| 168 | 3581974 | Derived & derived() | |
| 169 | { | ||
| 170 | 3581974 | return *static_cast<Derived *>(this); | |
| 171 | } | ||
| 172 | 52142 | const Derived & derived() const | |
| 173 | { | ||
| 174 | 94732 | return *static_cast<const Derived *>(this); | |
| 175 | } | ||
| 176 | |||
| 177 | 5808 | ConfigVectorTypeConstRef joint_q() const | |
| 178 | { | ||
| 179 | 5808 | return derived().joint_q_accessor(); | |
| 180 | } | ||
| 181 | 882 | ConfigVectorTypeRef joint_q() | |
| 182 | { | ||
| 183 | 1764 | return derived().joint_q_accessor(); | |
| 184 | } | ||
| 185 | |||
| 186 | 5008 | TangentVectorTypeConstRef joint_v() const | |
| 187 | { | ||
| 188 | 5008 | return derived().joint_v_accessor(); | |
| 189 | } | ||
| 190 | 8405 | TangentVectorTypeRef joint_v() | |
| 191 | { | ||
| 192 | 9287 | return derived().joint_v_accessor(); | |
| 193 | } | ||
| 194 | |||
| 195 | 5114 | ConstraintTypeConstRef S() const | |
| 196 | { | ||
| 197 | 5114 | return derived().S_accessor(); | |
| 198 | } | ||
| 199 | 297410 | ConstraintTypeRef S() | |
| 200 | { | ||
| 201 | 298738 | return derived().S_accessor(); | |
| 202 | } | ||
| 203 | 3066 | TansformTypeConstRef M() const | |
| 204 | { | ||
| 205 | 5120 | return derived().M_accessor(); | |
| 206 | } | ||
| 207 | 1299452 | TansformTypeRef M() | |
| 208 | { | ||
| 209 | 1300388 | return derived().M_accessor(); | |
| 210 | } | ||
| 211 | 3085 | MotionTypeConstRef v() const | |
| 212 | { | ||
| 213 | 5162 | return derived().v_accessor(); | |
| 214 | } | ||
| 215 | 328244 | MotionTypeRef v() | |
| 216 | { | ||
| 217 | 329176 | return derived().v_accessor(); | |
| 218 | } | ||
| 219 | 3033 | BiasTypeConstRef c() const | |
| 220 | { | ||
| 221 | 5058 | return derived().c_accessor(); | |
| 222 | } | ||
| 223 | 193374 | BiasTypeRef c() | |
| 224 | { | ||
| 225 | 194254 | return derived().c_accessor(); | |
| 226 | } | ||
| 227 | |||
| 228 | 5332 | UTypeConstRef U() const | |
| 229 | { | ||
| 230 | 5332 | return derived().U_accessor(); | |
| 231 | } | ||
| 232 | 23765 | UTypeRef U() | |
| 233 | { | ||
| 234 | 25926 | return derived().U_accessor(); | |
| 235 | } | ||
| 236 | 3089 | DTypeConstRef Dinv() const | |
| 237 | { | ||
| 238 | 5170 | return derived().Dinv_accessor(); | |
| 239 | } | ||
| 240 | 30048 | DTypeRef Dinv() | |
| 241 | { | ||
| 242 | 37456 | return derived().Dinv_accessor(); | |
| 243 | } | ||
| 244 | 3089 | UDTypeConstRef UDinv() const | |
| 245 | { | ||
| 246 | 5170 | return derived().UDinv_accessor(); | |
| 247 | } | ||
| 248 | 33804 | UDTypeRef UDinv() | |
| 249 | { | ||
| 250 | 46818 | return derived().UDinv_accessor(); | |
| 251 | } | ||
| 252 | 83 | DTypeConstRef StU() const | |
| 253 | { | ||
| 254 | 166 | return derived().StU_accessor(); | |
| 255 | } | ||
| 256 | 10954 | DTypeRef StU() | |
| 257 | { | ||
| 258 | 11926 | return derived().StU_accessor(); | |
| 259 | } | ||
| 260 | |||
| 261 | 106 | std::string shortname() const | |
| 262 | { | ||
| 263 | 106 | return derived().shortname(); | |
| 264 | } | ||
| 265 | 52 | static std::string classname() | |
| 266 | { | ||
| 267 | 52 | return Derived::classname(); | |
| 268 | } | ||
| 269 | |||
| 270 | 52 | void disp(std::ostream & os) const | |
| 271 | { | ||
| 272 | using namespace std; | ||
| 273 |
2/4✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
|
52 | os << shortname() << endl; |
| 274 | 52 | } | |
| 275 | |||
| 276 | 52 | friend std::ostream & operator<<(std::ostream & os, const JointDataBase<Derived> & joint) | |
| 277 | { | ||
| 278 | 52 | joint.disp(os); | |
| 279 | 52 | return os; | |
| 280 | } | ||
| 281 | |||
| 282 | template<typename OtherDerived> | ||
| 283 | 1280 | bool operator==(const JointDataBase<OtherDerived> & other) const | |
| 284 | { | ||
| 285 | 1280 | return derived().isEqual(other.derived()); | |
| 286 | } | ||
| 287 | |||
| 288 | /// \brief Default operator== implementation | ||
| 289 | 2042 | bool isEqual(const JointDataBase<Derived> & other) const | |
| 290 | { | ||
| 291 |
1/6✓ Branch 1 taken 431 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
2042 | return internal::comparison_eq(joint_q(), other.joint_q()) |
| 292 |
8/17✓ Branch 1 taken 331 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 590 times.
✓ Branch 4 taken 331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 331 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 331 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 331 times.
✓ Branch 13 taken 100 times.
✓ Branch 15 taken 431 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
|
2704 | && internal::comparison_eq(joint_v(), other.joint_v()) |
| 293 |
14/25✓ Branch 1 taken 331 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 590 times.
✓ Branch 4 taken 331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 331 times.
✓ Branch 8 taken 590 times.
✓ Branch 9 taken 331 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 331 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 331 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 331 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 331 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 331 times.
✓ Branch 24 taken 100 times.
✓ Branch 26 taken 331 times.
✓ Branch 27 taken 100 times.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
|
2704 | && internal::comparison_eq(S(), other.S()) && internal::comparison_eq(M(), other.M()) |
| 294 |
10/17✓ Branch 1 taken 331 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 590 times.
✓ Branch 4 taken 331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 331 times.
✓ Branch 8 taken 590 times.
✓ Branch 9 taken 331 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 331 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 331 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 331 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 331 times.
✗ Branch 21 not taken.
|
1842 | && internal::comparison_eq(v(), other.v()) && internal::comparison_eq(c(), other.c()) |
| 295 |
9/17✓ Branch 1 taken 331 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 590 times.
✓ Branch 4 taken 331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 331 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 331 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 331 times.
✓ Branch 13 taken 100 times.
✓ Branch 15 taken 331 times.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
|
2704 | && internal::comparison_eq(U(), other.U()) |
| 296 |
9/17✓ Branch 1 taken 331 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 590 times.
✓ Branch 4 taken 331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 331 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 331 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 331 times.
✓ Branch 13 taken 100 times.
✓ Branch 15 taken 331 times.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
|
2704 | && internal::comparison_eq(Dinv(), other.Dinv()) |
| 297 |
17/28✓ Branch 0 taken 590 times.
✓ Branch 1 taken 431 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 431 times.
✓ Branch 5 taken 590 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 431 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 331 times.
✓ Branch 10 taken 100 times.
✓ Branch 12 taken 331 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 331 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 331 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 331 times.
✗ Branch 21 not taken.
✓ Branch 22 taken 331 times.
✓ Branch 23 taken 100 times.
✓ Branch 25 taken 331 times.
✓ Branch 26 taken 100 times.
✓ Branch 28 taken 331 times.
✓ Branch 29 taken 100 times.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
|
4746 | && internal::comparison_eq(UDinv(), other.UDinv()); |
| 298 | } | ||
| 299 | |||
| 300 | /// \brief Default operator== implementation | ||
| 301 | template<typename OtherDerived> | ||
| 302 | ✗ | bool isEqual(const JointDataBase<OtherDerived> & /*other*/) const | |
| 303 | { | ||
| 304 | ✗ | return false; | |
| 305 | ; | ||
| 306 | } | ||
| 307 | |||
| 308 | ✗ | bool operator!=(const JointDataBase<Derived> & other) const | |
| 309 | { | ||
| 310 | ✗ | return derived().isNotEqual(other.derived()); | |
| 311 | } | ||
| 312 | |||
| 313 | /// \brief Default operator!= implementation | ||
| 314 | ✗ | bool isNotEqual(const JointDataBase<Derived> & other) const | |
| 315 | { | ||
| 316 | ✗ | return !(internal::comparison_eq(derived(), other.derived())); | |
| 317 | } | ||
| 318 | |||
| 319 | protected: | ||
| 320 | /// \brief Default constructor: protected. | ||
| 321 | 195015 | inline JointDataBase() | |
| 322 | { | ||
| 323 | 195015 | } | |
| 324 | |||
| 325 | }; // struct JointDataBase | ||
| 326 | |||
| 327 | } // namespace pinocchio | ||
| 328 | |||
| 329 | #endif // ifndef __pinocchio_multibody_joint_data_base_hpp__ | ||
| 330 |