6 #ifndef __pinocchio_joint_revolute_hpp__ 7 #define __pinocchio_joint_revolute_hpp__ 9 #include "pinocchio/math/sincos.hpp" 10 #include "pinocchio/spatial/inertia.hpp" 11 #include "pinocchio/multibody/constraint.hpp" 12 #include "pinocchio/multibody/joint/joint-base.hpp" 13 #include "pinocchio/spatial/spatial-axis.hpp" 14 #include "pinocchio/utils/axis-label.hpp" 21 template<
typename Scalar,
int Options,
int axis>
27 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
33 template<
typename _Scalar,
int _Options,
int axis>
36 typedef _Scalar Scalar;
37 enum { Options = _Options };
38 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
39 typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
40 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
41 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
42 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
43 typedef Vector3 AngularType;
44 typedef Vector3 LinearType;
45 typedef const Vector3 ConstAngularType;
46 typedef const Vector3 ConstLinearType;
47 typedef Matrix6 ActionMatrixType;
58 template<
typename _Scalar,
int _Options,
int _axis>
67 typedef _Scalar Scalar;
69 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
70 typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
71 typedef Matrix3 AngularType;
72 typedef Matrix3 AngularRef;
73 typedef Matrix3 ConstAngularRef;
74 typedef typename Vector3::ConstantReturnType LinearType;
75 typedef typename Vector3::ConstantReturnType LinearRef;
76 typedef const typename Vector3::ConstantReturnType ConstLinearRef;
81 template<
typename Scalar,
int Options,
int axis>
85 template<
typename _Scalar,
int _Options,
int axis>
88 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94 : m_sin(sin), m_cos(cos)
97 PlainType plain()
const 99 PlainType res(PlainType::Identity());
100 _setRotation (res.rotation());
104 operator PlainType()
const {
return plain(); }
106 template<
typename S2,
int O2>
116 res.rotation().col(0) = m.rotation().col(0);
117 res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2);
118 res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1));
123 res.rotation().col(1) = m.rotation().col(1);
124 res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
125 res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
130 res.rotation().col(2) = m.rotation().col(2);
131 res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
132 res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
137 assert(
false &&
"must nerver happened");
141 res.translation() = m.translation();
145 const Scalar & sin()
const {
return m_sin; }
146 const Scalar & cos()
const {
return m_cos; }
148 template<
typename OtherScalar>
149 void setValues(
const OtherScalar & sin,
const OtherScalar & cos)
150 { m_sin = sin; m_cos = cos; }
152 LinearType translation()
const {
return LinearType::PlainObject::Zero(3); };
153 AngularType rotation()
const {
154 AngularType m(AngularType::Identity(3));
162 inline void _setRotation (
typename PlainType::AngularRef& rot)
const 168 rot.coeffRef(1,1) = m_cos; rot.coeffRef(1,2) = -m_sin;
169 rot.coeffRef(2,1) = m_sin; rot.coeffRef(2,2) = m_cos;
174 rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,2) = m_sin;
175 rot.coeffRef(2,0) = -m_sin; rot.coeffRef(2,2) = m_cos;
180 rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,1) = -m_sin;
181 rot.coeffRef(1,0) = m_sin; rot.coeffRef(1,1) = m_cos;
186 assert(
false &&
"must nerver happened");
193 template<
typename _Scalar,
int _Options,
int axis>
195 :
MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
201 typedef typename Axis::CartesianAxis3 CartesianAxis3;
207 template<
typename Vector1Like>
211 using namespace Eigen;
212 EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like);
215 inline PlainReturnType plain()
const {
return Axis() * w; }
217 template<
typename OtherScalar>
223 template<
typename MotionDerived>
226 m.linear().setZero();
227 for(Eigen::DenseIndex k = 0; k < 3; ++k)
228 m.angular()[k] = k == axis ? w : (Scalar)0;
231 template<
typename MotionDerived>
235 v.angular()[axis] += (OtherScalar)w;
238 template<
typename S2,
int O2,
typename D2>
241 v.angular().noalias() = m.rotation().col(axis) * w;
242 v.linear().noalias() = m.translation().cross(v.angular());
245 template<
typename S2,
int O2>
249 se3Action_impl(m,res);
253 template<
typename S2,
int O2,
typename D2>
258 CartesianAxis3::alphaCross(w,m.translation(),v.angular());
259 v.linear().noalias() = m.rotation().transpose() * v.angular();
262 v.angular().noalias() = m.rotation().transpose().col(axis) * w;
265 template<
typename S2,
int O2>
266 MotionPlain se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m)
const 269 se3ActionInverse_impl(m,res);
273 template<
typename M1,
typename M2>
278 CartesianAxis3::alphaCross(-w,v.linear(),mout.linear());
281 CartesianAxis3::alphaCross(-w,v.angular(),mout.angular());
284 template<
typename M1>
296 template<
typename S1,
int O1,
int axis,
typename MotionDerived>
297 typename MotionDerived::MotionPlain
301 typename MotionDerived::MotionPlain res(m2);
306 template<
typename MotionDerived,
typename S2,
int O2,
int axis>
308 typename MotionDerived::MotionPlain
311 return m2.motionAction(m1);
316 template<
typename Scalar,
int Options,
int axis>
318 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
320 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
322 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
324 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
328 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
330 {
typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
332 template<
typename _Scalar,
int _Options,
int axis>
335 typedef _Scalar Scalar;
336 enum { Options = _Options };
342 typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
343 typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
344 typedef DenseBase MatrixReturnType;
345 typedef const DenseBase ConstMatrixReturnType;
348 template<
typename _Scalar,
int _Options,
int axis>
352 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
361 template<
typename Vector1Like>
362 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const 365 template<
typename S1,
int O1>
371 res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis));
372 res.template segment<3>(ANGULAR) = m.rotation().col(axis);
376 template<
typename S1,
int O1>
381 typedef typename Axis::CartesianAxis3 CartesianAxis3;
383 res.template segment<3>(LINEAR).noalias() = m.rotation().transpose()*CartesianAxis3::cross(m.translation());
384 res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis);
388 int nv_impl()
const {
return NV; }
395 template<
typename ForceDerived>
396 typename ConstraintForceOp<ConstraintRevoluteTpl,ForceDerived>::ReturnType
398 {
return f.
angular().template segment<1>(axis); }
401 template<
typename Derived>
402 typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType
406 return F.row(ANGULAR + axis);
418 DenseBase matrix_impl()
const 426 template<
typename MotionDerived>
438 template<
typename _Scalar,
int _Options,
int _axis>
441 typedef _Scalar Scalar;
450 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
453 typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
459 template<
typename S1,
int O1,
typename S2,
int O2>
465 static inline ReturnType run(
const Inertia & Y,
476 const typename Inertia::Symmetric3 & I = Y.inertia();
490 template<
typename S1,
int O1,
typename S2,
int O2>
496 static inline ReturnType run(
const Inertia & Y,
507 const typename Inertia::Symmetric3 & I = Y.inertia();
521 template<
typename S1,
int O1,
typename S2,
int O2>
527 static inline ReturnType run(
const Inertia & Y,
538 const typename Inertia::Symmetric3 & I = Y.inertia();
553 template<
typename M6Like,
typename S2,
int O2,
int axis>
556 typedef typename M6Like::ConstColXpr ReturnType;
562 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
567 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
570 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
571 return Y.col(Inertia::ANGULAR + axis);
576 template<
typename _Scalar,
int _Options,
int axis>
583 typedef _Scalar Scalar;
584 enum { Options = _Options };
593 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
594 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
595 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
597 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
599 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
600 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
603 template<
typename Scalar,
int Options,
int axis>
607 template<
typename Scalar,
int Options,
int axis>
611 template<
typename _Scalar,
int _Options,
int axis>
614 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
616 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
617 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
631 static std::string classname() {
return std::string(
"JointDataRevolute"); }
632 std::string
shortname()
const {
return classname(); }
636 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
642 template<
typename _Scalar,
int _Options,
int axis>
644 :
public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> >
646 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
648 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
654 using Base::setIndexes;
656 JointDataDerived
createData()
const {
return JointDataDerived(); }
660 template<
typename ConfigVector>
662 void calc(JointDataDerived & data,
663 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 665 typedef typename ConfigVector::Scalar OtherScalar;
667 const OtherScalar & q = qs[
idx_q()];
668 OtherScalar ca,sa;
SINCOS(q,&sa,&ca);
669 data.M.setValues(sa,ca);
672 template<
typename ConfigVector,
typename TangentVector>
674 void calc(JointDataDerived & data,
675 const typename Eigen::MatrixBase<ConfigVector> & qs,
676 const typename Eigen::MatrixBase<TangentVector> & vs)
const 678 calc(data,qs.derived());
680 data.v.w = (Scalar)vs[
idx_v()];
683 template<
typename Matrix6Like>
684 void calc_aba(JointDataDerived & data,
685 const Eigen::MatrixBase<Matrix6Like> & I,
686 const bool update_I)
const 688 data.U = I.col(Inertia::ANGULAR + axis);
689 data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
690 data.UDinv.noalias() = data.U * data.Dinv[0];
693 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
696 static std::string classname()
698 return std::string(
"JointModelR") + axisLabel<axis>();
700 std::string
shortname()
const {
return classname(); }
703 template<
typename NewScalar>
728 #include <boost/type_traits.hpp> 732 template<
typename Scalar,
int Options,
int axis>
734 :
public integral_constant<bool,true> {};
736 template<
typename Scalar,
int Options,
int axis>
738 :
public integral_constant<bool,true> {};
740 template<
typename Scalar,
int Options,
int axis>
742 :
public integral_constant<bool,true> {};
744 template<
typename Scalar,
int Options,
int axis>
746 :
public integral_constant<bool,true> {};
749 #endif // ifndef __pinocchio_joint_revolute_hpp__
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
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...
Return type of the Constraint::Transpose * ForceSet operation.
Return type of the ation of a Motion onto an object of type D.
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
Source from #include <cppad/example/cppad_eigen.hpp>
void SINCOS(const Scalar &a, Scalar *sa, Scalar *ca)
Computes sin/cos values of a given input scalar.
ConstAngularType angular() const
Return the angular part of the force vector.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointModelRevoluteTpl< NewScalar, Options, axis > cast() const
ConstraintForceSetOp< ConstraintRevoluteTpl, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
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...
Return type of the Constraint::Transpose * Force operation.
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...
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
.