joint-data-base.hpp 5.55 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_multibody_joint_data_base_hpp__
#define __pinocchio_multibody_joint_data_base_hpp__

#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-model-base.hpp"
  
#define PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,TYPENAME)              \
  PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,TYPENAME); \
  typedef TYPENAME traits<Joint>::ConstraintTypeConstRef ConstraintTypeConstRef;      \
15
  typedef TYPENAME traits<Joint>::ConstraintTypeRef ConstraintTypeRef;      \
16
  typedef TYPENAME traits<Joint>::TansformTypeConstRef TansformTypeConstRef;      \
17
  typedef TYPENAME traits<Joint>::TansformTypeRef TansformTypeRef;      \
18
  typedef TYPENAME traits<Joint>::MotionTypeConstRef MotionTypeConstRef;      \
19
  typedef TYPENAME traits<Joint>::MotionTypeRef MotionTypeRef;      \
20
  typedef TYPENAME traits<Joint>::BiasTypeConstRef BiasTypeConstRef;      \
21
  typedef TYPENAME traits<Joint>::BiasTypeRef BiasTypeRef;      \
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
  typedef TYPENAME traits<Joint>::UTypeConstRef UTypeConstRef;      \
  typedef TYPENAME traits<Joint>::UTypeRef UTypeRef;      \
  typedef TYPENAME traits<Joint>::DTypeConstRef DTypeConstRef;      \
  typedef TYPENAME traits<Joint>::UDTypeConstRef UDTypeConstRef
  
#ifdef __clang__

  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)

#elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)

  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)

#else

  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)

#endif
  
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR \
  ConstraintTypeConstRef S_accessor() const { return S; } \
46
  ConstraintTypeRef S_accessor() { return S; } \
47
  TansformTypeConstRef M_accessor() const { return M; } \
48
  TansformTypeRef M_accessor() { return M; } \
49
  MotionTypeConstRef v_accessor() const { return v; } \
50
  MotionTypeRef v_accessor() { return v; } \
51
  BiasTypeConstRef c_accessor() const { return c; } \
52
  BiasTypeRef c_accessor() { return c; } \
53
54
55
56
57
58
59
  UTypeConstRef U_accessor() const { return U; } \
  UTypeRef U_accessor() { return U; } \
  DTypeConstRef Dinv_accessor() const { return Dinv; } \
  UDTypeConstRef UDinv_accessor() const { return UDinv; }
  
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE \
  typedef const Constraint_t & ConstraintTypeConstRef; \
60
  typedef Constraint_t & ConstraintTypeRef; \
61
  typedef const Transformation_t & TansformTypeConstRef; \
62
  typedef Transformation_t & TansformTypeRef; \
63
  typedef const Motion_t & MotionTypeConstRef; \
64
  typedef Motion_t & MotionTypeRef; \
65
  typedef const Bias_t & BiasTypeConstRef; \
66
  typedef Bias_t & BiasTypeRef; \
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
  typedef const U_t & UTypeConstRef; \
  typedef U_t & UTypeRef; \
  typedef const D_t & DTypeConstRef; \
  typedef const UD_t & UDTypeConstRef;

namespace pinocchio
{

  template<typename Derived>
  struct JointDataBase
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    typedef typename traits<Derived>::JointDerived JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);

    Derived & derived() { return *static_cast<Derived*>(this); }
    const Derived & derived() const { return *static_cast<const Derived*>(this); }

    ConstraintTypeConstRef S() const     { return derived().S_accessor(); }
87
    ConstraintTypeRef S()     { return derived().S_accessor(); }
88
    TansformTypeConstRef M() const     { return derived().M_accessor(); }
89
    TansformTypeRef M() { return derived().M_accessor(); }
90
    MotionTypeConstRef v() const     { return derived().v_accessor(); }
91
    MotionTypeRef v()     { return derived().v_accessor(); }
92
    BiasTypeConstRef c() const     { return derived().c_accessor(); }
93
    BiasTypeRef c()     { return derived().c_accessor(); }
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114

    UTypeConstRef U() const     { return derived().U_accessor(); }
    UTypeRef U()           { return derived().U_accessor(); }
    DTypeConstRef Dinv() const  { return derived().Dinv_accessor(); }
    UDTypeConstRef UDinv() const { return derived().UDinv_accessor(); }

    std::string shortname() const { return derived().shortname(); }
    static std::string classname() { return Derived::classname(); }

    void disp(std::ostream & os) const
    {
      using namespace std;
      os << shortname() << endl;
    }
    
    friend std::ostream & operator << (std::ostream & os, const JointDataBase<Derived> & joint)
    {
      joint.disp(os);
      return os;
    }
    
Justin Carpentier's avatar
Justin Carpentier committed
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
    bool operator==(const JointDataBase<Derived> & other) const
    {
      return derived().isEqual(other.derived());
    }
    
    /// \brief Default operator== implementation
    bool isEqual(const JointDataBase<Derived> & other) const
    {
      return S() == other.S()
          && M() == other.M()
          && v() == other.v()
          && c() == other.c()
          && U() == other.U()
          && Dinv() == other.Dinv()
          && UDinv() == other.UDinv()
      ;
    }
    
133
134
135
136
137
138
139
140
141
142
  protected:
    
    /// \brief Default constructor: protected.
    inline JointDataBase() {}

  }; // struct JointDataBase

} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_joint_data_base_hpp__