joint-revolute-unbounded.hpp 8.02 KB
Newer Older
1
//
2
// Copyright (c) 2016-2019 CNRS INRIA
3
4
//

5
6
#ifndef __pinocchio_joint_revolute_unbounded_hpp__
#define __pinocchio_joint_revolute_unbounded_hpp__
7

Joseph Mirabel's avatar
Joseph Mirabel committed
8
#include "pinocchio/math/fwd.hpp"
9
10
11
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
12
#include "pinocchio/multibody/joint/joint-revolute.hpp"
13

14
namespace pinocchio
15
16
{

17
  template<typename Scalar, int Options, int axis> struct JointRevoluteUnboundedTpl;
18
  
19
20
  template<typename _Scalar, int _Options, int axis>
  struct traits< JointRevoluteUnboundedTpl<_Scalar,_Options,axis> >
21
22
23
24
25
  {
    enum {
      NQ = 2,
      NV = 1
    };
26
27
28
29
30
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataRevoluteUnboundedTpl<Scalar,Options,axis> JointDataDerived;
    typedef JointModelRevoluteUnboundedTpl<Scalar,Options,axis> JointModelDerived;
    typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint_t;
31
    typedef TransformRevoluteTpl<Scalar,Options,axis> Transformation_t;
32
    typedef MotionRevoluteTpl<Scalar,Options,axis> Motion_t;
33
    typedef MotionZeroTpl<Scalar,Options> Bias_t;
Justin Carpentier's avatar
Justin Carpentier committed
34

35
    // [ABA]
36
37
38
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
39
    
Gabriele Buondonno's avatar
Gabriele Buondonno committed
40
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
41

42
43
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
44
45
  };

46
47
48
49
50
51
52
  template<typename Scalar, int Options, int axis>
  struct traits< JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
  { typedef JointRevoluteUnboundedTpl<Scalar,Options,axis> JointDerived; };
  
  template<typename Scalar, int Options, int axis>
  struct traits< JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
  { typedef JointRevoluteUnboundedTpl<Scalar,Options,axis> JointDerived; };
53

54
55
  template<typename _Scalar, int _Options, int axis>
  struct JointDataRevoluteUnboundedTpl : public JointDataBase< JointDataRevoluteUnboundedTpl<_Scalar,_Options,axis> >
56
  {
57
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58
    typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> JointDerived;
59
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
Gabriele Buondonno's avatar
Gabriele Buondonno committed
60
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
61
62
63
64
65
66
67
68
69
70
71

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

72
    JointDataRevoluteUnboundedTpl()
73
74
75
    : M((Scalar)0,(Scalar)1)
    , v((Scalar)0)
    , U(U_t::Zero())
76
77
78
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}
79

80
81
82
83
    static std::string classname()
    {
      return std::string("JointDataRUB") + axisLabel<axis>();
    }
84
85
    std::string shortname() const { return classname(); }
    
86
  }; // struct JointDataRevoluteUnbounded
87
88
89
90
91
92
  
  template<typename NewScalar, typename Scalar, int Options, int axis>
  struct CastType< NewScalar, JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
  {
    typedef JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> type;
  };
93

94
  template<typename _Scalar, int _Options, int axis>
jcarpent's avatar
jcarpent committed
95
96
  struct JointModelRevoluteUnboundedTpl
  : public JointModelBase< JointModelRevoluteUnboundedTpl<_Scalar,_Options,axis> >
97
  {
98
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
99
    typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> JointDerived;
100
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
101
102
    typedef JointRevoluteTpl<Scalar,_Options,axis> JointDerivedBase;
    
jcarpent's avatar
jcarpent committed
103
104
105
106
107
    typedef JointModelBase<JointModelRevoluteUnboundedTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
108
109
    
    JointDataDerived createData() const { return JointDataDerived(); }
110
111
112
113
    
    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
114
    {
115
116
117
      typedef typename ConfigVector::Scalar OtherScalar;
      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
      & q = qs.template segment<NQ> (idx_q());
118

119
120
      const OtherScalar & ca = q(0);
      const OtherScalar & sa = q(1);
121

122
      data.M.setValues(sa,ca);
123
124
    }

125
126
127
128
    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
129
    {
130
131
      calc(data,qs.derived());
      
132
      data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
133
134
    }
    
135
136
    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
137
138
    {
      data.U = I.col(Inertia::ANGULAR + axis);
139
      data.Dinv[0] = (Scalar)(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
140
      data.UDinv.noalias() = data.U * data.Dinv[0];
141
142
      
      if (update_I)
gabrielebndn's avatar
gabrielebndn committed
143
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
144
    }
145
    
146
147
148
149
    static std::string classname()
    {
      return std::string("JointModelRUB") + axisLabel<axis>();
    }
150
    std::string shortname() const { return classname(); }
151
152
153
154
155
156
157
158
159
160
    
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> cast() const
    {
      typedef JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }
161

162
163
  }; // struct JointModelRevoluteUnboundedTpl
  
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
  struct UnboundedRevoluteAffineTransform
  {
    template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
    static void run(const Eigen::MatrixBase<ConfigVectorIn> & q,
                    const Scalar & scaling,
                    const Scalar & offset,
                    const Eigen::MatrixBase<ConfigVectorOut> & dest)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn,2);
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut,2);
      
      const typename ConfigVectorIn::Scalar & ca = q(0);
      const typename ConfigVectorIn::Scalar & sa = q(1);
      
      const typename ConfigVectorIn::Scalar & theta = math::atan2(sa,ca);
      const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset;
      
      ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,dest);
      SINCOS(theta_transform,&dest_.coeffRef(1),&dest_.coeffRef(0));
    }
  };
  
  template<typename Scalar, int Options, int axis>
  struct ConfigVectorAffineTransform< JointRevoluteUnboundedTpl<Scalar,Options,axis> >
  {
    typedef UnboundedRevoluteAffineTransform Type;
  };
  
192
193
194
  typedef JointRevoluteUnboundedTpl<double,0,0> JointRUBX;
  typedef JointDataRevoluteUnboundedTpl<double,0,0> JointDataRUBX;
  typedef JointModelRevoluteUnboundedTpl<double,0,0> JointModelRUBX;
195

196
197
198
  typedef JointRevoluteUnboundedTpl<double,0,1> JointRUBY;
  typedef JointDataRevoluteUnboundedTpl<double,0,1> JointDataRUBY;
  typedef JointModelRevoluteUnboundedTpl<double,0,1> JointModelRUBY;
199

200
201
202
  typedef JointRevoluteUnboundedTpl<double,0,2> JointRUBZ;
  typedef JointDataRevoluteUnboundedTpl<double,0,2> JointDataRUBZ;
  typedef JointModelRevoluteUnboundedTpl<double,0,2> JointModelRUBZ;
203

204
} //namespace pinocchio
205

206
207
208
209
210
#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options, int axis>
211
  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
212
213
214
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
215
  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
216
217
218
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
219
  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
220
221
222
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
223
  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
224
225
226
  : public integral_constant<bool,true> {};
}

227
#endif // ifndef __pinocchio_joint_revolute_unbounded_hpp__