6 #ifndef __pinocchio_multibody_constraint_generic_hpp__
7 #define __pinocchio_multibody_constraint_generic_hpp__
12 template<
int _Dim,
typename _Scalar,
int _Options>
15 typedef _Scalar Scalar;
25 typedef Eigen::Matrix<Scalar,Dim,1,Options> JointForce;
26 typedef Eigen::Matrix<Scalar,6,Dim,Options> DenseBase;
28 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType;
29 typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType;
33 template<
int Dim,
typename Scalar,
int Options>
35 {
typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };
37 template<
int Dim,
typename Scalar,
int Options,
typename MotionDerived>
39 {
typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };
41 template<
int _Dim,
typename _Scalar,
int _Options>
45 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 explicit ConstraintTpl(
const Eigen::MatrixBase<D> & _S) : S(_S)
61 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D);
66 EIGEN_STATIC_ASSERT(_Dim!=Eigen::Dynamic,
67 YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR)
73 EIGEN_STATIC_ASSERT(_Dim==Eigen::Dynamic,
74 YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR)
82 template<
typename VectorLike>
83 JointMotion __mult__(
const Eigen::MatrixBase<VectorLike> & vj)
const
85 return JointMotion(S*vj);
93 template<
typename Derived>
95 {
return (ref.S.transpose()*f.
toVector()).eval(); }
98 typename Eigen::Matrix<Scalar,NV,Eigen::Dynamic>
99 operator*(
const Eigen::MatrixBase<D> & F)
101 return (ref.S.transpose()*F).eval();
108 MatrixReturnType matrix_impl() {
return S; }
109 ConstMatrixReturnType matrix_impl()
const {
return S; }
111 int nv_impl()
const {
return (
int)S.cols(); }
113 template<
typename S2,
int O2>
114 friend typename ConstraintTpl<_Dim,_Scalar,_Options>::DenseBase
115 operator*(
const InertiaTpl<S2,O2> & Y,
const ConstraintTpl & S)
117 typedef typename ConstraintTpl::DenseBase ReturnType;
118 ReturnType res(6,S.nv());
123 template<
typename S2,
int O2>
124 friend Eigen::Matrix<_Scalar,6,_Dim>
125 operator*(
const Eigen::Matrix<S2,6,6,O2> & Ymatrix,
const ConstraintTpl & S)
127 typedef Eigen::Matrix<_Scalar,6,_Dim> ReturnType;
128 return ReturnType(Ymatrix*S.matrix());
132 DenseBase se3Action(
const SE3Tpl<Scalar,Options> & m)
const
134 DenseBase res(6,nv());
139 DenseBase se3ActionInverse(
const SE3Tpl<Scalar,Options> & m)
const
141 DenseBase res(6,nv());
146 template<
typename MotionDerived>
147 DenseBase motionAction(
const MotionDense<MotionDerived> & v)
const
149 DenseBase res(6,nv());
154 void disp_impl(std::ostream & os)
const { os <<
"S =\n" << S << std::endl;}
156 bool isEqual(
const ConstraintTpl & other)
const
168 #endif // ifndef __pinocchio_multibody_constraint_generic_hpp__