C++ BodyNode::getTransform方法代码示例

OldConstraint* MyWindow::addTailConstraint() {
    BodyNode *bd = mWorld->getSkeleton(0)->getBodyNode("link 10");
    Eigen::Vector3d offset(0.0, -0.025, 0.0);
    Eigen::Vector3d target = bd->getTransform() * offset;
//    OldBallJointConstraint *bj = new OldBallJointConstraint(bd, offset, target);
//    mWorld->getConstraintHandler()->addConstraint(bj);
//    return bj;
    return NULL;
}

void CollisionInterface::postProcess() {
    mContacts.clear();
    int numContacts = mCollisionChecker->getNumContacts();
    mContacts.resize(numContacts);
    for (int i = 0; i < numContacts; i++) {
        mContacts[i].point = mCollisionChecker->getContact(i).point;
        mContacts[i].normal = mCollisionChecker->getContact(i).normal;
        mContacts[i].rb1 = mNodeMap[mCollisionChecker->getContact(i).bodyNode1];
        mContacts[i].rb2 = mNodeMap[mCollisionChecker->getContact(i).bodyNode2];
        if(mContacts[i].rb1 == NULL) {
          BodyNode* bd = mCollisionChecker->getContact(i).bodyNode1;
          Vector3d localPoint = bd->getTransform().inverse() * mContacts[i].point;
          mContacts[i].pinataVelocity = bd->getWorldLinearVelocity(localPoint);
        } else if(mContacts[i].rb2 == NULL) {
          BodyNode* bd = mCollisionChecker->getContact(i).bodyNode2;
          Vector3d localPoint = bd->getTransform().inverse() * mContacts[i].point;
          mContacts[i].pinataVelocity = bd->getWorldLinearVelocity(localPoint);
        } else {
          mContacts[i].pinataVelocity.setZero();
        }        
    }
}

//==============================================================================
TEST(FORWARD_KINEMATICS, YAW_ROLL)
{
  // Checks forward kinematics for two DoF arm manipulators.
  // NOTE: The following is the reference frame description of the world
  //       frame. The x-axis is into the page, z-axis is to the top of the
  //       page and the y-axis is to the left. At the zero angle, the links
  //       are parallel to the z-axis and face the +x-axis.

  // Create the world
  const double l1 = 1.5, l2 = 1.0;
  SkeletonPtr robot = createTwoLinkRobot(Vector3d(0.3, 0.3, l1), DOF_YAW,
                                         Vector3d(0.3, 0.3, l2), DOF_ROLL);

  // Set the test cases with the joint values and the expected end-effector
  // positions
  const std::size_t numTests = 2;
  double temp = sqrt(0.5*l2*l2);
  Vector2d joints [numTests] = { Vector2d( constantsd::pi()/4.0,  constantsd::pi()/2.0),
                                 Vector2d(-constantsd::pi()/4.0, -constantsd::pi()/4.0) };
  Vector3d expectedPos [numTests] = { Vector3d(temp, -temp, l1),
                                      Vector3d(temp / sqrt(2.0),
                                      temp / sqrt(2.0), l1+temp) };

  // Check each case by setting the joint values and obtaining the end-effector
  // position
  for (std::size_t i = 0; i < numTests; i++)
  {
    robot->setPositions(Eigen::VectorXd(joints[i]));
    BodyNode* bn = robot->getBodyNode("ee");
    Vector3d actual = bn->getTransform().translation();
    bool equality = equals(actual, expectedPos[i], 1e-3);
    EXPECT_TRUE(equality);
    if(!equality)
    {
      std::cout << "Joint values: " << joints[i].transpose() << std::endl;
      std::cout << "Actual pos: " << actual.transpose() << std::endl;
      std::cout << "Expected pos: " <<  expectedPos[i].transpose() << std::endl;
    }
  }
}

