pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
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 };
28  typedef JointDataRevoluteUnboundedTpl<Scalar,Options,axis> JointDataDerived;
29  typedef JointModelRevoluteUnboundedTpl<Scalar,Options,axis> JointModelDerived;
30  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;
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> >
48  { typedef JointRevoluteUnboundedTpl<Scalar,Options,axis> JointDerived; };
49 
50  template<typename Scalar, int Options, int axis>
51  struct traits< JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
52  { typedef JointRevoluteUnboundedTpl<Scalar,Options,axis> JointDerived; };
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
58  typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> JointDerived;
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 
72  JointDataRevoluteUnboundedTpl()
73  : M((Scalar)0,(Scalar)1)
74  , v((Scalar)0)
75  , U(U_t::Zero())
76  , Dinv(D_t::Zero())
77  , UDinv(UD_t::Zero())
78  {}
79 
80  static std::string classname()
81  {
82  return std::string("JointDataRUB") + axisLabel<axis>();
83  }
84  std::string shortname() const { return classname(); }
85 
86  }; // struct JointDataRevoluteUnbounded
87 
88  template<typename NewScalar, typename Scalar, int Options, int axis>
89  struct CastType< NewScalar, JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
90  {
91  typedef JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> type;
92  };
93 
94  template<typename _Scalar, int _Options, int axis>
95  struct JointModelRevoluteUnboundedTpl
96  : 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  typedef JointRevoluteTpl<Scalar,_Options,axis> JointDerivedBase;
102 
103  typedef JointModelBase<JointModelRevoluteUnboundedTpl> Base;
104  using Base::id;
105  using Base::idx_q;
106  using Base::idx_v;
107  using Base::setIndexes;
108 
109  JointDataDerived createData() const { return JointDataDerived(); }
110 
111  const std::vector<bool> hasConfigurationLimit() const
112  {
113  return {false, false};
114  }
115 
116  const std::vector<bool> hasConfigurationLimitInTangent() const
117  {
118  return {false};
119  }
120 
121  template<typename ConfigVector>
122  void calc(JointDataDerived & data,
123  const typename Eigen::MatrixBase<ConfigVector> & qs) const
124  {
125  typedef typename ConfigVector::Scalar OtherScalar;
126  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
127  & q = qs.template segment<NQ> (idx_q());
128 
129  const OtherScalar & ca = q(0);
130  const OtherScalar & sa = q(1);
131 
132  data.M.setValues(sa,ca);
133  }
134 
135  template<typename ConfigVector, typename TangentVector>
136  void calc(JointDataDerived & data,
137  const typename Eigen::MatrixBase<ConfigVector> & qs,
138  const typename Eigen::MatrixBase<TangentVector> & vs) const
139  {
140  calc(data,qs.derived());
141 
142  data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
143  }
144 
145  template<typename Matrix6Like>
146  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
147  {
148  data.U = I.col(Inertia::ANGULAR + axis);
149  data.Dinv[0] = (Scalar)(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
150  data.UDinv.noalias() = data.U * data.Dinv[0];
151 
152  if (update_I)
153  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
154  }
155 
156  static std::string classname()
157  {
158  return std::string("JointModelRUB") + axisLabel<axis>();
159  }
160  std::string shortname() const { return classname(); }
161 
163  template<typename NewScalar>
164  JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> cast() const
165  {
166  typedef JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> ReturnType;
167  ReturnType res;
168  res.setIndexes(id(),idx_q(),idx_v());
169  return res;
170  }
171 
172  }; // struct JointModelRevoluteUnboundedTpl
173 
174  struct UnboundedRevoluteAffineTransform
175  {
176  template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
177  static void run(const Eigen::MatrixBase<ConfigVectorIn> & q,
178  const Scalar & scaling,
179  const Scalar & offset,
180  const Eigen::MatrixBase<ConfigVectorOut> & dest)
181  {
182  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn,2);
183  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut,2);
184 
185  const typename ConfigVectorIn::Scalar & ca = q(0);
186  const typename ConfigVectorIn::Scalar & sa = q(1);
187 
188  const typename ConfigVectorIn::Scalar & theta = math::atan2(sa,ca);
189  const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset;
190 
191  ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,dest);
192  SINCOS(theta_transform,&dest_.coeffRef(1),&dest_.coeffRef(0));
193  }
194  };
195 
196  template<typename Scalar, int Options, int axis>
197  struct ConfigVectorAffineTransform< JointRevoluteUnboundedTpl<Scalar,Options,axis> >
198  {
199  typedef UnboundedRevoluteAffineTransform Type;
200  };
201 
202  typedef JointRevoluteUnboundedTpl<double,0,0> JointRUBX;
203  typedef JointDataRevoluteUnboundedTpl<double,0,0> JointDataRUBX;
204  typedef JointModelRevoluteUnboundedTpl<double,0,0> JointModelRUBX;
205 
206  typedef JointRevoluteUnboundedTpl<double,0,1> JointRUBY;
207  typedef JointDataRevoluteUnboundedTpl<double,0,1> JointDataRUBY;
208  typedef JointModelRevoluteUnboundedTpl<double,0,1> JointModelRUBY;
209 
210  typedef JointRevoluteUnboundedTpl<double,0,2> JointRUBZ;
211  typedef JointDataRevoluteUnboundedTpl<double,0,2> JointDataRUBZ;
212  typedef JointModelRevoluteUnboundedTpl<double,0,2> JointModelRUBZ;
213 
214 } //namespace pinocchio
215 
216 #include <boost/type_traits.hpp>
217 
218 namespace boost
219 {
220  template<typename Scalar, int Options, int axis>
221  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
222  : public integral_constant<bool,true> {};
223 
224  template<typename Scalar, int Options, int axis>
225  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
226  : public integral_constant<bool,true> {};
227 
228  template<typename Scalar, int Options, int axis>
229  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
230  : public integral_constant<bool,true> {};
231 
232  template<typename Scalar, int Options, int axis>
233  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
234  : public integral_constant<bool,true> {};
235 }
236 
237 #endif // ifndef __pinocchio_joint_revolute_unbounded_hpp__
pinocchio::JointModelRevoluteUnboundedTpl::cast
JointModelRevoluteUnboundedTpl< NewScalar, Options, axis > cast() const
Definition: joint-revolute-unbounded.hpp:164
pinocchio::MotionRevoluteTpl
Definition: joint-revolute.hpp:19
pinocchio::idx_q
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
pinocchio::JointModelBase
Definition: joint-model-base.hpp:67
pinocchio::idx_v
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...
pinocchio::JointDataBase
Definition: joint-data-base.hpp:82
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
pinocchio::JointRevoluteTpl
Definition: joint-revolute.hpp:460
pinocchio::MotionZeroTpl
Definition: fwd.hpp:44
pinocchio::ConstraintRevoluteTpl
Definition: joint-revolute.hpp:332
pinocchio::TransformRevoluteTpl
Definition: joint-revolute.hpp:58
pinocchio::ConfigVectorAffineTransform
Assign the correct configuration vector space affine transformation according to the joint type.
Definition: joint-common-operations.hpp:69
pinocchio::JointRevoluteUnboundedTpl
Definition: joint-revolute-unbounded.hpp:17
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::UnboundedRevoluteAffineTransform
Definition: joint-revolute-unbounded.hpp:174
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:55
pinocchio::JointModelRevoluteUnboundedTpl
Definition: fwd.hpp:39
pinocchio::JointDataRevoluteUnboundedTpl
Definition: fwd.hpp:40
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11