joint-revolute-unbounded-unaligned.hpp 7.43 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
//
// Copyright (c) 2019 INRIA
//

#ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__
#define __pinocchio_joint_revolute_unbounded_unaligned_hpp__

#include "pinocchio/fwd.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/math/rotation.hpp"
11
#include "pinocchio/math/matrix.hpp"
12
13
14
15
16
17

#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"

namespace pinocchio
{

18
  template<typename Scalar, int Options = 0> struct JointRevoluteUnboundedUnalignedTpl;
19
20
21
22
23
24
25
26
27
28
  
  template<typename _Scalar, int _Options>
  struct traits< JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 2,
      NV = 1
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
29
30
31
32
    
    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
    
33
34
35
36
37
    typedef JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDataDerived;
    typedef JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionRevoluteUnalignedTpl<Scalar,Options> Motion_t;
38
    typedef MotionZeroTpl<Scalar,Options> Bias_t;
39
    typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
40
    
41
42
43
44
45
    // [ABA]
    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;
    
46
    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
  };

  template<typename Scalar, int Options>
  struct traits< JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  { typedef JointRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDerived; };
  
  template<typename Scalar, int Options>
  struct traits <JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  { typedef JointRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataRevoluteUnboundedUnalignedTpl
  : public JointDataBase< JointDataRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived;
Justin Carpentier's avatar
Justin Carpentier committed
63
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
64
65
66
67
68
69
70
71
72
73
74
75
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
    
    Transformation_t M;
    Constraint_t S;
    Motion_t v;
    Bias_t c;
    
    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;
    
76
77
78
79
80
    JointDataRevoluteUnboundedUnalignedTpl()
    : U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
    
    template<typename Vector3Like>
    JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : M(1)
    , S(axis)
    , v(axis,(Scalar)NAN)
    , U(), Dinv(), UDinv()
    {}
    
    static std::string classname() { return std::string("JointDataRevoluteUnboundedUnalignedTpl"); }
    std::string shortname() const { return classname(); }
    
  }; // struct JointDataRevoluteUnboundedUnalignedTpl

  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl);
  
  template<typename _Scalar, int _Options>
  struct JointModelRevoluteUnboundedUnalignedTpl
  : public JointModelBase< JointModelRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived;
Justin Carpentier's avatar
Justin Carpentier committed
103
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    
    typedef JointModelBase<JointModelRevoluteUnboundedUnalignedTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
    
    JointModelRevoluteUnboundedUnalignedTpl() {}
    
    JointModelRevoluteUnboundedUnalignedTpl(const Scalar & x,
                                            const Scalar & y,
                                            const Scalar & z)
    : axis(x,y,z)
    {
      axis.normalize();
      assert(isUnitary(axis) && "Rotation axis is not unitary");
    }
    
    template<typename Vector3Like>
    JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : axis(axis)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
      assert(isUnitary(axis) && "Rotation axis is not unitary");
    }

    JointDataDerived createData() const { return JointDataDerived(axis); }
    
    using Base::isEqual;
    bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const
    {
      return Base::isEqual(other) && axis == other.axis;
    }
    
    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename ConfigVector::Scalar OtherScalar;
      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
      & q = qs.template segment<NQ>(idx_q());
      
      const OtherScalar & ca = q(0);
      const OtherScalar & sa = q(1);
      
      toRotationMatrix(axis,ca,sa,data.M.rotation());
    }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());
159
      data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
    }
    
    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
    {
      data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
      data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
      data.UDinv.noalias() = data.U * data.Dinv;
      
      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
    }
    
175
    static std::string classname() { return std::string("JointModelRevoluteUnboundedUnaligned"); }
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
    std::string shortname() const { return classname(); }
    
    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelRevoluteUnboundedUnalignedTpl<NewScalar,Options> cast() const
    {
      typedef JointModelRevoluteUnboundedUnalignedTpl<NewScalar,Options> ReturnType;
      ReturnType res(axis.template cast<NewScalar>());
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

    // data
    ///
    /// \brief 3d main axis of the joint.
    ///
    Vector3 axis;
  }; // struct JointModelRevoluteUnboundedUnalignedTpl

} //namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
  
  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
}


#endif // ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__