pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-data-base.hpp
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_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>::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  typedef TYPENAME traits<Joint>::UTypeConstRef UTypeConstRef; \
23  typedef TYPENAME traits<Joint>::UTypeRef UTypeRef; \
24  typedef TYPENAME traits<Joint>::DTypeConstRef DTypeConstRef; \
25  typedef TYPENAME traits<Joint>::DTypeRef DTypeRef; \
26  typedef TYPENAME traits<Joint>::UDTypeConstRef UDTypeConstRef; \
27  typedef TYPENAME traits<Joint>::UDTypeRef UDTypeRef
28 
29 #ifdef __clang__
30 
31  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
32  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
33 
34 #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
35 
36  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
37  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
38 
39 #else
40 
41  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
42  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
43 
44 #endif
45 
46 #define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR \
47  ConstraintTypeConstRef S_accessor() const { return S; } \
48  ConstraintTypeRef S_accessor() { return S; } \
49  TansformTypeConstRef M_accessor() const { return M; } \
50  TansformTypeRef M_accessor() { return M; } \
51  MotionTypeConstRef v_accessor() const { return v; } \
52  MotionTypeRef v_accessor() { return v; } \
53  BiasTypeConstRef c_accessor() const { return c; } \
54  BiasTypeRef c_accessor() { return c; } \
55  UTypeConstRef U_accessor() const { return U; } \
56  UTypeRef U_accessor() { return U; } \
57  DTypeConstRef Dinv_accessor() const { return Dinv; } \
58  DTypeRef Dinv_accessor() { return Dinv; } \
59  UDTypeConstRef UDinv_accessor() const { return UDinv; } \
60  UDTypeRef UDinv_accessor() { return UDinv; }
61 
62 #define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE \
63  typedef const Constraint_t & ConstraintTypeConstRef; \
64  typedef Constraint_t & ConstraintTypeRef; \
65  typedef const Transformation_t & TansformTypeConstRef; \
66  typedef Transformation_t & TansformTypeRef; \
67  typedef const Motion_t & MotionTypeConstRef; \
68  typedef Motion_t & MotionTypeRef; \
69  typedef const Bias_t & BiasTypeConstRef; \
70  typedef Bias_t & BiasTypeRef; \
71  typedef const U_t & UTypeConstRef; \
72  typedef U_t & UTypeRef; \
73  typedef const D_t & DTypeConstRef; \
74  typedef D_t & DTypeRef; \
75  typedef const UD_t & UDTypeConstRef; \
76  typedef UD_t & UDTypeRef;
77 
78 namespace pinocchio
79 {
80 
81  template<typename Derived>
82  struct JointDataBase
83  {
84  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85 
86  typedef typename traits<Derived>::JointDerived JointDerived;
87  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
88 
89  Derived & derived() { return *static_cast<Derived*>(this); }
90  const Derived & derived() const { return *static_cast<const Derived*>(this); }
91 
92  ConstraintTypeConstRef S() const { return derived().S_accessor(); }
93  ConstraintTypeRef S() { return derived().S_accessor(); }
94  TansformTypeConstRef M() const { return derived().M_accessor(); }
95  TansformTypeRef M() { return derived().M_accessor(); }
96  MotionTypeConstRef v() const { return derived().v_accessor(); }
97  MotionTypeRef v() { return derived().v_accessor(); }
98  BiasTypeConstRef c() const { return derived().c_accessor(); }
99  BiasTypeRef c() { return derived().c_accessor(); }
100 
101  UTypeConstRef U() const { return derived().U_accessor(); }
102  UTypeRef U() { return derived().U_accessor(); }
103  DTypeConstRef Dinv() const { return derived().Dinv_accessor(); }
104  DTypeRef Dinv() { return derived().Dinv_accessor(); }
105  UDTypeConstRef UDinv() const { return derived().UDinv_accessor(); }
106  UDTypeRef UDinv() { return derived().UDinv_accessor(); }
107 
108  std::string shortname() const { return derived().shortname(); }
109  static std::string classname() { return Derived::classname(); }
110 
111  void disp(std::ostream & os) const
112  {
113  using namespace std;
114  os << shortname() << endl;
115  }
116 
117  friend std::ostream & operator << (std::ostream & os, const JointDataBase<Derived> & joint)
118  {
119  joint.disp(os);
120  return os;
121  }
122 
123  template<typename OtherDerived>
124  bool operator==(const JointDataBase<OtherDerived> & other) const
125  {
126  return derived().isEqual(other.derived());
127  }
128 
130  bool isEqual(const JointDataBase & other) const
131  {
132  return S() == other.S()
133  && M() == other.M()
134  && v() == other.v()
135  && c() == other.c()
136  && U() == other.U()
137  && Dinv() == other.Dinv()
138  && UDinv() == other.UDinv()
139  ;
140  }
141 
143  template<typename OtherDerived>
144  bool isEqual(const JointDataBase<OtherDerived> & /*other*/) const
145  {
146  return false;
147  ;
148  }
149 
150  bool operator!=(const JointDataBase<Derived> & other) const
151  {
152  return derived().isNotEqual(other.derived());
153  }
154 
156  bool isNotEqual(const JointDataBase<Derived> & other) const
157  {
158  return !(derived() == other.derived());
159  }
160 
161  protected:
162 
164  inline JointDataBase() {}
165 
166  }; // struct JointDataBase
167 
168 } // namespace pinocchio
169 
170 #endif // ifndef __pinocchio_multibody_joint_data_base_hpp__
pinocchio::JointDataBase::isNotEqual
bool isNotEqual(const JointDataBase< Derived > &other) const
&#160;
Definition: joint-data-base.hpp:156
pinocchio::JointDataBase::JointDataBase
JointDataBase()
Default constructor: protected.
Definition: joint-data-base.hpp:164
pinocchio::JointDataBase
Definition: joint-data-base.hpp:82
pinocchio::JointDataBase::isEqual
bool isEqual(const JointDataBase &other) const
&#160;
Definition: joint-data-base.hpp:130
pinocchio::shortname
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model.
pinocchio::JointDataBase::isEqual
bool isEqual(const JointDataBase< OtherDerived > &) const
&#160;
Definition: joint-data-base.hpp:144
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11