joint-revolute-unbounded-unaligned.hpp
Go to the documentation of this file.
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 
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
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 
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  const std::vector<bool> hasConfigurationLimit() const
139  {
140  return {false, false};
141  }
142 
143  const std::vector<bool> hasConfigurationLimitInTangent() const
144  {
145  return {false};
146  }
147 
148  using Base::isEqual;
150  {
151  return Base::isEqual(other) && axis == other.axis;
152  }
153 
154  template<typename ConfigVector>
155  void calc(JointDataDerived & data,
156  const typename Eigen::MatrixBase<ConfigVector> & qs) const
157  {
158  typedef typename ConfigVector::Scalar OtherScalar;
159  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
160  & q = qs.template segment<NQ>(idx_q());
161 
162  const OtherScalar & ca = q(0);
163  const OtherScalar & sa = q(1);
164 
165  toRotationMatrix(axis,ca,sa,data.M.rotation());
166  }
167 
168  template<typename ConfigVector, typename TangentVector>
169  void calc(JointDataDerived & data,
170  const typename Eigen::MatrixBase<ConfigVector> & qs,
171  const typename Eigen::MatrixBase<TangentVector> & vs) const
172  {
173  calc(data,qs.derived());
174  data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
175  }
176 
177  template<typename Matrix6Like>
178  void calc_aba(JointDataDerived & data,
179  const Eigen::MatrixBase<Matrix6Like> & I,
180  const bool update_I) const
181  {
182  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
183  data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
184  data.UDinv.noalias() = data.U * data.Dinv;
185 
186  if (update_I)
187  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
188  }
189 
190  static std::string classname() { return std::string("JointModelRevoluteUnboundedUnaligned"); }
191  std::string shortname() const { return classname(); }
192 
194  template<typename NewScalar>
196  {
198  ReturnType res(axis.template cast<NewScalar>());
199  res.setIndexes(id(),idx_q(),idx_v());
200  return res;
201  }
202 
203  // data
207  Vector3 axis;
208  }; // struct JointModelRevoluteUnboundedUnalignedTpl
209 
210 } //namespace pinocchio
211 
212 #include <boost/type_traits.hpp>
213 
214 namespace boost
215 {
216  template<typename Scalar, int Options>
217  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
218  : public integral_constant<bool,true> {};
219 
220  template<typename Scalar, int Options>
221  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
222  : public integral_constant<bool,true> {};
223 
224  template<typename Scalar, int Options>
225  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
226  : public integral_constant<bool,true> {};
227 
228  template<typename Scalar, int Options>
229  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
230  : public integral_constant<bool,true> {};
231 }
232 
233 
234 #endif // ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__
JointModelRevoluteUnboundedUnalignedTpl< NewScalar, Options > cast() const
JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
int NQ
Definition: dpendulum.py:8
axis
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...
JointModelBase< JointModelRevoluteUnboundedUnalignedTpl > Base
JointModelRevoluteUnboundedUnalignedTpl(const Scalar &x, const Scalar &y, const Scalar &z)
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Transformation_t M
bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl &other) const
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
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.
SE3::Scalar Scalar
Definition: conversions.cpp:13
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
virtual bool isEqual(const CollisionGeometry &other) const=0
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnboundedUnalignedTpl< _Scalar, _Options > JointDerived
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
Main pinocchio namespace.
Definition: timings.cpp:28
res
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
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
NV
Definition: dcrba.py:444
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnboundedUnalignedTpl< _Scalar, _Options > JointDerived
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:31