C++ (Cpp) Inertia::matrix示例

示例#1

文件： constraint.cpp 项目： thomasfla/pinocchio

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;
}