pinocchio  3.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-revolute-unbounded-unaligned.hpp
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__
6 #define __pinocchio_multibody_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>
20 
21  template<typename _Scalar, int _Options>
22  struct traits<JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
23  {
24  enum
25  {
26  NQ = 2,
27  NV = 1,
28  NVExtended = 1
29  };
30  typedef _Scalar Scalar;
31  enum
32  {
33  Options = _Options
34  };
35 
36  typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
37  typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
38 
45  typedef Eigen::Matrix<Scalar, 6, NV, Options> F_t;
46 
47  // [ABA]
48  typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
49  typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
50  typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
51 
52  typedef boost::mpl::true_ is_mimicable_t;
53 
54  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
55  };
56 
57  template<typename _Scalar, int _Options>
59  {
61  typedef _Scalar Scalar;
62  };
63 
64  template<typename _Scalar, int _Options>
66  {
68  typedef _Scalar Scalar;
69  };
70 
71  template<typename _Scalar, int _Options>
73  : public JointDataBase<JointDataRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
74  {
75  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
78  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
79 
80  ConfigVector_t joint_q;
81  TangentVector_t joint_v;
82 
83  Transformation_t M;
84  Constraint_t S;
85  Motion_t v;
86  Bias_t c;
87 
88  // [ABA] specific data
89  U_t U;
90  D_t Dinv;
91  UD_t UDinv;
92  D_t StU;
93 
95  : joint_q(Scalar(1), Scalar(0))
96  , joint_v(TangentVector_t::Zero())
97  , M(Transformation_t::Identity())
98  , S(Constraint_t::Vector3::Zero())
99  , v(Constraint_t::Vector3::Zero(), (Scalar)0)
100  , U(U_t::Zero())
101  , Dinv(D_t::Zero())
102  , UDinv(UD_t::Zero())
103  , StU(D_t::Zero())
104  {
105  }
106 
107  template<typename Vector3Like>
108  JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
109  : joint_q(Scalar(1), Scalar(0))
110  , joint_v(TangentVector_t::Zero())
111  , M(Transformation_t::Identity())
112  , S(axis)
113  , v(axis, (Scalar)0)
114  , U(U_t::Zero())
115  , Dinv(D_t::Zero())
116  , UDinv(UD_t::Zero())
117  , StU(D_t::Zero())
118  {
119  }
120 
121  static std::string classname()
122  {
123  return std::string("JointDataRevoluteUnboundedUnalignedTpl");
124  }
125  std::string shortname() const
126  {
127  return classname();
128  }
129 
130  }; // struct JointDataRevoluteUnboundedUnalignedTpl
131 
132  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl);
133 
134  template<typename _Scalar, int _Options>
136  : public JointModelBase<JointModelRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
137  {
138  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
140  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
141  typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
142 
144  using Base::id;
145  using Base::idx_q;
146  using Base::idx_v;
147  using Base::idx_vExtended;
148  using Base::setIndexes;
149 
151  : axis(Vector3::UnitX())
152  {
153  }
154 
155  JointModelRevoluteUnboundedUnalignedTpl(const Scalar & x, const Scalar & y, const Scalar & z)
156  : axis(x, y, z)
157  {
158  normalize(axis);
159  assert(isUnitary(axis) && "Rotation axis is not unitary");
160  }
161 
162  template<typename Vector3Like>
163  JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
164  : axis(axis)
165  {
166  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
167  assert(isUnitary(axis) && "Rotation axis is not unitary");
168  }
169 
170  JointDataDerived createData() const
171  {
172  return JointDataDerived(axis);
173  }
174 
175  const std::vector<bool> hasConfigurationLimit() const
176  {
177  return {false, false};
178  }
179 
180  const std::vector<bool> hasConfigurationLimitInTangent() const
181  {
182  return {false};
183  }
184 
185  using Base::isEqual;
186  bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const
187  {
188  return Base::isEqual(other) && internal::comparison_eq(axis, other.axis);
189  }
190 
191  template<typename ConfigVector>
192  void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
193  {
194  data.joint_q = qs.template segment<NQ>(idx_q());
195 
196  const Scalar & ca = data.joint_q(0);
197  const Scalar & sa = data.joint_q(1);
198 
199  toRotationMatrix(axis, ca, sa, data.M.rotation());
200  }
201 
202  template<typename TangentVector>
203  void
204  calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
205  const
206  {
207  data.joint_v[0] = vs[idx_v()];
208  data.v.angularRate() = data.joint_v[0];
209  }
210 
211  template<typename ConfigVector, typename TangentVector>
212  void calc(
213  JointDataDerived & data,
214  const typename Eigen::MatrixBase<ConfigVector> & qs,
215  const typename Eigen::MatrixBase<TangentVector> & vs) const
216  {
217  calc(data, qs.derived());
218  data.joint_v[0] = vs[idx_v()];
219  data.v.angularRate() = data.joint_v[0];
220  }
221 
222  template<typename VectorLike, typename Matrix6Like>
223  void calc_aba(
224  JointDataDerived & data,
225  const Eigen::MatrixBase<VectorLike> & armature,
226  const Eigen::MatrixBase<Matrix6Like> & I,
227  const bool update_I) const
228  {
229  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
230  data.Dinv[0] =
231  Scalar(1) / (axis.dot(data.U.template segment<3>(Motion::ANGULAR)) + armature[0]);
232  data.UDinv.noalias() = data.U * data.Dinv;
233 
234  if (update_I)
235  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
236  }
237 
238  static std::string classname()
239  {
240  return std::string("JointModelRevoluteUnboundedUnaligned");
241  }
242  std::string shortname() const
243  {
244  return classname();
245  }
246 
248  template<typename NewScalar>
250  {
252  ReturnType res(axis.template cast<NewScalar>());
253  res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
254  return res;
255  }
256 
257  // data
261  Vector3 axis;
262  }; // struct JointModelRevoluteUnboundedUnalignedTpl
263 
264  template<typename Scalar, int Options>
266  {
268  };
269 } // namespace pinocchio
270 
271 #include <boost/type_traits.hpp>
272 
273 namespace boost
274 {
275  template<typename Scalar, int Options>
276  struct has_nothrow_constructor<
278  : public integral_constant<bool, true>
279  {
280  };
281 
282  template<typename Scalar, int Options>
283  struct has_nothrow_copy<::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options>>
284  : public integral_constant<bool, true>
285  {
286  };
287 
288  template<typename Scalar, int Options>
289  struct has_nothrow_constructor<
291  : public integral_constant<bool, true>
292  {
293  };
294 
295  template<typename Scalar, int Options>
296  struct has_nothrow_copy<::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar, Options>>
297  : public integral_constant<bool, true>
298  {
299  };
300 } // namespace boost
301 
302 #endif // ifndef __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__
Main pinocchio namespace.
Definition: treeview.dox:11
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
Definition: matrix.hpp:155
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.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
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:26
Blank type.
Definition: fwd.hpp:77
Assign the correct configuration vector space affine transformation according to the joint type....
JointModelRevoluteUnboundedUnalignedTpl< NewScalar, Options > cast() const
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72