5 #ifndef __pinocchio_joint_revolute_unbounded_hpp__ 6 #define __pinocchio_joint_revolute_unbounded_hpp__ 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" 19 template<
typename _Scalar,
int _Options,
int axis>
26 typedef _Scalar Scalar;
27 enum { Options = _Options };
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;
40 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
42 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
43 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
46 template<
typename Scalar,
int Options,
int axis>
50 template<
typename Scalar,
int Options,
int axis>
54 template<
typename _Scalar,
int _Options,
int axis>
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
60 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
74 static std::string classname() {
return std::string(
"JointDataRevoluteUnbounded"); }
75 std::string
shortname()
const {
return classname(); }
79 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
85 template<
typename _Scalar,
int _Options,
int axis>
87 :
public JointModelBase< JointModelRevoluteUnboundedTpl<_Scalar,_Options,axis> >
89 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
91 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
98 using Base::setIndexes;
100 JointDataDerived
createData()
const {
return JointDataDerived(); }
102 template<
typename ConfigVector>
103 void calc(JointDataDerived & data,
104 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 106 typedef typename ConfigVector::Scalar OtherScalar;
107 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
108 & q = qs.template segment<NQ> (
idx_q());
110 const OtherScalar & ca = q(0);
111 const OtherScalar & sa = q(1);
113 data.M.setValues(sa,ca);
116 template<
typename ConfigVector,
typename TangentVector>
117 void calc(JointDataDerived & data,
118 const typename Eigen::MatrixBase<ConfigVector> & qs,
119 const typename Eigen::MatrixBase<TangentVector> & vs)
const 121 calc(data,qs.derived());
123 data.v.w = (Scalar)vs[
idx_v()];
126 template<
typename Matrix6Like>
127 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const 129 data.U = I.col(Inertia::ANGULAR + axis);
130 data.Dinv[0] = (Scalar)(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
131 data.UDinv.noalias() = data.U * data.Dinv[0];
134 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
137 static std::string classname()
139 return std::string(
"JointModelRUB") + axisLabel<axis>();
141 std::string
shortname()
const {
return classname(); }
144 template<
typename NewScalar>
157 template<
typename ConfigVectorIn,
typename Scalar,
typename ConfigVectorOut>
158 static void run(
const Eigen::MatrixBase<ConfigVectorIn> & q,
159 const Scalar & scaling,
160 const Scalar & offset,
161 const Eigen::MatrixBase<ConfigVectorOut> & dest)
163 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn,2);
164 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut,2);
166 const typename ConfigVectorIn::Scalar & ca = q(0);
167 const typename ConfigVectorIn::Scalar & sa = q(1);
169 const typename ConfigVectorIn::Scalar & theta = math::atan2(sa,ca);
170 const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset;
172 ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,dest);
173 SINCOS(theta_transform,&dest_.coeffRef(1),&dest_.coeffRef(0));
177 template<
typename Scalar,
int Options,
int axis>
197 #include <boost/type_traits.hpp> 201 template<
typename Scalar,
int Options,
int axis>
203 :
public integral_constant<bool,true> {};
205 template<
typename Scalar,
int Options,
int axis>
207 :
public integral_constant<bool,true> {};
209 template<
typename Scalar,
int Options,
int axis>
211 :
public integral_constant<bool,true> {};
213 template<
typename Scalar,
int Options,
int axis>
215 :
public integral_constant<bool,true> {};
218 #endif // ifndef __pinocchio_joint_revolute_unbounded_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...
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.
void SINCOS(const Scalar &a, Scalar *sa, Scalar *ca)
Computes sin/cos values of a given input scalar.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointModelRevoluteUnboundedTpl< NewScalar, Options, axis > cast() const
Main pinocchio namespace.
Common traits structure to fully define base classes for CRTP.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
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...