//==============================================================================
void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/)
{
  using namespace std;
  using namespace Eigen;
  using namespace dart::math;
  using namespace dart::collision;
  using namespace dart::constraint;
  using namespace dart::dynamics;
  using namespace dart::simulation;
  using namespace dart::io;

  //----------------------------------------------------------------------------
  // Settings
  //----------------------------------------------------------------------------
  // Number of random state tests for each skeletons
#ifndef NDEBUG  // Debug mode
  // std::size_t testCount = 1;
#else
  // std::size_t testCount = 1;
#endif

  WorldPtr world = World::create();
  EXPECT_TRUE(world != nullptr);
  world->setGravity(Vector3d(0.0, -10.00, 0.0));
  world->setTimeStep(0.001);
  world->getConstraintSolver()->setCollisionDetector(
        DARTCollisionDetector::create());

  SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
  BodyNode* sphere = sphereSkel->getBodyNode(0);
  Joint* sphereJoint = sphere->getParentJoint();
  sphereJoint->setVelocity(3, Random::uniform(-2.0, 2.0));  // x-axis
  sphereJoint->setVelocity(5, Random::uniform(-2.0, 2.0));  // z-axis
  world->addSkeleton(sphereSkel);
  EXPECT_EQ(sphereSkel->getGravity(), world->getGravity());
  assert(sphere);

  SkeletonPtr boxSkel = createBox(Vector3d(1.0, 1.0, 1.0),
                                  Vector3d(0.0, 1.0, 0.0));
  BodyNode* box = boxSkel->getBodyNode(0);
  Joint* boxJoint = box->getParentJoint();
  boxJoint->setVelocity(3, Random::uniform(-2.0, 2.0));  // x-axis
  boxJoint->setVelocity(5, Random::uniform(-2.0, 2.0));  // z-axis
//  world->addSkeleton(boxSkel);
//  EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
//  assert(box);

  SkeletonPtr groundSkel = createGround(Vector3d(10000.0, 0.1, 10000.0),
                                      Vector3d(0.0, -0.05, 0.0));
  groundSkel->setMobile(false);
  // BodyNode* ground = groundSkel->getBodyNode(0);
  world->addSkeleton(groundSkel);
  EXPECT_EQ(groundSkel->getGravity(), world->getGravity());
  // assert(ground);

  EXPECT_EQ((int)world->getNumSkeletons(), 2);

  // Lower and upper bound of configuration for system
  // double lb = -1.5 * constantsd::pi();
  // double ub =  1.5 * constantsd::pi();

  int maxSteps = 500;
  for (int i = 0; i < maxSteps; ++i)
  {
//    Vector3d pos1 = sphere->getWorldTransform().translation();
//    Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);

//    std::cout << "pos1:" << pos1.transpose() << std::endl;
//    std::cout << "vel1:" << vel1.transpose() << std::endl;

    if (!world->checkCollision())
    {
      world->step();
      continue;
    }

    // for (std::size_t j = 0; j < cd->getNumContacts(); ++j)
    // {
      // Contact contact = cd->getContact(j);
      // Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
      // Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);

      // std::cout << "pos1:" << pos1.transpose() << std::endl;
      // std::cout << "vel1:" << vel1.transpose() << std::endl;
    // }

    world->step();

    const auto& result = world->getConstraintSolver()->getLastCollisionResult();

    for (const auto& contact : result.getContacts())
    {
      Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
      Vector3d vel1 = sphere->getLinearVelocity(pos1);

//      std::cout << "pos1:" << pos1.transpose() << std::endl;

//      std::cout << "pos1[1]: " << pos1[1] << std::endl;

//.........这里部分代码省略.........

//==============================================================================
void ConstraintTest::SingleContactTest(const std::string& _fileName)
{
  using namespace std;
  using namespace Eigen;
  using namespace dart::math;
  using namespace dart::collision;
  using namespace dart::constraint;
  using namespace dart::dynamics;
  using namespace dart::simulation;
  using namespace dart::utils;

  //----------------------------------------------------------------------------
  // Settings
  //----------------------------------------------------------------------------
  // Number of random state tests for each skeletons
#ifndef NDEBUG  // Debug mode
  int testCount = 1;
#else
  int testCount = 1;
#endif

  World* world = new World;
  EXPECT_TRUE(world != NULL);
  world->setGravity(Vector3d(0.0, -10.00, 0.0));
  world->setTimeStep(0.001);
  world->getConstraintSolver()->setCollisionDetector(
        new DARTCollisionDetector());

  Skeleton* sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
  BodyNode* sphere = sphereSkel->getBodyNode(0);
  Joint* sphereJoint = sphere->getParentJoint();
  sphereJoint->setVelocity(3, random(-2.0, 2.0));  // x-axis
  sphereJoint->setVelocity(5, random(-2.0, 2.0));  // z-axis
  world->addSkeleton(sphereSkel);
  EXPECT_EQ(sphereSkel->getGravity(), world->getGravity());
  assert(sphere);

  Skeleton* boxSkel = createBox(Vector3d(1.0, 1.0, 1.0),
                                Vector3d(0.0, 1.0, 0.0));
  BodyNode* box = boxSkel->getBodyNode(0);
  Joint* boxJoint = box->getParentJoint();
  boxJoint->setVelocity(3, random(-2.0, 2.0));  // x-axis
  boxJoint->setVelocity(5, random(-2.0, 2.0));  // z-axis
//  world->addSkeleton(boxSkel);
//  EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
//  assert(box);

  Skeleton* groundSkel = createGround(Vector3d(10000.0, 0.1, 10000.0),
                                      Vector3d(0.0, -0.05, 0.0));
  groundSkel->setMobile(false);
  BodyNode* ground = groundSkel->getBodyNode(0);
  world->addSkeleton(groundSkel);
  EXPECT_EQ(groundSkel->getGravity(), world->getGravity());
  assert(ground);

  EXPECT_EQ(world->getNumSkeletons(), 2);

  ConstraintSolver* cs = world->getConstraintSolver();
  CollisionDetector* cd = cs->getCollisionDetector();

  // Lower and upper bound of configuration for system
  double lb = -1.5 * DART_PI;
  double ub =  1.5 * DART_PI;

  int maxSteps = 500;
  for (int i = 0; i < maxSteps; ++i)
  {
//    Vector3d pos1 = sphere->getWorldTransform().translation();
//    Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);

//    std::cout << "pos1:" << pos1.transpose() << std::endl;
//    std::cout << "vel1:" << vel1.transpose() << std::endl;

    cd->detectCollision(true, true);
    if (cd->getNumContacts() == 0)
    {
      world->step();
      continue;
    }

    for (int j = 0; j < cd->getNumContacts(); ++j)
    {
      Contact contact = cd->getContact(j);
      Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
      Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);

//      std::cout << "pos1:" << pos1.transpose() << std::endl;
//      std::cout << "vel1:" << vel1.transpose() << std::endl;
    }

    world->step();

    for (int j = 0; j < cd->getNumContacts(); ++j)
    {
      Contact contact = cd->getContact(j);
      Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
      Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);

//      std::cout << "pos1:" << pos1.transpose() << std::endl;
//.........这里部分代码省略.........