joint-generic.hpp 11.4 KB
Newer Older
1
//
2
// Copyright (c) 2016-2021 CNRS INRIA
3
4
//

5
6
#ifndef __pinocchio_joint_generic_hpp__
#define __pinocchio_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
14
#include <boost/mpl/contains.hpp>

15
namespace pinocchio
16
17
{

18
19
20
  template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
  struct JointTpl;
  typedef JointTpl<double> Joint;
21

22
23
  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointTpl<_Scalar,_Options,JointCollectionTpl> >
24
25
  {
    enum {
26
      Options = _Options,
27
28
      NQ = Eigen::Dynamic, // Dynamic because unknown at compile time
      NV = Eigen::Dynamic
29
    };
30
31
32
33
34
35
    
    typedef _Scalar Scalar;
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
    
36
37
38
39
40
    typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionTpl<Scalar,Options>  Motion_t;
    typedef MotionTpl<Scalar,Options>  Bias_t;

41
    // [ABA]
42
43
44
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;
45
46
    
    typedef Constraint_t ConstraintTypeConstRef;
47
    typedef Constraint_t ConstraintTypeRef;
48
    typedef Transformation_t TansformTypeConstRef;
49
    typedef Transformation_t TansformTypeRef;
50
    typedef Motion_t MotionTypeConstRef;
51
    typedef Motion_t MotionTypeRef;
52
    typedef Bias_t BiasTypeConstRef;
53
    typedef Bias_t BiasTypeRef;
54
55
56
    typedef U_t UTypeConstRef;
    typedef U_t UTypeRef;
    typedef D_t DTypeConstRef;
57
    typedef D_t DTypeRef;
58
    typedef UD_t UDTypeConstRef;
59
    typedef UD_t UDTypeRef;
60

61
62
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
63
  };
jcarpent's avatar
jcarpent committed
64
  
65
66
67
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointDataTpl<Scalar,Options,JointCollectionTpl> >
  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
68
  
69
70
71
72
73
74
75
76
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointModelTpl<Scalar,Options,JointCollectionTpl> >
  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct JointDataTpl
  : public JointDataBase< JointDataTpl<_Scalar,_Options,JointCollectionTpl> >
  , JointCollectionTpl<_Scalar,_Options>::JointDataVariant
77
  {
jcarpent's avatar
jcarpent committed
78
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
79
80
81
    
    typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
    typedef JointDataBase<JointDataTpl> Base;
jcarpent's avatar
jcarpent committed
82
    
83
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
84

85
86
87
    typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
    typedef typename JointCollection::JointDataVariant JointDataVariant;
    
Justin Carpentier's avatar
Justin Carpentier committed
88
89
90
    using Base::operator==;
    using Base::operator!=;
    
jcarpent's avatar
jcarpent committed
91
92
    JointDataVariant & toVariant() { return *static_cast<JointDataVariant*>(this); }
    const JointDataVariant & toVariant() const { return *static_cast<const JointDataVariant*>(this); }
93

94
95
96
97
    Constraint_t      S() const  { return constraint_xd(*this); }
    Transformation_t  M() const  { return joint_transform(*this); }
    Motion_t          v() const  { return motion(*this); }
    Bias_t            c() const  { return bias(*this); }
98
    
99
100
101
102
    // [ABA CCRBA]
    U_t               U()     const { return u_inertia(*this); }
    D_t               Dinv()  const { return dinv_inertia(*this); }
    UD_t              UDinv() const { return udinv_inertia(*this); }
103

104
105
106
    JointDataTpl()
    : JointDataVariant()
    {}
107
108
109
110
    
    JointDataTpl(const JointDataVariant & jdata_variant)
    : JointDataVariant(jdata_variant)
    {}
111
112
113
114
115
116
117
    
    template<typename JointDataDerived>
    JointDataTpl(const JointDataBase<JointDataDerived> & jdata)
    : JointCollection::JointDataVariant((JointDataVariant)jdata.derived())
    {
      BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types,JointDataDerived>));
    }
118
    
119
    // Define all the standard accessors
120
121
122
123
124
125
126
    Constraint_t S_accessor() const { return S(); }
    Transformation_t M_accessor() const { return M(); }
    Motion_t v_accessor() const { return v(); }
    Bias_t c_accessor() const { return c(); }
    U_t U_accessor() const { return U(); }
    D_t Dinv_accessor() const { return Dinv(); }
    UD_t UDinv_accessor() const { return UDinv(); }
127

128
129
    static std::string classname() { return "JointData"; }
    std::string shortname() const { return ::pinocchio::shortname(*this); }
Justin Carpentier's avatar
Justin Carpentier committed
130
    
131
132
133
    template<typename JointDataDerived>
    bool isEqual(const JointDataBase<JointDataDerived> & other) const
    {
134
      return ::pinocchio::isEqual(*this,other.derived());
135
136
    }
    
Justin Carpentier's avatar
Justin Carpentier committed
137
138
    bool isEqual(const JointDataTpl & other) const
    {
139
140
141
142
143
144
      return Base::isEqual(other) && toVariant() == other.toVariant();
    }
    
    bool operator==(const JointDataTpl & other) const
    {
      return isEqual(other);
Justin Carpentier's avatar
Justin Carpentier committed
145
    }
146
147
148
149
150
    
    bool operator!=(const JointDataTpl & other) const
    {
      return !(*this == other);
    }
151

152
  };
153
154
155
156
157
158
  
  template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct CastType< NewScalar, JointModelTpl<Scalar,Options,JointCollectionTpl> >
  {
    typedef JointModelTpl<NewScalar,Options,JointCollectionTpl> type;
  };
