6 #ifndef __pinocchio_multibody_joint_prismatic_hpp__
7 #define __pinocchio_multibody_joint_prismatic_hpp__
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/joint-motion-subspace.hpp"
12 #include "pinocchio/spatial/inertia.hpp"
13 #include "pinocchio/spatial/spatial-axis.hpp"
14 #include "pinocchio/utils/axis-label.hpp"
19 template<
typename Scalar,
int Options,
int _axis>
20 struct MotionPrismaticTpl;
22 template<
typename Scalar,
int Options,
int axis>
28 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
34 template<
typename _Scalar,
int _Options,
int _axis>
37 typedef _Scalar Scalar;
42 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
43 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
44 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
45 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
46 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
47 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
48 typedef Vector3 AngularType;
49 typedef Vector3 LinearType;
50 typedef const Vector3 ConstAngularType;
51 typedef const Vector3 ConstLinearType;
52 typedef Matrix6 ActionMatrixType;
53 typedef Matrix4 HomogeneousMatrixType;
63 template<
typename _Scalar,
int _Options,
int _axis>
66 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85 inline PlainReturnType plain()
const
90 template<
typename OtherScalar>
96 template<
typename Derived>
99 typedef typename MotionDense<Derived>::Scalar OtherScalar;
100 other.linear()[_axis] += (OtherScalar)m_v;
103 template<
typename MotionDerived>
106 for (Eigen::DenseIndex k = 0; k < 3; ++k)
108 other.linear()[k] = k == axis ? m_v : Scalar(0);
110 other.angular().setZero();
113 template<
typename S2,
int O2,
typename D2>
116 v.angular().setZero();
117 v.linear().noalias() = m_v * (m.rotation().col(axis));
120 template<
typename S2,
int O2>
124 se3Action_impl(m, res);
128 template<
typename S2,
int O2,
typename D2>
132 v.linear().noalias() = m_v * (m.rotation().transpose().col(axis));
135 v.angular().setZero();
138 template<
typename S2,
int O2>
142 se3ActionInverse_impl(m, res);
146 template<
typename M1,
typename M2>
150 CartesianAxis3::alphaCross(-m_v, v.angular(), mout.linear());
153 mout.angular().setZero();
156 template<
typename M1>
160 motionAction(v, res);
164 Scalar & linearRate()
168 const Scalar & linearRate()
const
175 return internal::comparison_eq(m_v, other.m_v);
182 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
183 typename MotionDerived::MotionPlain operator+(
186 typename MotionDerived::MotionPlain res(m2);
191 template<
typename MotionDerived,
typename S2,
int O2,
int axis>
192 EIGEN_STRONG_INLINE
typename MotionDerived::MotionPlain
193 operator^(
const MotionDense<MotionDerived> & m1,
const MotionPrismaticTpl<S2, O2, axis> & m2)
195 return m2.motionAction(m1);
198 template<
typename Scalar,
int Options,
int axis>
199 struct TransformPrismaticTpl;
201 template<
typename _Scalar,
int _Options,
int _axis>
211 typedef _Scalar Scalar;
213 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
214 typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
215 typedef typename Matrix3::IdentityReturnType AngularType;
216 typedef AngularType AngularRef;
217 typedef AngularType ConstAngularRef;
218 typedef Vector3 LinearType;
219 typedef const Vector3 LinearRef;
220 typedef const Vector3 ConstLinearRef;
225 template<
typename Scalar,
int Options,
int axis>
231 template<
typename _Scalar,
int _Options,
int axis>
234 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
244 : m_displacement(displacement)
248 PlainType plain()
const
250 PlainType res(PlainType::Identity());
251 res.rotation().setIdentity();
252 res.translation()[axis] = m_displacement;
257 operator PlainType()
const
262 template<
typename S2,
int O2>
263 typename SE3GroupAction<TransformPrismaticTpl>::ReturnType
266 typedef typename SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType;
268 res.translation()[axis] += m_displacement;
273 const Scalar & displacement()
const
275 return m_displacement;
277 Scalar & displacement()
279 return m_displacement;
282 ConstLinearRef translation()
const
286 AngularType rotation()
const
288 return AngularType(3, 3);
293 return internal::comparison_eq(m_displacement, other.m_displacement);
297 Scalar m_displacement;
300 template<
typename Scalar,
int Options,
int axis>
303 template<
typename _Scalar,
int _Options,
int axis>
306 typedef _Scalar Scalar;
317 typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce;
318 typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase;
319 typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix;
321 typedef DenseBase MatrixReturnType;
322 typedef const DenseBase ConstMatrixReturnType;
324 typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
327 template<
typename Scalar,
int Options,
int axis>
330 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
333 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
336 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
339 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
343 ForceDerived>::ConstLinearType::template ConstFixedSegmentReturnType<1>::Type ReturnType;
346 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
349 typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType;
352 template<
typename _Scalar,
int _Options,
int axis>
356 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
367 template<
typename Vector1Like>
368 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const
370 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1);
371 assert(v.size() == 1);
372 return JointMotion(v[0]);
375 template<
typename S2,
int O2>
376 typename SE3GroupAction<JointMotionSubspacePrismaticTpl>::ReturnType
379 typename SE3GroupAction<JointMotionSubspacePrismaticTpl>::ReturnType res;
381 v.linear() = m.rotation().col(axis);
382 v.angular().setZero();
386 template<
typename S2,
int O2>
387 typename SE3GroupAction<JointMotionSubspacePrismaticTpl>::ReturnType
390 typename SE3GroupAction<JointMotionSubspacePrismaticTpl>::ReturnType res;
392 v.linear() = m.rotation().transpose().col(axis);
393 v.angular().setZero();
410 template<
typename ForceDerived>
411 typename ConstraintForceOp<JointMotionSubspacePrismaticTpl, ForceDerived>::ReturnType
414 return f.
linear().template segment<1>(axis);
418 template<
typename Derived>
419 typename ConstraintForceSetOp<JointMotionSubspacePrismaticTpl, Derived>::ReturnType
420 operator*(
const Eigen::MatrixBase<Derived> & F)
422 assert(F.rows() == 6);
423 return F.row(LINEAR + axis);
438 DenseBase matrix_impl()
const
446 template<
typename MotionDerived>
447 typename MotionAlgebraAction<JointMotionSubspacePrismaticTpl, MotionDerived>::ReturnType
448 motionAction(
const MotionDense<MotionDerived> & m)
const
450 typename MotionAlgebraAction<JointMotionSubspacePrismaticTpl, MotionDerived>::ReturnType res;
451 MotionRef<DenseBase> v(res);
456 bool isEqual(
const JointMotionSubspacePrismaticTpl &)
const
463 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
466 typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType;
472 template<
typename S1,
int O1,
typename S2,
int O2>
483 const S1 &m = Y.mass(), &y = Y.lever()[1], &z = Y.lever()[2];
484 res << m, S1(0), S1(0), S1(0), m * z, -m * y;
490 template<
typename S1,
int O1,
typename S2,
int O2>
501 const S1 &m = Y.mass(), &x = Y.lever()[0], &z = Y.lever()[2];
503 res << S1(0), m, S1(0), -m * z, S1(0), m * x;
509 template<
typename S1,
int O1,
typename S2,
int O2>
520 const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1];
522 res << S1(0), S1(0), m, m * y, -m * x, S1(0);
529 template<
typename M6Like,
typename S2,
int O2,
int axis>
532 typedef typename M6Like::ConstColXpr ReturnType;
538 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
540 Eigen::MatrixBase<M6Like>,
546 static inline ReturnType
547 run(
const Eigen::MatrixBase<M6Like> & Y,
const Constraint & )
549 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
550 return Y.derived().col(Inertia::LINEAR + axis);
555 template<
typename _Scalar,
int _Options,
int _axis>
558 typedef _Scalar Scalar;
567 template<
typename _Scalar,
int _Options,
int axis>
576 typedef _Scalar Scalar;
589 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
590 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
591 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
593 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
594 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
596 typedef boost::mpl::true_ is_mimicable_t;
598 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
601 template<
typename _Scalar,
int _Options,
int axis>
605 typedef _Scalar Scalar;
608 template<
typename _Scalar,
int _Options,
int axis>
612 typedef _Scalar Scalar;
615 template<
typename _Scalar,
int _Options,
int axis>
617 :
public JointDataBase<JointDataPrismaticTpl<_Scalar, _Options, axis>>
619 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
622 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
624 ConfigVector_t joint_q;
625 TangentVector_t joint_v;
639 :
joint_q(ConfigVector_t::Zero())
640 ,
joint_v(TangentVector_t::Zero())
645 , UDinv(UD_t::Zero())
650 static std::string classname()
652 return std::string(
"JointDataP") + axisLabel<axis>();
654 std::string shortname()
const
661 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
667 template<
typename _Scalar,
int _Options,
int axis>
669 :
public JointModelBase<JointModelPrismaticTpl<_Scalar, _Options, axis>>
671 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
679 using Base::idx_vExtended;
680 using Base::setIndexes;
682 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
684 JointDataDerived createData()
const
686 return JointDataDerived();
689 const std::vector<bool> hasConfigurationLimit()
const
694 const std::vector<bool> hasConfigurationLimitInTangent()
const
699 template<
typename ConfigVector>
700 void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs)
const
702 data.joint_q[0] = qs[idx_q()];
703 data.M.displacement() = data.joint_q[0];
706 template<
typename TangentVector>
708 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
711 data.joint_v[0] = vs[idx_v()];
712 data.v.linearRate() = data.joint_v[0];
715 template<
typename ConfigVector,
typename TangentVector>
717 JointDataDerived & data,
718 const typename Eigen::MatrixBase<ConfigVector> & qs,
719 const typename Eigen::MatrixBase<TangentVector> & vs)
const
721 calc(data, qs.derived());
723 data.joint_v[0] = vs[idx_v()];
724 data.v.linearRate() = data.joint_v[0];
727 template<
typename VectorLike,
typename Matrix6Like>
729 JointDataDerived & data,
730 const Eigen::MatrixBase<VectorLike> & armature,
731 const Eigen::MatrixBase<Matrix6Like> & I,
732 const bool update_I)
const
734 data.U = I.col(Inertia::LINEAR + axis);
735 data.Dinv[0] = Scalar(1) / (I(Inertia::LINEAR + axis, Inertia::LINEAR + axis) + armature[0]);
736 data.UDinv.noalias() = data.U * data.Dinv[0];
739 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
742 static std::string classname()
744 return std::string(
"JointModelP") + axisLabel<axis>();
746 std::string shortname()
const
751 Vector3 getMotionAxis()
const
756 return Vector3::UnitX();
758 return Vector3::UnitY();
760 return Vector3::UnitZ();
762 assert(
false &&
"must never happen");
768 template<
typename NewScalar>
773 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
779 typedef JointPrismaticTpl<context::Scalar, context::Options, 0> JointPX;
780 typedef JointDataPrismaticTpl<context::Scalar, context::Options, 0> JointDataPX;
781 typedef JointModelPrismaticTpl<context::Scalar, context::Options, 0> JointModelPX;
783 typedef JointPrismaticTpl<context::Scalar, context::Options, 1> JointPY;
784 typedef JointDataPrismaticTpl<context::Scalar, context::Options, 1> JointDataPY;
785 typedef JointModelPrismaticTpl<context::Scalar, context::Options, 1> JointModelPY;
787 typedef JointPrismaticTpl<context::Scalar, context::Options, 2> JointPZ;
788 typedef JointDataPrismaticTpl<context::Scalar, context::Options, 2> JointDataPZ;
789 typedef JointModelPrismaticTpl<context::Scalar, context::Options, 2> JointModelPZ;
791 template<
typename Scalar,
int Options,
int axis>
798 #include <boost/type_traits.hpp>
802 template<
typename Scalar,
int Options,
int axis>
804 :
public integral_constant<bool, true>
808 template<
typename Scalar,
int Options,
int axis>
810 :
public integral_constant<bool, true>
814 template<
typename Scalar,
int Options,
int axis>
816 :
public integral_constant<bool, true>
820 template<
typename Scalar,
int Options,
int axis>
822 :
public integral_constant<bool, true>
ConstLinearType linear() const
Return the linear part of the force vector.
Main pinocchio namespace.
JointDataTpl< Scalar, Options, JointCollectionTpl >::TangentVector_t joint_v(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint velocity vector.
JointDataTpl< Scalar, Options, JointCollectionTpl >::ConfigVector_t joint_q(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint configuration vector.
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.
Return type of the Constraint::Transpose * ForceSet operation.
JointModelPrismaticTpl< NewScalar, Options, axis > cast() const
Return type of the ation of a Motion onto an object of type D.
Forward declaration of the multiplication operation return type. Should be overloaded,...
Base class for rigid transformation.
Common traits structure to fully define base classes for CRTP.