bool testConstraintRX() { using namespace se3; Inertia Y = Inertia::Random(); JointRX::ConstraintRevolute S; std::cout << "Y = \n" << Y.matrix() << std::endl; std::cout << "S = \n" << ((ConstraintXd)S).matrix() << std::endl; ForceSet F(1); F.block(0,1) = Y*S; std::cout << "Y*S = \n" << (Y*S).matrix() << std::endl; std::cout << "F=Y*S = \n" << F.matrix() << std::endl; assert( F.matrix().isApprox( Y.toMatrix().col(3) ) ); ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() ); Eigen::MatrixXd StF2 = S.transpose()*F2.block(5,3); assert( StF2.isApprox( ConstraintXd(S).matrix().transpose()*F2.matrix().block(0,5,6,3) ) ); return true; }