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
92 TransformRevoluteTpl(
const Scalar & sin,
const Scalar & cos)
93 : m_sin(sin), m_cos(cos)
96 PlainType plain()
const
98 PlainType res(PlainType::Identity());
99 _setRotation (res.rotation());
103 operator PlainType()
const {
return plain(); }
105 template<
typename S2,
int O2>
106 typename SE3GroupAction<TransformRevoluteTpl>::ReturnType
107 se3action(
const SE3Tpl<S2,O2> & m)
const
109 typedef typename SE3GroupAction<TransformRevoluteTpl>::ReturnType ReturnType;
115 res.rotation().col(0) = m.rotation().col(0);
116 res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2);
117 res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1));
122 res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
123 res.rotation().col(1) = m.rotation().col(1);
124 res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
129 res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
130 res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
131 res.rotation().col(2) = m.rotation().col(2);
136 assert(
false &&
"must nerver happened");
140 res.translation() = m.translation();
144 const Scalar & sin()
const {
return m_sin; }
145 Scalar & sin() {
return m_sin; }
147 const Scalar & cos()
const {
return m_cos; }
148 Scalar & cos() {
return m_cos; }
150 template<
typename OtherScalar>
151 void setValues(
const OtherScalar & sin,
const OtherScalar & cos)
152 { m_sin = sin; m_cos = cos; }
154 LinearType translation()
const {
return LinearType::PlainObject::Zero(3); };
155 AngularType rotation()
const {
156 AngularType m(AngularType::Identity(3));
161 bool isEqual(
const TransformRevoluteTpl & other)
const
163 return m_cos == other.m_cos && m_sin == other.m_sin;
169 inline void _setRotation (
typename PlainType::AngularRef& rot)
const
175 rot.coeffRef(1,1) = m_cos; rot.coeffRef(1,2) = -m_sin;
176 rot.coeffRef(2,1) = m_sin; rot.coeffRef(2,2) = m_cos;
181 rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,2) = m_sin;
182 rot.coeffRef(2,0) = -m_sin; rot.coeffRef(2,2) = m_cos;
187 rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,1) = -m_sin;
188 rot.coeffRef(1,0) = m_sin; rot.coeffRef(1,1) = m_cos;
193 assert(
false &&
"must nerver happened");
200 template<
typename _Scalar,
int _Options,
int axis>
201 struct MotionRevoluteTpl
202 : MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
204 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
206 MOTION_TYPEDEF_TPL(MotionRevoluteTpl);
207 typedef SpatialAxis<axis+ANGULAR> Axis;
208 typedef typename Axis::CartesianAxis3 CartesianAxis3;
210 MotionRevoluteTpl() {}
212 MotionRevoluteTpl(
const Scalar & w) : m_w(w) {}
214 template<
typename Vector1Like>
215 MotionRevoluteTpl(
const Eigen::MatrixBase<Vector1Like> & v)
218 using namespace Eigen;
219 EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like);
222 inline PlainReturnType plain()
const {
return Axis() * m_w; }
224 template<
typename OtherScalar>
225 MotionRevoluteTpl __mult__(
const OtherScalar & alpha)
const
227 return MotionRevoluteTpl(alpha*m_w);
230 template<
typename MotionDerived>
231 void setTo(MotionDense<MotionDerived> & m)
const
233 m.linear().setZero();
234 for(Eigen::DenseIndex k = 0; k < 3; ++k)
235 m.angular()[k] = k == axis ? m_w : (Scalar)0;
238 template<
typename MotionDerived>
239 inline void addTo(MotionDense<MotionDerived> & v)
const
241 typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
242 v.angular()[axis] += (OtherScalar)m_w;
245 template<
typename S2,
int O2,
typename D2>
246 inline void se3Action_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
248 v.angular().noalias() = m.rotation().col(axis) * m_w;
249 v.linear().noalias() = m.translation().cross(v.angular());
252 template<
typename S2,
int O2>
253 MotionPlain se3Action_impl(
const SE3Tpl<S2,O2> & m)
const
256 se3Action_impl(m,res);
260 template<
typename S2,
int O2,
typename D2>
261 void se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m,
262 MotionDense<D2> & v)
const
265 CartesianAxis3::alphaCross(m_w,m.translation(),v.angular());
266 v.linear().noalias() = m.rotation().transpose() * v.angular();
269 v.angular().noalias() = m.rotation().transpose().col(axis) * m_w;
272 template<
typename S2,
int O2>
273 MotionPlain se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m)
const
276 se3ActionInverse_impl(m,res);
280 template<
typename M1,
typename M2>
282 void motionAction(
const MotionDense<M1> & v, MotionDense<M2> & mout)
const
285 CartesianAxis3::alphaCross(-m_w,v.linear(),mout.linear());
288 CartesianAxis3::alphaCross(-m_w,v.angular(),mout.angular());
291 template<
typename M1>
292 MotionPlain motionAction(
const MotionDense<M1> & v)
const
299 Scalar & angularRate() {
return m_w; }
300 const Scalar & angularRate()
const {
return m_w; }
302 bool isEqual_impl(
const MotionRevoluteTpl & other)
const
304 return m_w == other.m_w;
312 template<
typename S1,
int O1,
int axis,
typename MotionDerived>
313 typename MotionDerived::MotionPlain
314 operator+(
const MotionRevoluteTpl<S1,O1,axis> & m1,
315 const MotionDense<MotionDerived> & m2)
317 typename MotionDerived::MotionPlain res(m2);
322 template<
typename MotionDerived,
typename S2,
int O2,
int axis>
324 typename MotionDerived::MotionPlain
325 operator^(
const MotionDense<MotionDerived> & m1,
const MotionRevoluteTpl<S2,O2,axis>& m2)
327 return m2.motionAction(m1);
332 template<
typename Scalar,
int Options,
int axis>
334 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
336 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
338 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
340 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
344 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
346 {
typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
348 template<
typename _Scalar,
int _Options,
int axis>
351 typedef _Scalar Scalar;
352 enum { Options = _Options };
358 typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
359 typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
360 typedef DenseBase MatrixReturnType;
361 typedef const DenseBase ConstMatrixReturnType;
364 template<
typename _Scalar,
int _Options,
int axis>
368 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
373 typedef SpatialAxis<ANGULAR+axis> Axis;
375 ConstraintRevoluteTpl() {}
377 template<
typename Vector1Like>
378 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const
379 {
return JointMotion(v[0]); }
381 template<
typename S1,
int O1>
382 typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType
383 se3Action(
const SE3Tpl<S1,O1> & m)
const
385 typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
387 res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis));
388 res.template segment<3>(ANGULAR) = m.rotation().col(axis);
392 template<
typename S1,
int O1>
393 typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType
394 se3ActionInverse(
const SE3Tpl<S1,O1> & m)
const
396 typedef typename SE3GroupAction<ConstraintRevoluteTpl>::ReturnType ReturnType;
397 typedef typename Axis::CartesianAxis3 CartesianAxis3;
399 res.template segment<3>(LINEAR).noalias() = m.rotation().transpose()*CartesianAxis3::cross(m.translation());
400 res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis);
404 int nv_impl()
const {
return NV; }
411 template<
typename ForceDerived>
412 typename ConstraintForceOp<ConstraintRevoluteTpl,ForceDerived>::ReturnType
414 {
return f.
angular().template segment<1>(axis); }
417 template<
typename Derived>
418 typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType
422 return F.row(ANGULAR + axis);
426 TransposeConst transpose()
const {
return TransposeConst(*
this); }
434 DenseBase matrix_impl()
const
437 MotionRef<DenseBase> v(S);
442 template<
typename MotionDerived>
443 typename MotionAlgebraAction<ConstraintRevoluteTpl,MotionDerived>::ReturnType
444 motionAction(
const MotionDense<MotionDerived> & m)
const
446 typedef typename MotionAlgebraAction<ConstraintRevoluteTpl,MotionDerived>::ReturnType ReturnType;
448 MotionRef<ReturnType> v(res);
453 bool isEqual(
const ConstraintRevoluteTpl &)
const {
return true; }
457 template<
typename _Scalar,
int _Options,
int _axis>
460 typedef _Scalar Scalar;
469 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
472 typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
478 template<
typename S1,
int O1,
typename S2,
int O2>
484 static inline ReturnType run(
const Inertia & Y,
495 const typename Inertia::Symmetric3 & I = Y.inertia();
509 template<
typename S1,
int O1,
typename S2,
int O2>
515 static inline ReturnType run(
const Inertia & Y,
526 const typename Inertia::Symmetric3 & I = Y.inertia();
540 template<
typename S1,
int O1,
typename S2,
int O2>
546 static inline ReturnType run(
const Inertia & Y,
557 const typename Inertia::Symmetric3 & I = Y.inertia();
572 template<
typename M6Like,
typename S2,
int O2,
int axis>
575 typedef typename M6Like::ConstColXpr ReturnType;
581 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
586 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
589 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
590 return Y.col(Inertia::ANGULAR + axis);
595 template<
typename _Scalar,
int _Options,
int axis>
602 typedef _Scalar Scalar;
603 enum { Options = _Options };
612 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
613 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
614 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
616 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
618 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
619 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
622 template<
typename Scalar,
int Options,
int axis>
626 template<
typename Scalar,
int Options,
int axis>
630 template<
typename _Scalar,
int _Options,
int axis>
633 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
635 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
636 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
649 : M((Scalar)0,(Scalar)1)
653 , UDinv(UD_t::Zero())
656 static std::string classname()
658 return std::string(
"JointDataR") + axisLabel<axis>();
660 std::string
shortname()
const {
return classname(); }
664 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
670 template<
typename _Scalar,
int _Options,
int axis>
672 :
public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> >
674 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
676 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
682 using Base::setIndexes;
684 JointDataDerived
createData()
const {
return JointDataDerived(); }
686 JointModelRevoluteTpl() {}
688 template<
typename ConfigVector>
690 void calc(JointDataDerived & data,
691 const typename Eigen::MatrixBase<ConfigVector> & qs)
const
693 typedef typename ConfigVector::Scalar OtherScalar;
695 const OtherScalar & q = qs[
idx_q()];
696 OtherScalar ca,sa;
SINCOS(q,&sa,&ca);
697 data.M.setValues(sa,ca);
700 template<
typename ConfigVector,
typename TangentVector>
702 void calc(JointDataDerived & data,
703 const typename Eigen::MatrixBase<ConfigVector> & qs,
704 const typename Eigen::MatrixBase<TangentVector> & vs)
const
706 calc(data,qs.derived());
708 data.v.angularRate() =
static_cast<Scalar
>(vs[
idx_v()]);
711 template<
typename Matrix6Like>
712 void calc_aba(JointDataDerived & data,
713 const Eigen::MatrixBase<Matrix6Like> & I,
714 const bool update_I)
const
716 data.U = I.col(Inertia::ANGULAR + axis);
717 data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
718 data.UDinv.noalias() = data.U * data.Dinv[0];
721 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
724 static std::string classname()
726 return std::string(
"JointModelR") + axisLabel<axis>();
728 std::string
shortname()
const {
return classname(); }
731 template<
typename NewScalar>
742 typedef JointRevoluteTpl<double,0,0> JointRX;
743 typedef JointDataRevoluteTpl<double,0,0> JointDataRX;
744 typedef JointModelRevoluteTpl<double,0,0> JointModelRX;
746 typedef JointRevoluteTpl<double,0,1> JointRY;
747 typedef JointDataRevoluteTpl<double,0,1> JointDataRY;
748 typedef JointModelRevoluteTpl<double,0,1> JointModelRY;
750 typedef JointRevoluteTpl<double,0,2> JointRZ;
751 typedef JointDataRevoluteTpl<double,0,2> JointDataRZ;
752 typedef JointModelRevoluteTpl<double,0,2> JointModelRZ;
756 #include <boost/type_traits.hpp>
760 template<
typename Scalar,
int Options,
int axis>
762 :
public integral_constant<bool,true> {};
764 template<
typename Scalar,
int Options,
int axis>
766 :
public integral_constant<bool,true> {};
768 template<
typename Scalar,
int Options,
int axis>
770 :
public integral_constant<bool,true> {};
772 template<
typename Scalar,
int Options,
int axis>
774 :
public integral_constant<bool,true> {};
777 #endif // ifndef __pinocchio_joint_revolute_hpp__