pinocchio  3.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-revolute-unbounded.hpp
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_joint_revolute_unbounded_hpp__
6 #define __pinocchio_multibody_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>
19 
20  template<typename _Scalar, int _Options, int axis>
21  struct traits<JointRevoluteUnboundedTpl<_Scalar, _Options, axis>>
22  {
23  enum
24  {
25  NQ = 2,
26  NV = 1,
27  NVExtended = 1
28  };
29  typedef _Scalar Scalar;
30  enum
31  {
32  Options = _Options
33  };
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  typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
47  typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
48 
49  typedef boost::mpl::true_ is_mimicable_t;
50 
51  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
52  };
53 
54  template<typename _Scalar, int _Options, int axis>
55  struct traits<JointDataRevoluteUnboundedTpl<_Scalar, _Options, axis>>
56  {
58  typedef _Scalar Scalar;
59  };
60 
61  template<typename _Scalar, int _Options, int axis>
62  struct traits<JointModelRevoluteUnboundedTpl<_Scalar, _Options, axis>>
63  {
65  typedef _Scalar Scalar;
66  };
67 
68  template<typename _Scalar, int _Options, int axis>
70  : public JointDataBase<JointDataRevoluteUnboundedTpl<_Scalar, _Options, axis>>
71  {
72  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
74  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
75  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
76 
77  ConfigVector_t joint_q;
78  TangentVector_t joint_v;
79 
80  Constraint_t S;
81  Transformation_t M;
82  Motion_t v;
83  Bias_t c;
84 
85  // [ABA] specific data
86  U_t U;
87  D_t Dinv;
88  UD_t UDinv;
89  D_t StU;
90 
92  : joint_q(Scalar(1), Scalar(0))
93  , joint_v(TangentVector_t::Zero())
94  , M((Scalar)0, (Scalar)1)
95  , v((Scalar)0)
96  , U(U_t::Zero())
97  , Dinv(D_t::Zero())
98  , UDinv(UD_t::Zero())
99  , StU(D_t::Zero())
100  {
101  }
102 
103  static std::string classname()
104  {
105  return std::string("JointDataRUB") + axisLabel<axis>();
106  }
107  std::string shortname() const
108  {
109  return classname();
110  }
111 
112  }; // struct JointDataRevoluteUnbounded
113 
114  template<typename NewScalar, typename Scalar, int Options, int axis>
115  struct CastType<NewScalar, JointModelRevoluteUnboundedTpl<Scalar, Options, axis>>
116  {
118  };
119 
120  template<typename _Scalar, int _Options, int axis>
122  : public JointModelBase<JointModelRevoluteUnboundedTpl<_Scalar, _Options, axis>>
123  {
124  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
126  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
128 
130  using Base::id;
131  using Base::idx_q;
132  using Base::idx_v;
133  using Base::idx_vExtended;
134  using Base::setIndexes;
135 
136  typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
137 
138  JointDataDerived createData() const
139  {
140  return JointDataDerived();
141  }
142 
143  const std::vector<bool> hasConfigurationLimit() const
144  {
145  return {false, false};
146  }
147 
148  const std::vector<bool> hasConfigurationLimitInTangent() const
149  {
150  return {false};
151  }
152 
153  template<typename ConfigVector>
154  void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
155  {
156  data.joint_q = qs.template segment<NQ>(idx_q());
157 
158  const Scalar & ca = data.joint_q[0];
159  const Scalar & sa = data.joint_q[1];
160 
161  data.M.setValues(sa, ca);
162  }
163 
164  template<typename TangentVector>
165  void
166  calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
167  const
168  {
169  data.joint_v[0] = vs[idx_v()];
170  data.v.angularRate() = data.joint_v[0];
171  }
172 
173  template<typename ConfigVector, typename TangentVector>
174  void calc(
175  JointDataDerived & data,
176  const typename Eigen::MatrixBase<ConfigVector> & qs,
177  const typename Eigen::MatrixBase<TangentVector> & vs) const
178  {
179  calc(data, qs.derived());
180  data.joint_v[0] = vs[idx_v()];
181  data.v.angularRate() = data.joint_v[0];
182  }
183 
184  template<typename VectorLike, typename Matrix6Like>
185  void calc_aba(
186  JointDataDerived & data,
187  const Eigen::MatrixBase<VectorLike> & armature,
188  const Eigen::MatrixBase<Matrix6Like> & I,
189  const bool update_I) const
190  {
191  data.U = I.col(Inertia::ANGULAR + axis);
192  data.Dinv[0] =
193  (Scalar)(1) / (I(Inertia::ANGULAR + axis, Inertia::ANGULAR + axis) + armature[0]);
194  data.UDinv.noalias() = data.U * data.Dinv[0];
195 
196  if (update_I)
197  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
198  }
199 
200  static std::string classname()
201  {
202  return std::string("JointModelRUB") + axisLabel<axis>();
203  }
204  std::string shortname() const
205  {
206  return classname();
207  }
208 
209  Vector3 getMotionAxis() const
210  {
211  switch (axis)
212  {
213  case 0:
214  return Vector3::UnitX();
215  case 1:
216  return Vector3::UnitY();
217  case 2:
218  return Vector3::UnitZ();
219  default:
220  assert(false && "must never happen");
221  break;
222  }
223  }
224 
226  template<typename NewScalar>
228  {
230  ReturnType res;
231  res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
232  return res;
233  }
234 
235  }; // struct JointModelRevoluteUnboundedTpl
236 
237  typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointRUBX;
238  typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointDataRUBX;
239  typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointModelRUBX;
240 
241  typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointRUBY;
242  typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointDataRUBY;
243  typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointModelRUBY;
244 
245  typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointRUBZ;
246  typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointDataRUBZ;
247  typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointModelRUBZ;
248 
249  template<typename Scalar, int Options, int axis>
251  {
253  };
254 } // namespace pinocchio
255 
256 #include <boost/type_traits.hpp>
257 
258 namespace boost
259 {
260  template<typename Scalar, int Options, int axis>
261  struct has_nothrow_constructor<::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, axis>>
262  : public integral_constant<bool, true>
263  {
264  };
265 
266  template<typename Scalar, int Options, int axis>
267  struct has_nothrow_copy<::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, axis>>
268  : public integral_constant<bool, true>
269  {
270  };
271 
272  template<typename Scalar, int Options, int axis>
273  struct has_nothrow_constructor<::pinocchio::JointDataRevoluteUnboundedTpl<Scalar, Options, axis>>
274  : public integral_constant<bool, true>
275  {
276  };
277 
278  template<typename Scalar, int Options, int axis>
279  struct has_nothrow_copy<::pinocchio::JointDataRevoluteUnboundedTpl<Scalar, Options, axis>>
280  : public integral_constant<bool, true>
281  {
282  };
283 } // namespace boost
284 
285 #endif // ifndef __pinocchio_multibody_joint_revolute_unbounded_hpp__
Main pinocchio namespace.
Definition: treeview.dox:11
JointDataTpl< Scalar, Options, JointCollectionTpl >::TangentVector_t joint_v(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint velocity vector.
JointDataTpl< Scalar, Options, JointCollectionTpl >::ConfigVector_t joint_q(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint configuration vector.
Blank type.
Definition: fwd.hpp:77
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:99
Assign the correct configuration vector space affine transformation according to the joint type....
JointModelRevoluteUnboundedTpl< NewScalar, Options, axis > cast() const
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72