5 #ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__ 6 #define __pinocchio_joint_revolute_unbounded_unaligned_hpp__ 8 #include "pinocchio/fwd.hpp" 9 #include "pinocchio/spatial/inertia.hpp" 10 #include "pinocchio/math/rotation.hpp" 11 #include "pinocchio/math/matrix.hpp" 13 #include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp" 20 template<
typename _Scalar,
int _Options>
27 typedef _Scalar Scalar;
28 enum { Options = _Options };
30 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
31 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
39 typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
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;
46 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
49 template<
typename Scalar,
int Options>
53 template<
typename Scalar,
int Options>
57 template<
typename _Scalar,
int _Options>
59 :
public JointDataBase< JointDataRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
61 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
64 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
78 template<
typename Vector3Like>
83 , U(), Dinv(), UDinv()
86 static std::string classname() {
return std::string(
"JointDataRevoluteUnboundedUnalignedTpl"); }
87 std::string
shortname()
const {
return classname(); }
93 template<
typename _Scalar,
int _Options>
95 :
public JointModelBase< JointModelRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
97 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
99 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
100 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
106 using Base::setIndexes;
116 assert(isUnitary(axis) &&
"Rotation axis is not unitary");
119 template<
typename Vector3Like>
123 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
124 assert(isUnitary(axis) &&
"Rotation axis is not unitary");
127 JointDataDerived
createData()
const {
return JointDataDerived(axis); }
132 return Base::isEqual(other) && axis == other.
axis;
135 template<
typename ConfigVector>
136 void calc(JointDataDerived & data,
137 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 139 typedef typename ConfigVector::Scalar OtherScalar;
140 typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
141 & q = qs.template segment<NQ>(
idx_q());
143 const OtherScalar & ca = q(0);
144 const OtherScalar & sa = q(1);
149 template<
typename ConfigVector,
typename TangentVector>
150 void calc(JointDataDerived & data,
151 const typename Eigen::MatrixBase<ConfigVector> & qs,
152 const typename Eigen::MatrixBase<TangentVector> & vs)
const 154 calc(data,qs.derived());
155 data.v.w = (Scalar)vs[
idx_v()];
158 template<
typename Matrix6Like>
159 void calc_aba(JointDataDerived & data,
160 const Eigen::MatrixBase<Matrix6Like> & I,
161 const bool update_I)
const 163 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
164 data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
165 data.UDinv.noalias() = data.U * data.Dinv;
168 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
171 static std::string classname() {
return std::string(
"JointModelRevoluteUnboundedUnaligned"); }
172 std::string
shortname()
const {
return classname(); }
175 template<
typename NewScalar>
179 ReturnType res(axis.template cast<NewScalar>());
193 #include <boost/type_traits.hpp> 197 template<
typename Scalar,
int Options>
199 :
public integral_constant<bool,true> {};
201 template<
typename Scalar,
int Options>
203 :
public integral_constant<bool,true> {};
205 template<
typename Scalar,
int Options>
207 :
public integral_constant<bool,true> {};
209 template<
typename Scalar,
int Options>
211 :
public integral_constant<bool,true> {};
215 #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...
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.
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.
Common traits structure to fully define base classes for CRTP.
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.
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...
Vector3 axis
3d main axis of the joint.