joint-revolute-unbounded.hpp
Go to the documentation of this file.
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 };
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 
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>
49 
50  template<typename Scalar, int Options, int axis>
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
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 
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>
90  {
92  };
93 
94  template<typename _Scalar, int _Options, int axis>
96  : public JointModelBase< JointModelRevoluteUnboundedTpl<_Scalar,_Options,axis> >
97  {
98  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
100  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
102 
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>
165  {
167  ReturnType res;
168  res.setIndexes(id(),idx_q(),idx_v());
169  return res;
170  }
171 
172  }; // struct JointModelRevoluteUnboundedTpl
173 
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>
198  {
200  };
201 
205 
209 
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__
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnboundedTpl< _Scalar, _Options, axis > JointDerived
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...
JointRevoluteUnboundedTpl< double, 0, 0 > JointRUBX
JointRevoluteUnboundedTpl< double, 0, 2 > JointRUBZ
JointModelRevoluteUnboundedTpl< double, 0, 1 > JointModelRUBY
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
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnboundedTpl< _Scalar, _Options, axis > JointDerived
JointModelRevoluteUnboundedTpl< double, 0, 2 > JointModelRUBZ
JointModelRevoluteUnboundedTpl< Scalar, Options, axis > JointModelDerived
Assign the correct configuration vector space affine transformation according to the joint type...
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
JointModelRevoluteUnboundedTpl< NewScalar, Options, axis > cast() const
JointDataRevoluteUnboundedTpl< Scalar, Options, axis > JointDataDerived
SE3::Scalar Scalar
Definition: conversions.cpp:13
JointDataRevoluteUnboundedTpl< double, 0, 0 > JointDataRUBX
const std::vector< bool > hasConfigurationLimitInTangent() const
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Constraint_t S
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
const std::vector< bool > hasConfigurationLimit() const
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
static void run(const Eigen::MatrixBase< ConfigVectorIn > &q, const Scalar &scaling, const Scalar &offset, const Eigen::MatrixBase< ConfigVectorOut > &dest)
JointRevoluteUnboundedTpl< double, 0, 1 > JointRUBY
JointDataRevoluteUnboundedTpl< double, 0, 2 > JointDataRUBZ
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
JointRevoluteTpl< Scalar, _Options, axis > JointDerivedBase
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 calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
JointModelBase< JointModelRevoluteUnboundedTpl > Base
NV
Definition: dcrba.py:444
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
JointDataRevoluteUnboundedTpl< double, 0, 1 > JointDataRUBY
JointModelRevoluteUnboundedTpl< double, 0, 0 > JointModelRUBX
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: src/fwd.hpp:55
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)


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