joint-generic.hpp 8.88 KB
Newer Older
1
//
2
// Copyright (c) 2016,2018 CNRS
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

18
19
#ifndef __se3_joint_generic_hpp__
#define __se3_joint_generic_hpp__
20
21

#include "pinocchio/assert.hpp"
22
#include "pinocchio/multibody/joint/joint-collection.hpp"
23
#include "pinocchio/multibody/joint/joint-composite.hpp"
24
#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
25
#include "pinocchio/container/aligned-vector.hpp"
26

27
28
#include <boost/mpl/contains.hpp>

29
30
31
namespace se3
{

32
33
34
  template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
  struct JointTpl;
  typedef JointTpl<double> Joint;
35

36
37
  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointTpl<_Scalar,_Options,JointCollectionTpl> >
38
39
  {
    enum {
40
      Options = _Options,
41
42
      NQ = Eigen::Dynamic, // Dynamic because unknown at compile time
      NV = Eigen::Dynamic
43
    };
44
45
46
47
48
49
    
    typedef _Scalar Scalar;
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
    
50
51
52
53
54
55
    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;

    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> F_t;
56
    // [ABA]
57
58
59
    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;
60
61
62
63
64
65
66
67
68
    
    typedef Constraint_t ConstraintTypeConstRef;
    typedef Transformation_t TansformTypeConstRef;
    typedef Motion_t MotionTypeConstRef;
    typedef Bias_t BiasTypeConstRef;
    typedef U_t UTypeConstRef;
    typedef U_t UTypeRef;
    typedef D_t DTypeConstRef;
    typedef UD_t UDTypeConstRef;
69

70
71
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
72
  };
jcarpent's avatar
jcarpent committed
73
  
74
75
76
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointDataTpl<Scalar,Options,JointCollectionTpl> >
  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
77
  
78
79
80
81
82
83
84
85
  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
86
  {
jcarpent's avatar
jcarpent committed
87
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
88
89
90
    
    typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
    typedef JointDataBase<JointDataTpl> Base;
jcarpent's avatar
jcarpent committed
91
    
92
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE;
93

94
95
96
    typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
    typedef typename JointCollection::JointDataVariant JointDataVariant;
    
jcarpent's avatar
jcarpent committed
97
98
    JointDataVariant & toVariant() { return *static_cast<JointDataVariant*>(this); }
    const JointDataVariant & toVariant() const { return *static_cast<const JointDataVariant*>(this); }
99

100
101
102
103
    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); }
104
    
105
106
107
108
    // [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); }
109

110
    JointDataTpl() : JointDataVariant() {}
111
112
113
114
    
    JointDataTpl(const JointDataVariant & jdata_variant)
    : JointDataVariant(jdata_variant)
    {}
115
116
117
118
119
120
121
    
    template<typename JointDataDerived>
    JointDataTpl(const JointDataBase<JointDataDerived> & jdata)
    : JointCollection::JointDataVariant((JointDataVariant)jdata.derived())
    {
      BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types,JointDataDerived>));
    }
122
123
124
125
126
127
128
129
130
    
    /// Define all the standard accessors
    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(); }
131
132

  };
133
134
135
136
137
138
  
  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;
  };
139

140
141
142
143
  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct JointModelTpl
  : JointModelBase< JointModelTpl<_Scalar,_Options,JointCollectionTpl> >
  , JointCollectionTpl<_Scalar,_Options>::JointModelVariant
144
  {
jcarpent's avatar
jcarpent committed
145
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
146
    
147
148
149
150
151
152
    typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;

    SE3_JOINT_TYPEDEF_TEMPLATE;
    SE3_JOINT_USE_INDEXES;
    
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
153
154
155
    typedef typename JointCollection::JointDataVariant JointDataVariant;
    typedef typename JointCollection::JointModelVariant JointModelVariant;
    
156
157
158
    using Base::id;
    using Base::setIndexes;
    using Base::operator==;
159
    using Base::operator!=;
160

161
    JointModelTpl() : JointModelVariant() {}
162
163
164
    
    JointModelTpl(const JointModelVariant & jmodel_variant)
    : JointCollection::JointModelVariant(jmodel_variant)
jcarpent's avatar
jcarpent committed
165
166
    {}
    
167
168
    template<typename JointModelDerived>
    JointModelTpl(const JointModelBase<JointModelDerived> & jmodel)
169
    : JointModelVariant((JointModelVariant)jmodel.derived())
170
171
172
173
    {
      BOOST_MPL_ASSERT((boost::mpl::contains<typename JointModelVariant::types,JointModelDerived>));
    }
    
174
175
176
177
178
    JointModelVariant & toVariant()
    { return *static_cast<JointModelVariant*>(this); }
    
    const JointModelVariant & toVariant() const
    { return *static_cast<const JointModelVariant*>(this); }
179

180
    JointDataDerived createData() const
181
    { return ::se3::createData<Scalar,Options,JointCollectionTpl>(*this); }
182

183
184
185
186
187
188
189
    using Base::isEqual;
    bool isEqual(const JointModelTpl & other) const
    {
      return Base::isEqual(other)
      && toVariant() == other.toVariant();
    }

190
191
192
    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const Eigen::MatrixBase<ConfigVector> & q) const
193
    { calc_zero_order(*this,data,q); }
194

195
196
197
198
    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
199
    { calc_first_order(*this,data,q,v); }
200
    
201
202
203
    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
    { ::se3::calc_aba(*this,data,EIGEN_CONST_CAST(Matrix6Like,I),update_I); }
204
    
205
206
    std::string shortname() const { return ::se3::shortname(*this); }
    static std::string classname() { return "JointModel"; }
207

208
209
    int     nq_impl() const { return ::se3::nq(*this); }
    int     nv_impl() const { return ::se3::nv(*this); }
210

211
212
    int     idx_q()   const { return ::se3::idx_q(*this); }
    int     idx_v()   const { return ::se3::idx_v(*this); }
213

214
    JointIndex     id()      const { return ::se3::id(*this); }
215

216
217
    void setIndexes(JointIndex id, int nq, int nv)
    { ::se3::setIndexes(*this,id, nq, nv); }
218
219
220
221
222
223
224
    
    /// \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);
    }
225
226
  };
  
227
228
  typedef container::aligned_vector<JointData> JointDataVector;
  typedef container::aligned_vector<JointModel> JointModelVector;
229
230
231

} // namespace se3

232
#endif // ifndef __se3_joint_generic_hpp__