6 #ifndef __pinocchio_multibody_joint_prismatic_unaligned_hpp__
7 #define __pinocchio_multibody_joint_prismatic_unaligned_hpp__
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/joint/joint-translation.hpp"
12 #include "pinocchio/multibody/joint-motion-subspace.hpp"
13 #include "pinocchio/spatial/inertia.hpp"
14 #include "pinocchio/math/matrix.hpp"
19 template<
typename Scalar,
int Options = context::Options>
20 struct MotionPrismaticUnalignedTpl;
21 typedef MotionPrismaticUnalignedTpl<context::Scalar> MotionPrismaticUnaligned;
23 template<
typename Scalar,
int Options>
29 template<
typename Scalar,
int Options,
typename MotionDerived>
35 template<
typename _Scalar,
int _Options>
38 typedef _Scalar Scalar;
43 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
44 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
45 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
46 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
47 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
48 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
49 typedef Vector3 AngularType;
50 typedef Vector3 LinearType;
51 typedef const Vector3 ConstAngularType;
52 typedef const Vector3 ConstLinearType;
53 typedef Matrix6 ActionMatrixType;
54 typedef Matrix4 HomogeneousMatrixType;
64 template<
typename _Scalar,
int _Options>
67 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
74 template<
typename Vector3Like,
typename S2>
79 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
82 inline PlainReturnType plain()
const
84 return PlainReturnType(m_axis * m_v, PlainReturnType::Vector3::Zero());
87 template<
typename OtherScalar>
93 template<
typename Derived>
96 other.linear() += m_axis * m_v;
99 template<
typename Derived>
102 other.linear().noalias() = m_axis * m_v;
103 other.angular().setZero();
106 template<
typename S2,
int O2,
typename D2>
109 v.linear().noalias() = m_v * (m.rotation() * m_axis);
110 v.angular().setZero();
113 template<
typename S2,
int O2>
117 se3Action_impl(m, res);
121 template<
typename S2,
int O2,
typename D2>
125 v.linear().noalias() = m_v * (m.rotation().transpose() * m_axis);
128 v.angular().setZero();
131 template<
typename S2,
int O2>
135 se3ActionInverse_impl(m, res);
139 template<
typename M1,
typename M2>
143 mout.linear().noalias() = v.angular().cross(m_axis);
144 mout.linear() *= m_v;
147 mout.angular().setZero();
150 template<
typename M1>
154 motionAction(v, res);
160 return internal::comparison_eq(m_axis, other.m_axis)
161 && internal::comparison_eq(m_v, other.m_v);
164 const Scalar & linearRate()
const
168 Scalar & linearRate()
173 const Vector3 & axis()
const
187 template<
typename Scalar,
int Options,
typename MotionDerived>
188 inline typename MotionDerived::MotionPlain operator+(
191 typedef typename MotionDerived::MotionPlain ReturnType;
192 return ReturnType(m1.linearRate() * m1.axis() + m2.linear(), m2.angular());
195 template<
typename MotionDerived,
typename S2,
int O2>
196 inline typename MotionDerived::MotionPlain
197 operator^(
const MotionDense<MotionDerived> & m1,
const MotionPrismaticUnalignedTpl<S2, O2> & m2)
199 return m2.motionAction(m1);
202 template<
typename Scalar,
int Options>
203 struct JointMotionSubspacePrismaticUnalignedTpl;
205 template<
typename _Scalar,
int _Options>
208 typedef _Scalar Scalar;
220 typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce;
221 typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase;
222 typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix;
223 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
225 typedef DenseBase MatrixReturnType;
226 typedef const DenseBase ConstMatrixReturnType;
228 typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
231 template<
typename Scalar,
int Options>
234 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
237 template<
typename Scalar,
int Options,
typename MotionDerived>
242 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
245 template<
typename Scalar,
int Options,
typename ForceDerived>
250 typedef Eigen::Matrix<
251 typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(
259 template<
typename Scalar,
int Options,
typename ForceSet>
265 Eigen::Transpose<const Vector3>,
266 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type>::type ReturnType;
269 template<
typename _Scalar,
int _Options>
273 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
287 template<
typename Vector3Like>
291 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
294 template<
typename Vector1Like>
295 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const
297 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1);
298 return JointMotion(m_axis, v[0]);
301 template<
typename S1,
int O1>
302 typename SE3GroupAction<JointMotionSubspacePrismaticUnalignedTpl>::ReturnType
305 typename SE3GroupAction<JointMotionSubspacePrismaticUnalignedTpl>::ReturnType res;
307 v.linear().noalias() = m.rotation() * m_axis;
308 v.angular().setZero();
312 template<
typename S1,
int O1>
313 typename SE3GroupAction<JointMotionSubspacePrismaticUnalignedTpl>::ReturnType
316 typename SE3GroupAction<JointMotionSubspacePrismaticUnalignedTpl>::ReturnType res;
318 v.linear().noalias() = m.rotation().transpose() * m_axis;
319 v.angular().setZero();
337 template<
typename ForceDerived>
338 typename ConstraintForceOp<JointMotionSubspacePrismaticUnalignedTpl, ForceDerived>::ReturnType
344 res[0] = ref.axis().dot(f.
linear());
349 template<
typename ForceSet>
350 typename ConstraintForceSetOp<JointMotionSubspacePrismaticUnalignedTpl, ForceSet>::ReturnType
351 operator*(
const Eigen::MatrixBase<ForceSet> & F)
354 ForceSet::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
356 return ref.axis().transpose() * F.template middleRows<3>(LINEAR);
371 DenseBase matrix_impl()
const
374 S.template segment<3>(LINEAR) = m_axis;
375 S.template segment<3>(ANGULAR).setZero();
379 template<
typename MotionDerived>
380 DenseBase motionAction(
const MotionDense<MotionDerived> & v)
const
383 res.template segment<3>(LINEAR).noalias() = v.angular().cross(m_axis);
384 res.template segment<3>(ANGULAR).setZero();
389 const Vector3 & axis()
const
398 bool isEqual(
const JointMotionSubspacePrismaticUnalignedTpl & other)
const
400 return internal::comparison_eq(m_axis, other.m_axis);
408 template<
typename S1,
int O1,
typename S2,
int O2>
411 typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType;
417 template<
typename S1,
int O1,
typename S2,
int O2>
428 const S1 & m = Y.mass();
429 const typename Inertia::Vector3 & c = Y.lever();
431 res.template segment<3>(Constraint::LINEAR).noalias() = m * cpu.axis();
432 res.template segment<3>(Constraint::ANGULAR).noalias() =
433 c.cross(res.template segment<3>(Constraint::LINEAR));
440 template<
typename M6Like,
typename Scalar,
int Options>
442 Eigen::MatrixBase<M6Like>,
446 typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
449 typedef typename Constraint::Vector3 Vector3;
450 typedef const typename MatrixMatrixProduct<M6LikeColsNonConst, Vector3>::type ReturnType;
456 template<
typename M6Like,
typename Scalar,
int Options>
458 Eigen::MatrixBase<M6Like>,
464 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
const Constraint & cru)
466 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
467 return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis();
472 template<
typename Scalar,
int Options>
475 template<
typename _Scalar,
int _Options>
484 typedef _Scalar Scalar;
497 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
498 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
499 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
501 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
502 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
504 typedef boost::mpl::true_ is_mimicable_t;
506 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
509 template<
typename _Scalar,
int _Options>
513 typedef _Scalar Scalar;
516 template<
typename _Scalar,
int _Options>
518 :
public JointDataBase<JointDataPrismaticUnalignedTpl<_Scalar, _Options>>
520 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
523 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
525 ConfigVector_t joint_q;
526 TangentVector_t joint_v;
540 :
joint_q(ConfigVector_t::Zero())
541 ,
joint_v(TangentVector_t::Zero())
542 , M(Transformation_t::Vector3::Zero())
543 , S(Constraint_t::Vector3::Zero())
544 , v(Constraint_t::Vector3::Zero(), (Scalar)0)
547 , UDinv(UD_t::Zero())
552 template<
typename Vector3Like>
554 :
joint_q(ConfigVector_t::Zero())
555 ,
joint_v(TangentVector_t::Zero())
556 , M(Transformation_t::Vector3::Zero())
561 , UDinv(UD_t::Zero())
566 static std::string classname()
568 return std::string(
"JointDataPrismaticUnaligned");
570 std::string shortname()
const
577 template<
typename _Scalar,
int _Options>
581 typedef _Scalar Scalar;
585 template<
typename _Scalar,
int _Options>
587 :
public JointModelBase<JointModelPrismaticUnalignedTpl<_Scalar, _Options>>
589 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
597 using Base::idx_vExtended;
598 using Base::setIndexes;
600 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
603 :
axis(Vector3::UnitX())
610 assert(
isUnitary(
axis) &&
"Translation axis is not unitary");
613 template<
typename Vector3Like>
617 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
618 assert(
isUnitary(
axis) &&
"Translation axis is not unitary");
621 JointDataDerived createData()
const
623 return JointDataDerived(
axis);
626 const std::vector<bool> hasConfigurationLimit()
const
631 const std::vector<bool> hasConfigurationLimitInTangent()
const
639 return Base::isEqual(other) && internal::comparison_eq(
axis, other.
axis);
642 template<
typename ConfigVector>
643 void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs)
const
645 data.joint_q[0] = qs[idx_q()];
646 data.M.translation().noalias() =
axis * data.joint_q[0];
649 template<
typename TangentVector>
651 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
654 data.joint_v[0] = vs[idx_v()];
655 data.v.linearRate() = data.joint_v[0];
658 template<
typename ConfigVector,
typename TangentVector>
660 JointDataDerived & data,
661 const typename Eigen::MatrixBase<ConfigVector> & qs,
662 const typename Eigen::MatrixBase<TangentVector> & vs)
const
664 calc(data, qs.derived());
666 data.joint_v[0] = vs[idx_v()];
667 data.v.linearRate() = data.joint_v[0];
670 template<
typename VectorLike,
typename Matrix6Like>
672 JointDataDerived & data,
673 const Eigen::MatrixBase<VectorLike> & armature,
674 const Eigen::MatrixBase<Matrix6Like> & I,
675 const bool update_I)
const
677 data.U.noalias() = I.template block<6, 3>(0, Inertia::LINEAR) *
axis;
679 Scalar(1) / (
axis.dot(data.U.template segment<3>(Inertia::LINEAR)) + armature[0]);
680 data.UDinv.noalias() = data.U * data.Dinv;
683 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
686 static std::string classname()
688 return std::string(
"JointModelPrismaticUnaligned");
690 std::string shortname()
const
696 template<
typename NewScalar>
700 ReturnType res(
axis.template cast<NewScalar>());
701 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
713 template<
typename Scalar,
int Options>
720 #include <boost/type_traits.hpp>
724 template<
typename Scalar,
int Options>
726 :
public integral_constant<bool, true>
730 template<
typename Scalar,
int Options>
732 :
public integral_constant<bool, true>
736 template<
typename Scalar,
int Options>
738 :
public integral_constant<bool, true>
742 template<
typename Scalar,
int Options>
744 :
public integral_constant<bool, true>
ConstLinearType linear() const
Return the linear part of the force vector.
Main pinocchio namespace.
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
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.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Return type of the Constraint::Transpose * Force operation.
Return type of the Constraint::Transpose * ForceSet operation.
Vector3 axis
3d main axis of the joint.
JointModelPrismaticUnalignedTpl< NewScalar, Options > 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,...
Common traits structure to fully define base classes for CRTP.