pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
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  : M(Transformation_t::Identity())
78  , S(Constraint_t::Vector3::Zero())
79  , v(Constraint_t::Vector3::Zero(),(Scalar)0)
80  , U(U_t::Zero())
81  , Dinv(D_t::Zero())
82  , UDinv(UD_t::Zero())
83  {}
84 
85  template<typename Vector3Like>
86  JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
87  : M(Transformation_t::Identity())
88  , S(axis)
89  , v(axis,(Scalar)NAN)
90  , U(U_t::Zero())
91  , Dinv(D_t::Zero())
92  , UDinv(UD_t::Zero())
93  {}
94 
95  static std::string classname() { return std::string("JointDataRevoluteUnboundedUnalignedTpl"); }
96  std::string shortname() const { return classname(); }
97 
98  }; // struct JointDataRevoluteUnboundedUnalignedTpl
99 
100  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl);
101 
102  template<typename _Scalar, int _Options>
104  : public JointModelBase< JointModelRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
105  {
106  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
108  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
109  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
110 
112  using Base::id;
113  using Base::idx_q;
114  using Base::idx_v;
115  using Base::setIndexes;
116 
118 
120  const Scalar & y,
121  const Scalar & z)
122  : axis(x,y,z)
123  {
124  axis.normalize();
125  assert(isUnitary(axis) && "Rotation axis is not unitary");
126  }
127 
128  template<typename Vector3Like>
129  JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
130  : axis(axis)
131  {
132  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
133  assert(isUnitary(axis) && "Rotation axis is not unitary");
134  }
135 
136  JointDataDerived createData() const { return JointDataDerived(axis); }
137 
138  using Base::isEqual;
139  bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const
140  {
141  return Base::isEqual(other) && axis == other.axis;
142  }
143 
144  template<typename ConfigVector>
145  void calc(JointDataDerived & data,
146  const typename Eigen::MatrixBase<ConfigVector> & qs) const
147  {
148  typedef typename ConfigVector::Scalar OtherScalar;
149  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
150  & q = qs.template segment<NQ>(idx_q());
151 
152  const OtherScalar & ca = q(0);
153  const OtherScalar & sa = q(1);
154 
155  toRotationMatrix(axis,ca,sa,data.M.rotation());
156  }
157 
158  template<typename ConfigVector, typename TangentVector>
159  void calc(JointDataDerived & data,
160  const typename Eigen::MatrixBase<ConfigVector> & qs,
161  const typename Eigen::MatrixBase<TangentVector> & vs) const
162  {
163  calc(data,qs.derived());
164  data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
165  }
166 
167  template<typename Matrix6Like>
168  void calc_aba(JointDataDerived & data,
169  const Eigen::MatrixBase<Matrix6Like> & I,
170  const bool update_I) const
171  {
172  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
173  data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
174  data.UDinv.noalias() = data.U * data.Dinv;
175 
176  if (update_I)
177  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
178  }
179 
180  static std::string classname() { return std::string("JointModelRevoluteUnboundedUnaligned"); }
181  std::string shortname() const { return classname(); }
182 
184  template<typename NewScalar>
186  {
188  ReturnType res(axis.template cast<NewScalar>());
189  res.setIndexes(id(),idx_q(),idx_v());
190  return res;
191  }
192 
193  // data
197  Vector3 axis;
198  }; // struct JointModelRevoluteUnboundedUnalignedTpl
199 
200 } //namespace pinocchio
201 
202 #include <boost/type_traits.hpp>
203 
204 namespace boost
205 {
206  template<typename Scalar, int Options>
207  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
208  : public integral_constant<bool,true> {};
209 
210  template<typename Scalar, int Options>
211  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
212  : public integral_constant<bool,true> {};
213 
214  template<typename Scalar, int Options>
215  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
216  : public integral_constant<bool,true> {};
217 
218  template<typename Scalar, int Options>
219  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
220  : public integral_constant<bool,true> {};
221 }
222 
223 
224 #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...
Definition: casadi.hpp:13
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.
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:100
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: fwd.hpp:35
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:24
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...