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
50 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTpl)
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)
71 explicit ConstraintTpl(
const int dim) : S(6,dim)
73 EIGEN_STATIC_ASSERT(_Dim==Eigen::Dynamic,
74 YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR)
77 template<
typename VectorLike>
78 JointMotion __mult__(
const Eigen::MatrixBase<VectorLike> & vj)
const 80 return JointMotion(S*vj);
85 const ConstraintTpl & ref;
86 Transpose(
const ConstraintTpl & ref ) : ref(ref) {}
88 template<
typename Derived>
90 {
return (ref.S.transpose()*f.
toVector()).eval(); }
93 typename Eigen::Matrix<Scalar,NV,Eigen::Dynamic>
96 return (ref.S.transpose()*F).eval();
103 MatrixReturnType matrix_impl() {
return S; }
104 ConstMatrixReturnType matrix_impl()
const {
return S; }
106 int nv_impl()
const {
return (
int)S.cols(); }
108 template<
typename S2,
int O2>
112 typedef typename ConstraintTpl::DenseBase ReturnType;
113 ReturnType res(6,S.nv());
118 template<
typename S2,
int O2>
119 friend Eigen::Matrix<_Scalar,6,_Dim>
120 operator*(
const Eigen::Matrix<S2,6,6,O2> & Ymatrix,
const ConstraintTpl & S)
122 typedef Eigen::Matrix<_Scalar,6,_Dim> ReturnType;
123 return ReturnType(Ymatrix*S.matrix());
129 DenseBase res(6,
nv());
136 DenseBase res(6,
nv());
141 template<
typename MotionDerived>
144 DenseBase res(6,
nv());
149 void disp_impl(std::ostream & os)
const { os <<
"S =\n" << S << std::endl;}
158 #endif // ifndef __pinocchio_multibody_constraint_generic_hpp__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
Return type of the ation of a Motion onto an object of type D.
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
static void motionAction(const MotionDense< MotionDerived > &v, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion on a set of motions, represented by a 6xN matrix whose columns represent a spatial...
static void se3ActionInverse(const SE3Tpl< Scalar, Options > &m, const Eigen::MatrixBase< Mat > &iV, Eigen::MatrixBase< MatRet > const &jV)
Inverse SE3 action on a set of motions, represented by a 6xN matrix whose column represent a spatial ...
Main pinocchio namespace.
Common traits structure to fully define base classes for CRTP.
static void inertiaAction(const InertiaTpl< Scalar, Options > &I, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
Action of an Inertia matrix on a set of motions, represented by a 6xN matrix whose columns represent ...
static void se3Action(const SE3Tpl< Scalar, Options > &m, const Eigen::MatrixBase< Mat > &iV, Eigen::MatrixBase< MatRet > const &jV)
SE3 action on a set of motions, represented by a 6xN matrix whose column represent a spatial motion...
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
.