pinocchio  2.2.1-dirty
joint-revolute-unbounded-unaligned.hpp
1 //
2 // Copyright (c) 2019 INRIA
3 //
4 
5 #ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__
6 #define __pinocchio_joint_revolute_unbounded_unaligned_hpp__
7 
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/spatial/inertia.hpp"
10 #include "pinocchio/math/rotation.hpp"
11 #include "pinocchio/math/matrix.hpp"
12 
13 #include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
14 
15 namespace pinocchio
16 {
17 
18  template<typename Scalar, int Options = 0> struct JointRevoluteUnboundedUnalignedTpl;
19 
20  template<typename _Scalar, int _Options>
21  struct traits< JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
22  {
23  enum {
24  NQ = 2,
25  NV = 1
26  };
27  typedef _Scalar Scalar;
28  enum { Options = _Options };
29 
30  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
31  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
32 
39  typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
40 
41  // [ABA]
42  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
43  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
44  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
45 
46  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
47  };
48 
49  template<typename Scalar, int Options>
52 
53  template<typename Scalar, int Options>
56 
57  template<typename _Scalar, int _Options>
59  : public JointDataBase< JointDataRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
60  {
61  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
64  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
65 
66  Transformation_t M;
67  Constraint_t S;
68  Motion_t v;
69  Bias_t c;
70 
71  // [ABA] specific data
72  U_t U;
73  D_t Dinv;
74  UD_t UDinv;
75 
77 
78  template<typename Vector3Like>
79  JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
80  : M(1)
81  , S(axis)
82  , v(axis,(Scalar)NAN)
83  , U(), Dinv(), UDinv()
84  {}
85 
86  static std::string classname() { return std::string("JointDataRevoluteUnboundedUnalignedTpl"); }
87  std::string shortname() const { return classname(); }
88 
89  }; // struct JointDataRevoluteUnboundedUnalignedTpl
90 
91  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl);
92 
93  template<typename _Scalar, int _Options>
95  : public JointModelBase< JointModelRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
96  {
97  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
99  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
100  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
101 
103  using Base::id;
104  using Base::idx_q;
105  using Base::idx_v;
106  using Base::setIndexes;
107 
109 
111  const Scalar & y,
112  const Scalar & z)
113  : axis(x,y,z)
114  {
115  axis.normalize();
116  assert(isUnitary(axis) && "Rotation axis is not unitary");
117  }
118 
119  template<typename Vector3Like>
120  JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
121  : axis(axis)
122  {
123  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
124  assert(isUnitary(axis) && "Rotation axis is not unitary");
125  }
126 
127  JointDataDerived createData() const { return JointDataDerived(axis); }
128 
129  using Base::isEqual;
130  bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const
131  {
132  return Base::isEqual(other) && axis == other.axis;
133  }
134 
135  template<typename ConfigVector>
136  void calc(JointDataDerived & data,
137  const typename Eigen::MatrixBase<ConfigVector> & qs) const
138  {
139  typedef typename ConfigVector::Scalar OtherScalar;
140  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
141  & q = qs.template segment<NQ>(idx_q());
142 
143  const OtherScalar & ca = q(0);
144  const OtherScalar & sa = q(1);
145 
146  toRotationMatrix(axis,ca,sa,data.M.rotation());
147  }
148 
149  template<typename ConfigVector, typename TangentVector>
150  void calc(JointDataDerived & data,
151  const typename Eigen::MatrixBase<ConfigVector> & qs,
152  const typename Eigen::MatrixBase<TangentVector> & vs) const
153  {
154  calc(data,qs.derived());
155  data.v.w = (Scalar)vs[idx_v()];
156  }
157 
158  template<typename Matrix6Like>
159  void calc_aba(JointDataDerived & data,
160  const Eigen::MatrixBase<Matrix6Like> & I,
161  const bool update_I) const
162  {
163  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
164  data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
165  data.UDinv.noalias() = data.U * data.Dinv;
166 
167  if (update_I)
168  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
169  }
170 
171  static std::string classname() { return std::string("JointModelRevoluteUnboundedUnaligned"); }
172  std::string shortname() const { return classname(); }
173 
175  template<typename NewScalar>
177  {
179  ReturnType res(axis.template cast<NewScalar>());
180  res.setIndexes(id(),idx_q(),idx_v());
181  return res;
182  }
183 
184  // data
188  Vector3 axis;
189  }; // struct JointModelRevoluteUnboundedUnalignedTpl
190 
191 } //namespace pinocchio
192 
193 #include <boost/type_traits.hpp>
194 
195 namespace boost
196 {
197  template<typename Scalar, int Options>
198  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
199  : public integral_constant<bool,true> {};
200 
201  template<typename Scalar, int Options>
202  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
203  : public integral_constant<bool,true> {};
204 
205  template<typename Scalar, int Options>
206  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
207  : public integral_constant<bool,true> {};
208 
209  template<typename Scalar, int Options>
210  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
211  : public integral_constant<bool,true> {};
212 }
213 
214 
215 #endif // ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model tangent space corre...
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
JointModelRevoluteUnboundedUnalignedTpl< NewScalar, Options > cast() const
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
Definition: rotation.hpp:21
void calc_aba(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcAbaVisitor to...