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.
ConstraintForceSetOp< ConstraintRevoluteTpl, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
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>
JointModelRevoluteTpl< NewScalar, Options, axis > cast() const
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...
ConstAngularType angular() const
Return the angular part of the force vector.
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)
.