pinocchio  2.2.1-dirty
joint-revolute-unbounded.hpp
1 //
2 // Copyright (c) 2016-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_joint_revolute_unbounded_hpp__
6 #define __pinocchio_joint_revolute_unbounded_hpp__
7 
8 #include "pinocchio/math/fwd.hpp"
9 #include "pinocchio/math/sincos.hpp"
10 #include "pinocchio/spatial/inertia.hpp"
11 #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  template<typename _Scalar, int _Options, int axis>
20  struct traits< JointRevoluteUnboundedTpl<_Scalar,_Options,axis> >
21  {
22  enum {
23  NQ = 2,
24  NV = 1
25  };
26  typedef _Scalar Scalar;
27  enum { Options = _Options };
34 
35  // [ABA]
36  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
37  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
38  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
39 
40  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
41 
42  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
43  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
44  };
45 
46  template<typename Scalar, int Options, int axis>
47  struct traits< JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
49 
50  template<typename Scalar, int Options, int axis>
51  struct traits< JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
53 
54  template<typename _Scalar, int _Options, int axis>
55  struct JointDataRevoluteUnboundedTpl : public JointDataBase< JointDataRevoluteUnboundedTpl<_Scalar,_Options,axis> >
56  {
57  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
60  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
61 
62  Constraint_t S;
63  Transformation_t M;
64  Motion_t v;
65  Bias_t c;
66 
67  // [ABA] specific data
68  U_t U;
69  D_t Dinv;
70  UD_t UDinv;
71 
73 
74  static std::string classname() { return std::string("JointDataRevoluteUnbounded"); }
75  std::string shortname() const { return classname(); }
76 
77  }; // struct JointDataRevoluteUnbounded
78 
79  template<typename NewScalar, typename Scalar, int Options, int axis>
80  struct CastType< NewScalar, JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
81  {
83  };
84 
85  template<typename _Scalar, int _Options, int axis>
87  : public JointModelBase< JointModelRevoluteUnboundedTpl<_Scalar,_Options,axis> >
88  {
89  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
91  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
92  typedef JointRevoluteTpl<Scalar,_Options,axis> JointDerivedBase;
93 
95  using Base::id;
96  using Base::idx_q;
97  using Base::idx_v;
98  using Base::setIndexes;
99 
100  JointDataDerived createData() const { return JointDataDerived(); }
101 
102  template<typename ConfigVector>
103  void calc(JointDataDerived & data,
104  const typename Eigen::MatrixBase<ConfigVector> & qs) const
105  {
106  typedef typename ConfigVector::Scalar OtherScalar;
107  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
108  & q = qs.template segment<NQ> (idx_q());
109 
110  const OtherScalar & ca = q(0);
111  const OtherScalar & sa = q(1);
112 
113  data.M.setValues(sa,ca);
114  }
115 
116  template<typename ConfigVector, typename TangentVector>
117  void calc(JointDataDerived & data,
118  const typename Eigen::MatrixBase<ConfigVector> & qs,
119  const typename Eigen::MatrixBase<TangentVector> & vs) const
120  {
121  calc(data,qs.derived());
122 
123  data.v.w = (Scalar)vs[idx_v()];
124  }
125 
126  template<typename Matrix6Like>
127  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)
134  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
135  }
136 
137  static std::string classname()
138  {
139  return std::string("JointModelRUB") + axisLabel<axis>();
140  }
141  std::string shortname() const { return classname(); }
142 
144  template<typename NewScalar>
146  {
148  ReturnType res;
149  res.setIndexes(id(),idx_q(),idx_v());
150  return res;
151  }
152 
153  }; // struct JointModelRevoluteUnboundedTpl
154 
156  {
157  template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
158  static void run(const Eigen::MatrixBase<ConfigVectorIn> & q,
159  const Scalar & scaling,
160  const Scalar & offset,
161  const Eigen::MatrixBase<ConfigVectorOut> & dest)
162  {
163  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn,2);
164  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut,2);
165 
166  const typename ConfigVectorIn::Scalar & ca = q(0);
167  const typename ConfigVectorIn::Scalar & sa = q(1);
168 
169  const typename ConfigVectorIn::Scalar & theta = math::atan2(sa,ca);
170  const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset;
171 
172  ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,dest);
173  SINCOS(theta_transform,&dest_.coeffRef(1),&dest_.coeffRef(0));
174  }
175  };
176 
177  template<typename Scalar, int Options, int axis>
179  {
181  };
182 
186 
190 
194 
195 } //namespace pinocchio
196 
197 #include <boost/type_traits.hpp>
198 
199 namespace boost
200 {
201  template<typename Scalar, int Options, int axis>
202  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
203  : public integral_constant<bool,true> {};
204 
205  template<typename Scalar, int Options, int axis>
206  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
207  : public integral_constant<bool,true> {};
208 
209  template<typename Scalar, int Options, int axis>
210  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
211  : public integral_constant<bool,true> {};
212 
213  template<typename Scalar, int Options, int axis>
214  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
215  : public integral_constant<bool,true> {};
216 }
217 
218 #endif // ifndef __pinocchio_joint_revolute_unbounded_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.
Assign the correct configuration vector space affine transformation according to the joint type...
void SINCOS(const Scalar &a, Scalar *sa, Scalar *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointModelRevoluteUnboundedTpl< NewScalar, Options, axis > cast() const
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: spatial/fwd.hpp:43
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...