159

160
161
162
163
  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct JointModelTpl
  : JointModelBase< JointModelTpl<_Scalar,_Options,JointCollectionTpl> >
  , JointCollectionTpl<_Scalar,_Options>::JointModelVariant
164
  {
jcarpent's avatar
jcarpent committed
165
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
166
    
167
168
    typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;

169
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
170
    PINOCCHIO_JOINT_USE_INDEXES(JointModelTpl);
171
172
    
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
173
174
175
    typedef typename JointCollection::JointDataVariant JointDataVariant;
    typedef typename JointCollection::JointModelVariant JointModelVariant;
    
176
177
178
    using Base::id;
    using Base::setIndexes;
    using Base::operator==;
179
    using Base::operator!=;
180

181
    JointModelTpl() : JointModelVariant() {}
182
183
184
    
    JointModelTpl(const JointModelVariant & jmodel_variant)
    : JointCollection::JointModelVariant(jmodel_variant)
185
186
187
188
189
    {
      setIndexes(::pinocchio::id(jmodel_variant),
                 ::pinocchio::nq(jmodel_variant),
                 ::pinocchio::nv(jmodel_variant));
    }
jcarpent's avatar
jcarpent committed
190
    
191
192
    template<typename JointModelDerived>
    JointModelTpl(const JointModelBase<JointModelDerived> & jmodel)
193
    : JointModelVariant((JointModelVariant)jmodel.derived())
194
195
    {
      BOOST_MPL_ASSERT((boost::mpl::contains<typename JointModelVariant::types,JointModelDerived>));
196
197

      setIndexes(jmodel.id(),jmodel.nq(),jmodel.nv());
198
199
    }
    
200
201
202
203
204
    JointModelVariant & toVariant()
    { return *static_cast<JointModelVariant*>(this); }
    
    const JointModelVariant & toVariant() const
    { return *static_cast<const JointModelVariant*>(this); }
205

206
    JointDataDerived createData() const
207
    { return ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(*this); }
208

209
210
211
    template<typename JointModelDerived>
    bool isEqual(const JointModelBase<JointModelDerived> & other) const
    {
212
213
214
215
216
217
218
      return ::pinocchio::isEqual(*this,other.derived());;
    }
    
    template<typename JointModelDerived>
    bool hasSameIndexes(const JointModelBase<JointModelDerived> & other) const
    {
      return ::pinocchio::hasSameIndexes(*this,other.derived());
219
220
    }
    
221
222
    bool isEqual(const JointModelTpl & other) const
    {
223
224
225
226
227
228
229
230
231
232
233
      return Base::isEqual(other) && toVariant() == other.toVariant();
    }
    
    bool operator==(const JointModelTpl & other) const
    {
      return isEqual(other);
    }
    
    bool operator!=(const JointModelTpl & other) const
    {
      return !(*this == other);
234
235
    }

236
237
238
    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const Eigen::MatrixBase<ConfigVector> & q) const
239
    { calc_zero_order(*this,data,q); }
240

241
242
243
244
    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const Eigen::MatrixBase<ConfigVector> & q,
              const Eigen::MatrixBase<TangentVector> & v) const
jcarpent's avatar
jcarpent committed
245
    { calc_first_order(*this,data,q,v); }
246
    
247
248
    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
gabrielebndn's avatar
gabrielebndn committed
249
    { ::pinocchio::calc_aba(*this,data,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),update_I); }
250
    
251
    std::string shortname() const { return ::pinocchio::shortname(*this); }
252
    static std::string classname() { return "JointModel"; }
253

254
255
    int     nq_impl() const { return ::pinocchio::nq(*this); }
    int     nv_impl() const { return ::pinocchio::nv(*this); }
256

257
258
    int     idx_q_impl()   const { return ::pinocchio::idx_q(*this); }
    int     idx_v_impl()   const { return ::pinocchio::idx_v(*this); }
259

260
    JointIndex     id_impl()      const { return ::pinocchio::id(*this); }
261

262
    void setIndexes(JointIndex id, int nq, int nv)
263
264
265
    {
      ::pinocchio::setIndexes(*this, id, nq, nv);
    }
266
267
268
269
270
271
272
    
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelTpl<NewScalar,Options,JointCollectionTpl> cast() const
    {
      return cast_joint<NewScalar,Scalar,Options,JointCollectionTpl>(*this);
    }
273
274
  };
  
275
276
  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
277

278
279
280
281
282
283
284
285
  
  template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
  bool operator==(const JointDataBase<JointDataDerived> & joint_data,
                  const JointDataTpl<Scalar,Options,JointCollectionTpl> & joint_data_generic)
  {
    return joint_data_generic == joint_data.derived();
  }

286
287
288
289
290
291
292
293
  
  template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
  bool operator!=(const JointDataBase<JointDataDerived> & joint_data,
                  const JointDataTpl<Scalar,Options,JointCollectionTpl> & joint_data_generic)
  {
    return joint_data_generic != joint_data.derived();
  }

294
295
296
297
298
299
300
301
302
303
304
305
306
307
  template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
  bool operator==(const JointModelBase<JointModelDerived> & joint_model,
                  const JointModelTpl<Scalar,Options,JointCollectionTpl> & joint_model_generic)
  {
    return joint_model_generic == joint_model.derived();
  }

  template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
  bool operator!=(const JointModelBase<JointModelDerived> & joint_model,
                  const JointModelTpl<Scalar,Options,JointCollectionTpl> & joint_model_generic)
  {
    return joint_model_generic != joint_model.derived();
  }

308
} // namespace pinocchio
309

310
#endif // ifndef __pinocchio_joint_generic_hpp__