6 #ifndef __pinocchio_joint_revolute_unaligned_hpp__
7 #define __pinocchio_joint_revolute_unaligned_hpp__
9 #include "pinocchio/fwd.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/constraint.hpp"
12 #include "pinocchio/spatial/inertia.hpp"
13 #include "pinocchio/math/matrix.hpp"
21 template<
typename Scalar,
int Options>
27 template<
typename Scalar,
int Options,
typename MotionDerived>
33 template<
typename _Scalar,
int _Options>
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;
56 template<
typename _Scalar,
int _Options>
58 :
MotionBase< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
60 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65 template<
typename Vector3Like,
typename OtherScalar>
66 MotionRevoluteUnalignedTpl(
const Eigen::MatrixBase<Vector3Like> & axis,
67 const OtherScalar & w)
72 inline PlainReturnType plain()
const
74 return PlainReturnType(PlainReturnType::Vector3::Zero(),
78 template<
typename OtherScalar>
79 MotionRevoluteUnalignedTpl __mult__(
const OtherScalar & alpha)
const
81 return MotionRevoluteUnalignedTpl(m_axis,alpha*m_w);
84 template<
typename MotionDerived>
85 inline void addTo(MotionDense<MotionDerived> & v)
const
87 v.angular() += m_axis*m_w;
90 template<
typename Derived>
91 void setTo(MotionDense<Derived> & other)
const
93 other.linear().setZero();
94 other.angular().noalias() = m_axis*m_w;
97 template<
typename S2,
int O2,
typename D2>
98 void se3Action_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
101 v.angular().noalias() = m_w * m.rotation() * m_axis;
104 v.linear().noalias() = m.translation().cross(v.angular());
107 template<
typename S2,
int O2>
108 MotionPlain se3Action_impl(
const SE3Tpl<S2,O2> & m)
const
111 se3Action_impl(m,res);
115 template<
typename S2,
int O2,
typename D2>
116 void se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
121 v3_tmp.noalias() = m_axis.cross(m.translation());
123 v.linear().noalias() = m.rotation().transpose() * v3_tmp;
126 v.angular().noalias() = m.rotation().transpose() * m_axis;
130 template<
typename S2,
int O2>
131 MotionPlain se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m)
const
134 se3ActionInverse_impl(m,res);
138 template<
typename M1,
typename M2>
139 void motionAction(
const MotionDense<M1> & v, MotionDense<M2> & mout)
const
142 mout.linear().noalias() = v.linear().cross(m_axis);
143 mout.linear() *= m_w;
146 mout.angular().noalias() = v.angular().cross(m_axis);
147 mout.angular() *= m_w;
150 template<
typename M1>
151 MotionPlain motionAction(
const MotionDense<M1> & v)
const
158 bool isEqual_impl(
const MotionRevoluteUnalignedTpl & other)
const
160 return m_axis == other.m_axis && m_w == other.m_w;
163 const Scalar & angularRate()
const {
return m_w; }
164 Scalar & angularRate() {
return m_w; }
166 const Vector3 & axis()
const {
return m_axis; }
167 Vector3 & axis() {
return m_axis; }
175 template<
typename S1,
int O1,
typename MotionDerived>
176 inline typename MotionDerived::MotionPlain
177 operator+(
const MotionRevoluteUnalignedTpl<S1,O1> & m1,
178 const MotionDense<MotionDerived> & m2)
180 typename MotionDerived::MotionPlain res(m2);
185 template<
typename MotionDerived,
typename S2,
int O2>
186 inline typename MotionDerived::MotionPlain
187 operator^(
const MotionDense<MotionDerived> & m1,
188 const MotionRevoluteUnalignedTpl<S2,O2> & m2)
190 return m2.motionAction(m1);
195 template<
typename _Scalar,
int _Options>
198 typedef _Scalar Scalar;
199 enum { Options = _Options };
205 typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
206 typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
207 typedef DenseBase MatrixReturnType;
208 typedef const DenseBase ConstMatrixReturnType;
210 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
213 template<
typename Scalar,
int Options>
215 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
217 template<
typename Scalar,
int Options,
typename MotionDerived>
219 {
typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
221 template<
typename Scalar,
int Options,
typename ForceDerived>
228 template<
typename Scalar,
int Options,
typename ForceSet>
233 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
237 template<
typename _Scalar,
int _Options>
239 :
ConstraintBase< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> >
241 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
246 typedef typename traits<ConstraintRevoluteUnalignedTpl>::Vector3 Vector3;
248 ConstraintRevoluteUnalignedTpl() {}
250 template<
typename Vector3Like>
251 ConstraintRevoluteUnalignedTpl(
const Eigen::MatrixBase<Vector3Like> & axis)
255 template<
typename Vector1Like>
256 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const
258 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
259 return JointMotion(m_axis,v[0]);
262 template<
typename S1,
int O1>
263 typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType
264 se3Action(
const SE3Tpl<S1,O1> & m)
const
266 typedef typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType ReturnType;
270 res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis;
271 res.template segment<3>(LINEAR).noalias() = m.translation().cross(res.template segment<3>(ANGULAR));
275 template<
typename S1,
int O1>
276 typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType
277 se3ActionInverse(
const SE3Tpl<S1,O1> & m)
const
279 typedef typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType ReturnType;
282 res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis;
283 res.template segment<3>(LINEAR).noalias() = -m.rotation().transpose() * m.translation().cross(m_axis);
287 int nv_impl()
const {
return NV; }
294 template<
typename ForceDerived>
295 typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType
298 typedef typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType ReturnType;
300 res[0] = ref.axis().dot(f.
angular());
305 template<
typename ForceSet>
306 typename ConstraintForceSetOp<ConstraintRevoluteUnalignedTpl,ForceSet>::ReturnType
307 operator*(
const Eigen::MatrixBase<ForceSet> & F)
309 EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
311 return ref.axis().transpose() * F.template middleRows<3>(ANGULAR);
324 DenseBase matrix_impl()
const
327 S.template segment<3>(LINEAR).setZero();
328 S.template segment<3>(ANGULAR) = m_axis;
332 template<
typename MotionDerived>
333 typename MotionAlgebraAction<ConstraintRevoluteUnalignedTpl,MotionDerived>::ReturnType
334 motionAction(
const MotionDense<MotionDerived> & m)
const
336 const typename MotionDerived::ConstLinearType v = m.linear();
337 const typename MotionDerived::ConstAngularType w = m.angular();
340 res.template segment<3>(LINEAR).noalias() = v.cross(m_axis);
341 res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis);
346 const Vector3 & axis()
const {
return m_axis; }
347 Vector3 & axis() {
return m_axis; }
349 bool isEqual(
const ConstraintRevoluteUnalignedTpl & other)
const
351 return m_axis == other.m_axis;
360 template<
typename S1,
int O1,
typename S2,
int O2>
363 typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
369 template<
typename S1,
int O1,
typename S2,
int O2>
375 static inline ReturnType run(
const Inertia & Y,
381 const typename Inertia::Scalar & m = Y.mass();
382 const typename Inertia::Vector3 & c = Y.lever();
383 const typename Inertia::Symmetric3 & I = Y.inertia();
385 res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis());
386 res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis();
387 res.template segment<3>(Inertia::ANGULAR) += c.cross(res.template segment<3>(Inertia::LINEAR));
394 template<
typename M6Like,
typename Scalar,
int Options>
398 typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
401 typedef typename Constraint::Vector3 Vector3;
408 template<
typename M6Like,
typename Scalar,
int Options>
414 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
417 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
418 return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis();
425 template<
typename _Scalar,
int _Options>
432 typedef _Scalar Scalar;
433 enum { Options = _Options };
442 typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
443 typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
444 typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
446 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
448 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
449 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
453 template<
typename Scalar,
int Options>
457 template<
typename Scalar,
int Options>
461 template<
typename _Scalar,
int _Options>
463 :
public JointDataBase< JointDataRevoluteUnalignedTpl<_Scalar,_Options> >
465 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
467 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
468 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
481 : M(Transformation_t::Identity())
482 , S(Constraint_t::Vector3::Zero())
483 , v(Constraint_t::Vector3::Zero(),(Scalar)0)
486 , UDinv(UD_t::Zero())
489 template<
typename Vector3Like>
490 JointDataRevoluteUnalignedTpl(
const Eigen::MatrixBase<Vector3Like> & axis)
491 : M(Transformation_t::Identity())
493 , v(axis,(Scalar)NAN)
496 , UDinv(UD_t::Zero())
499 static std::string classname() {
return std::string(
"JointDataRevoluteUnaligned"); }
500 std::string shortname()
const {
return classname(); }
504 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnalignedTpl);
505 template<
typename _Scalar,
int _Options>
506 struct JointModelRevoluteUnalignedTpl
507 :
public JointModelBase< JointModelRevoluteUnalignedTpl<_Scalar,_Options> >
509 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
510 typedef JointRevoluteUnalignedTpl<_Scalar,_Options> JointDerived;
511 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
512 typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
514 typedef JointModelBase<JointModelRevoluteUnalignedTpl> Base;
518 using Base::setIndexes;
520 JointModelRevoluteUnalignedTpl() {}
522 JointModelRevoluteUnalignedTpl(
const Scalar & x,
531 template<
typename Vector3Like>
532 JointModelRevoluteUnalignedTpl(
const Eigen::MatrixBase<Vector3Like> &
axis)
535 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
539 JointDataDerived createData()
const {
return JointDataDerived(
axis); }
542 bool isEqual(
const JointModelRevoluteUnalignedTpl & other)
const
544 return Base::isEqual(other) &&
axis == other.axis;
547 template<
typename ConfigVector>
548 void calc(JointDataDerived & data,
549 const typename Eigen::MatrixBase<ConfigVector> & qs)
const
551 typedef typename ConfigVector::Scalar OtherScalar;
552 typedef Eigen::AngleAxis<Scalar> AngleAxis;
554 const OtherScalar & q = qs[
idx_q()];
559 template<
typename ConfigVector,
typename TangentVector>
560 void calc(JointDataDerived & data,
561 const typename Eigen::MatrixBase<ConfigVector> & qs,
562 const typename Eigen::MatrixBase<TangentVector> & vs)
const
564 calc(data,qs.derived());
566 data.v.angularRate() =
static_cast<Scalar
>(vs[
idx_v()]);
569 template<
typename Matrix6Like>
570 void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const
572 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) *
axis;
573 data.Dinv[0] = (Scalar)(1)/
axis.dot(data.U.template segment<3>(Motion::ANGULAR));
574 data.UDinv.noalias() = data.U * data.Dinv;
577 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
580 static std::string classname() {
return std::string(
"JointModelRevoluteUnaligned"); }
581 std::string shortname()
const {
return classname(); }
584 template<
typename NewScalar>
588 ReturnType res(
axis.template cast<NewScalar>());
603 #include <boost/type_traits.hpp>
607 template<
typename Scalar,
int Options>
609 :
public integral_constant<bool,true> {};
611 template<
typename Scalar,
int Options>
613 :
public integral_constant<bool,true> {};
615 template<
typename Scalar,
int Options>
617 :
public integral_constant<bool,true> {};
619 template<
typename Scalar,
int Options>
621 :
public integral_constant<bool,true> {};
625 #endif // ifndef __pinocchio_joint_revolute_unaligned_hpp__