6 #ifndef __pinocchio_multibody_constraint_base_hpp__
7 #define __pinocchio_multibody_constraint_base_hpp__
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/spatial/fwd.hpp"
11 #include "pinocchio/spatial/motion.hpp"
12 #include "pinocchio/spatial/act-on-set.hpp"
17 #define PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,TYPENAME) \
18 typedef TYPENAME traits<DERIVED>::Scalar Scalar; \
19 typedef TYPENAME traits<DERIVED>::JointMotion JointMotion; \
20 typedef TYPENAME traits<DERIVED>::JointForce JointForce; \
21 typedef TYPENAME traits<DERIVED>::DenseBase DenseBase; \
22 typedef TYPENAME traits<DERIVED>::MatrixReturnType MatrixReturnType; \
23 typedef TYPENAME traits<DERIVED>::ConstMatrixReturnType ConstMatrixReturnType; \
24 enum { LINEAR = traits<DERIVED>::LINEAR, ANGULAR = traits<DERIVED>::ANGULAR }; \
25 enum { Options = traits<DERIVED>::Options };
27 #define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED) PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,typename)
28 #define PINOCCHIO_CONSTRAINT_TYPEDEF(DERIVED) PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,PINOCCHIO_EMPTY_ARG)
34 template<
class Constra
intDerived,
typename Force>
37 typedef ReturnTypeNotDefined ReturnType;
41 template<
class Constra
intDerived,
typename ForceSet>
44 typedef ReturnTypeNotDefined ReturnType;
47 template<
class Derived>
51 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(Derived)
54 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
56 Derived & derived() {
return *
static_cast<Derived*
>(
this); }
57 const Derived & derived()
const {
return *
static_cast<const Derived*
>(
this); }
59 template<
typename VectorLike>
60 JointMotion operator*(
const Eigen::MatrixBase<VectorLike> & vj)
const
61 {
return derived().__mult__(vj); }
63 MatrixReturnType matrix() {
return derived().matrix_impl(); }
64 ConstMatrixReturnType matrix()
const {
return derived().matrix_impl(); }
66 int nv()
const {
return derived().nv_impl(); }
68 static int rows() {
return 6; }
69 int cols()
const {
return nv(); }
71 template<
class OtherDerived>
73 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
74 {
return matrix().isApprox(other.matrix(),prec); }
76 void disp(std::ostream & os)
const { derived().disp_impl(os); }
83 typename SE3GroupAction<Derived>::ReturnType
86 return derived().se3Action(m);
89 typename SE3GroupAction<Derived>::ReturnType
92 return derived().se3ActionInverse(m);
95 template<
typename MotionDerived>
96 typename MotionAlgebraAction<Derived,MotionDerived>::ReturnType
99 return derived().motionAction(v);
104 return derived().isEqual(other.derived());
110 template<
typename Scalar,
int Options,
typename Constra
intDerived>
116 constraint.derived());
120 template<
typename MatrixDerived,
typename Constra
intDerived>
121 typename MultiplicationOp<Eigen::MatrixBase<MatrixDerived>,ConstraintDerived>::ReturnType
126 constraint.derived());
131 #endif // ifndef __pinocchio_multibody_constraint_base_hpp__