joint-revolute-unbounded.hpp 7.86 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
76
    static std::string classname() { return std::string("JointDataRevoluteUnbounded"); }
    std::string shortname() const { return classname(); }
    
77
  }; // struct JointDataRevoluteUnbounded
78
79
80
81
82
83
  
  template<typename NewScalar, typename Scalar, int Options, int axis>
  struct CastType< NewScalar, JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
  {
    typedef JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> type;
  };
84

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

110
111
      const OtherScalar & ca = q(0);
      const OtherScalar & sa = q(1);
112

113
      data.M.setValues(sa,ca);
114
115
    }

116
117
118
119
    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
120
    {
121
122
      calc(data,qs.derived());
      
Qiang Qiu's avatar
Qiang Qiu committed
123
      data.v.w = (Scalar)vs[idx_v()];
124
125
    }
    
126
127
    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
128
129
    {
      data.U = I.col(Inertia::ANGULAR + axis);
130
      data.Dinv[0] = (Scalar)(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
131
      data.UDinv.noalias() = data.U * data.Dinv[0];
132
133
      
      if (update_I)
gabrielebndn's avatar
gabrielebndn committed
134
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
135
    }
136
    
137
138
139
140
    static std::string classname()
    {
      return std::string("JointModelRUB") + axisLabel<axis>();
    }
141
    std::string shortname() const { return classname(); }
142
143
144
145
146
147
148
149
150
151
    
    /// \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;
    }
152

153
154
  }; // struct JointModelRevoluteUnboundedTpl
  
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
  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;
  };
  
183
184
185
  typedef JointRevoluteUnboundedTpl<double,0,0> JointRUBX;
  typedef JointDataRevoluteUnboundedTpl<double,0,0> JointDataRUBX;
  typedef JointModelRevoluteUnboundedTpl<double,0,0> JointModelRUBX;
186

187
188
189
  typedef JointRevoluteUnboundedTpl<double,0,1> JointRUBY;
  typedef JointDataRevoluteUnboundedTpl<double,0,1> JointDataRUBY;
  typedef JointModelRevoluteUnboundedTpl<double,0,1> JointModelRUBY;
190

191
192
193
  typedef JointRevoluteUnboundedTpl<double,0,2> JointRUBZ;
  typedef JointDataRevoluteUnboundedTpl<double,0,2> JointDataRUBZ;
  typedef JointModelRevoluteUnboundedTpl<double,0,2> JointModelRUBZ;
194

195
} //namespace pinocchio
196

197
198
199
200
201
#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options, int axis>
202
  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
203
204
205
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
206
  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
207
208
209
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
210
  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
211
212
213
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options, int axis>
214
  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
215
216
217
  : public integral_constant<bool,true> {};
}

218
#endif // ifndef __pinocchio_joint_revolute_unbounded_hpp__