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(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
124 res.rotation().col(1) = m.rotation().col(1);
125 res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
130 res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
131 res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
132 res.rotation().col(2) = m.rotation().col(2);
137 assert(
false &&
"must nerver happened");
141 res.translation() = m.translation();
145 const Scalar & sin()
const {
return m_sin; }
146 Scalar & sin() {
return m_sin; }
148 const Scalar & cos()
const {
return m_cos; }
149 Scalar & cos() {
return m_cos; }
151 template<
typename OtherScalar>
152 void setValues(
const OtherScalar & sin,
const OtherScalar & cos)
153 { m_sin = sin; m_cos = cos; }
155 LinearType translation()
const {
return LinearType::PlainObject::Zero(3); };
156 AngularType rotation()
const {
157 AngularType m(AngularType::Identity(3));
164 return m_cos == other.m_cos && m_sin == other.m_sin;
170 inline void _setRotation (
typename PlainType::AngularRef& rot)
const 176 rot.coeffRef(1,1) = m_cos; rot.coeffRef(1,2) = -m_sin;
177 rot.coeffRef(2,1) = m_sin; rot.coeffRef(2,2) = m_cos;
182 rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,2) = m_sin;
183 rot.coeffRef(2,0) = -m_sin; rot.coeffRef(2,2) = m_cos;
188 rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,1) = -m_sin;
189 rot.coeffRef(1,0) = m_sin; rot.coeffRef(1,1) = m_cos;
194 assert(
false &&
"must nerver happened");
201 template<
typename _Scalar,
int _Options,
int axis>
203 :
MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
205 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
209 typedef typename Axis::CartesianAxis3 CartesianAxis3;
215 template<
typename Vector1Like>
219 using namespace Eigen;
220 EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like);
223 inline PlainReturnType plain()
const {
return Axis() * m_w; }
225 template<
typename OtherScalar>
231 template<
typename MotionDerived>
234 m.linear().setZero();
235 for(Eigen::DenseIndex k = 0; k < 3; ++k)
236 m.angular()[k] = k == axis ? m_w : (Scalar)0;
239 template<
typename MotionDerived>
243 v.angular()[axis] += (OtherScalar)m_w;
246 template<
typename S2,
int O2,
typename D2>
249 v.angular().noalias() = m.rotation().col(axis) * m_w;
250 v.linear().noalias() = m.translation().cross(v.angular());
253 template<
typename S2,
int O2>
257 se3Action_impl(m,res);
261 template<
typename S2,
int O2,
typename D2>
266 CartesianAxis3::alphaCross(m_w,m.translation(),v.angular());
267 v.linear().noalias() = m.rotation().transpose() * v.angular();
270 v.angular().noalias() = m.rotation().transpose().col(axis) * m_w;
273 template<
typename S2,
int O2>
274 MotionPlain se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m)
const 277 se3ActionInverse_impl(m,res);
281 template<
typename M1,
typename M2>
286 CartesianAxis3::alphaCross(-m_w,v.linear(),mout.linear());
289 CartesianAxis3::alphaCross(-m_w,v.angular(),mout.angular());
292 template<
typename M1>
300 Scalar & angularRate() {
return m_w; }
301 const Scalar & angularRate()
const {
return m_w; }
305 return m_w == other.m_w;
313 template<
typename S1,
int O1,
int axis,
typename MotionDerived>
314 typename MotionDerived::MotionPlain
318 typename MotionDerived::MotionPlain res(m2);
323 template<
typename MotionDerived,
typename S2,
int O2,
int axis>
325 typename MotionDerived::MotionPlain
328 return m2.motionAction(m1);
333 template<
typename Scalar,
int Options,
int axis>
335 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
337 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
339 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
341 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
345 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
347 {
typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
349 template<
typename _Scalar,
int _Options,
int axis>
352 typedef _Scalar Scalar;
353 enum { Options = _Options };
359 typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
360 typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
361 typedef DenseBase MatrixReturnType;
362 typedef const DenseBase ConstMatrixReturnType;
365 template<
typename _Scalar,
int _Options,
int axis>
369 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
378 template<
typename Vector1Like>
379 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const 382 template<
typename S1,
int O1>
388 res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis));
389 res.template segment<3>(ANGULAR) = m.rotation().col(axis);
393 template<
typename S1,
int O1>
398 typedef typename Axis::CartesianAxis3 CartesianAxis3;
400 res.template segment<3>(LINEAR).noalias() = m.rotation().transpose()*CartesianAxis3::cross(m.translation());
401 res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis);
405 int nv_impl()
const {
return NV; }
412 template<
typename ForceDerived>
413 typename ConstraintForceOp<ConstraintRevoluteTpl,ForceDerived>::ReturnType
415 {
return f.
angular().template segment<1>(axis); }
418 template<
typename Derived>
419 typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType
423 return F.row(ANGULAR + axis);
435 DenseBase matrix_impl()
const 443 template<
typename MotionDerived>
458 template<
typename _Scalar,
int _Options,
int _axis>
461 typedef _Scalar Scalar;
470 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
473 typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
479 template<
typename S1,
int O1,
typename S2,
int O2>
485 static inline ReturnType run(
const Inertia & Y,
496 const typename Inertia::Symmetric3 & I = Y.inertia();
510 template<
typename S1,
int O1,
typename S2,
int O2>
516 static inline ReturnType run(
const Inertia & Y,
527 const typename Inertia::Symmetric3 & I = Y.inertia();
541 template<
typename S1,
int O1,
typename S2,
int O2>
547 static inline ReturnType run(
const Inertia & Y,
558 const typename Inertia::Symmetric3 & I = Y.inertia();
573 template<
typename M6Like,
typename S2,
int O2,
int axis>
576 typedef typename M6Like::ConstColXpr ReturnType;
582 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
587 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
590 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
591 return Y.col(Inertia::ANGULAR + axis);
596 template<
typename _Scalar,
int _Options,
int axis>
603 typedef _Scalar Scalar;
604 enum { Options = _Options };
613 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
614 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
615 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
617 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
619 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
620 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
623 template<
typename Scalar,
int Options,
int axis>
627 template<
typename Scalar,
int Options,
int axis>
631 template<
typename _Scalar,
int _Options,
int axis>
634 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
636 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
637 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
650 : M((Scalar)0,(Scalar)1)
654 , UDinv(UD_t::Zero())
657 static std::string classname()
659 return std::string(
"JointDataR") + axisLabel<axis>();
661 std::string
shortname()
const {
return classname(); }
665 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
671 template<
typename _Scalar,
int _Options,
int axis>
673 :
public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> >
675 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
677 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
683 using Base::setIndexes;
685 JointDataDerived
createData()
const {
return JointDataDerived(); }
689 template<
typename ConfigVector>
691 void calc(JointDataDerived & data,
692 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 694 typedef typename ConfigVector::Scalar OtherScalar;
696 const OtherScalar & q = qs[
idx_q()];
697 OtherScalar ca,sa;
SINCOS(q,&sa,&ca);
698 data.M.setValues(sa,ca);
701 template<
typename ConfigVector,
typename TangentVector>
703 void calc(JointDataDerived & data,
704 const typename Eigen::MatrixBase<ConfigVector> & qs,
705 const typename Eigen::MatrixBase<TangentVector> & vs)
const 707 calc(data,qs.derived());
709 data.v.angularRate() =
static_cast<Scalar
>(vs[
idx_v()]);
712 template<
typename Matrix6Like>
713 void calc_aba(JointDataDerived & data,
714 const Eigen::MatrixBase<Matrix6Like> & I,
715 const bool update_I)
const 717 data.U = I.col(Inertia::ANGULAR + axis);
718 data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
719 data.UDinv.noalias() = data.U * data.Dinv[0];
722 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
725 static std::string classname()
727 return std::string(
"JointModelR") + axisLabel<axis>();
729 std::string
shortname()
const {
return classname(); }
732 template<
typename NewScalar>
757 #include <boost/type_traits.hpp> 761 template<
typename Scalar,
int Options,
int axis>
763 :
public integral_constant<bool,true> {};
765 template<
typename Scalar,
int Options,
int axis>
767 :
public integral_constant<bool,true> {};
769 template<
typename Scalar,
int Options,
int axis>
771 :
public integral_constant<bool,true> {};
773 template<
typename Scalar,
int Options,
int axis>
775 :
public integral_constant<bool,true> {};
778 #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>
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)
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
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)