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

void MyWindow::initIK()
{
    // create variables
    for (int i = 0; i < mSkel->getNumDofs(); i++) {
        Var *v = new Var(mSkel->getDof(i)->getValue(), mSkel->getDof(i)->getMin(), mSkel->getDof(i)->getMax());
        mVariables.push_back(v);
    }

    // create objBox
    mObjBox = new ObjectiveBox(mVariables.size());

    // create constraints
    BodyNode *node = mSkel->getNode("fullbody1_h_heel_left");
    Vector3d offset(0, 0, 0);
    Vector3d target = xformHom(node->getWorldTransform(), offset);
    PositionConstraint* pos = new PositionConstraint(mVariables, mSkel, node, offset, target);
    mObjBox->add(pos);

    node = mSkel->getNode("fullbody1_h_heel_right");
    target = xformHom(node->getWorldTransform(), offset);
    pos = new PositionConstraint(mVariables, mSkel, node, offset, target);
    mObjBox->add(pos);
}

//==============================================================================
void DynamicsTest::compareAccelerations(const std::string& _fileName)
{
  using namespace std;
  using namespace Eigen;
  using namespace dart;
  using namespace math;
  using namespace dynamics;
  using namespace simulation;
  using namespace utils;

  //----------------------------- Settings -------------------------------------
  const double TOLERANCE = 1.0e-2;
#ifndef NDEBUG  // Debug mode
  int nRandomItr = 2;
#else
  int nRandomItr = 10;
#endif
  double qLB   = -0.5 * DART_PI;
  double qUB   =  0.5 * DART_PI;
  double dqLB  = -0.5 * DART_PI;
  double dqUB  =  0.5 * DART_PI;
  double ddqLB = -0.5 * DART_PI;
  double ddqUB =  0.5 * DART_PI;
  Vector3d gravity(0.0, -9.81, 0.0);
  double timeStep = 1.0e-6;

  // load skeleton
  World* world = SkelParser::readWorld(_fileName);
  assert(world != NULL);
  world->setGravity(gravity);
  world->setTimeStep(timeStep);

  //------------------------------ Tests ---------------------------------------
  for (int i = 0; i < world->getNumSkeletons(); ++i)
  {
    Skeleton* skeleton = world->getSkeleton(i);
    assert(skeleton != NULL);
    int dof = skeleton->getNumGenCoords();

    for (int j = 0; j < nRandomItr; ++j)
    {
      // Generate a random state and ddq
      VectorXd q   = VectorXd(dof);
      VectorXd dq  = VectorXd(dof);
      VectorXd ddq = VectorXd(dof);
      for (int k = 0; k < dof; ++k)
      {
        q[k]   = math::random(qLB,   qUB);
        dq[k]  = math::random(dqLB,  dqUB);
        ddq[k] = math::random(ddqLB, ddqUB);

//        q[k]   = 0.0;
//        dq[k]  = 0.0;
//        ddq[k] = 0.0;
      }
      VectorXd x = VectorXd::Zero(dof * 2);
      x << q, dq;
      skeleton->setState(x, true, true, false);
      skeleton->setGenAccs(ddq, true);

      // Integrate state
      skeleton->integrateConfigs(timeStep);
      skeleton->integrateGenVels(timeStep);
      VectorXd qNext  = skeleton->getConfigs();
      VectorXd dqNext = skeleton->getGenVels();
      VectorXd xNext  = VectorXd::Zero(dof * 2);
      xNext << qNext, dqNext;

      // For each body node
      for (int k = 0; k < skeleton->getNumBodyNodes(); ++k)
      {
        BodyNode* bn = skeleton->getBodyNode(k);
        int nDepGenCoord = bn->getNumDependentGenCoords();

        // Calculation of velocities and Jacobian at k-th time step
        skeleton->setState(x, true, true, false);
        skeleton->setGenAccs(ddq, true);
        Vector6d vBody1  = bn->getBodyVelocity();
        Vector6d vWorld1 = bn->getWorldVelocity();
        MatrixXd JBody1  = bn->getBodyJacobian();
        MatrixXd JWorld1 = bn->getWorldJacobian();
        Isometry3d T1    = bn->getWorldTransform();

        // Get accelerations and time derivatives of Jacobians at k-th time step
        Vector6d aBody1   = bn->getBodyAcceleration();
        Vector6d aWorld1  = bn->getWorldAcceleration();
        MatrixXd dJBody1  = bn->getBodyJacobianTimeDeriv();
        MatrixXd dJWorld1 = bn->getWorldJacobianTimeDeriv();

        // Calculation of velocities and Jacobian at (k+1)-th time step
        skeleton->setState(xNext, true, true, false);
        skeleton->setGenAccs(ddq, true);
        Vector6d vBody2  = bn->getBodyVelocity();
        Vector6d vWorld2 = bn->getWorldVelocity();
        MatrixXd JBody2  = bn->getBodyJacobian();
        MatrixXd JWorld2 = bn->getWorldJacobian();
        Isometry3d T2    = bn->getWorldTransform();

        // Get accelerations and time derivatives of Jacobians at k-th time step
//.........这里部分代码省略.........

/* ********************************************************************************************* */
TEST(KINEMATICS, TRANS_AND_DERIV) {
    using namespace Eigen;
    using namespace kinematics;
  
    FileInfoSkel<Skeleton> modelFile;
    bool loadModelResult = modelFile.loadFile(DART_DATA_PATH"skel/SehoonVSK3.vsk");
    ASSERT_TRUE(loadModelResult);
    
    Skeleton* skel = modelFile.getSkel();
    EXPECT_TRUE(skel != NULL);
    /* LOG(INFO) << "# Dofs = " << skel->getNumDofs(); */
    /* LOG(INFO) << "# Nodes  = " << skel->getNumNodes(); */


    FileInfoDof dofFile(skel);
    bool loadDofResult = dofFile.loadFile(DART_DATA_PATH"dof/init_Tpose.dof");
    ASSERT_TRUE(loadDofResult);
    /* LOG(INFO) << "# frames = " << dofFile.getNumFrames(); */

    VectorXd pose = dofFile.getPoseAtFrame(0);
    skel->setPose(pose, true, true);

    const int nodeIndex = 1;
    BodyNode* node = skel->getNode(nodeIndex);
    EXPECT_TRUE(node != NULL);

    const double TOLERANCE = 0.000001;

    // Check the one global transform matrix
    const Matrix4d& W = skel->getNode(20)->getWorldTransform();
    Matrix4d W_truth;
    W_truth << -0.110662, 0.991044, 0.074734, 0.642841
        , -0.987564, -0.118099, 0.103775, 0.327496
        , 0.111672, -0.0623207, 0.991789, -0.12855
        , 0, 0, 0, 1;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            EXPECT_NEAR(W(i, j), W_truth(i, j), TOLERANCE);
        }
    }

    // Check the derivative of one global transform matrix
    // const Matrix4d& Wq = skel->getNode(10)->mWq.at(4);
    const Matrix4d& Wq = skel->getNode(10)->getDerivWorldTransform(4);
    Matrix4d Wq_truth;
    Wq_truth << 0.121382, 0.413015, 0.902378, 0.161838
        ,-0.0175714, 0.00698451, 0.0149899, 0.00571836
        ,-0.992336, 0.0556535, 0.107702, 0.175059
        ,0, 0, 0, 0;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            EXPECT_NEAR(Wq(i, j), Wq_truth(i, j), TOLERANCE);
        }
    }


    // Check Jacobians
    BodyNode *nodecheck = skel->getNode(23);

    // Linear Jacobian
    const MatrixXd Jv = nodecheck->getJacobianLinear();
    MatrixXd Jv_truth = MatrixXd::Zero(Jv.rows(), Jv.cols());
    Jv_truth<<1, 0, 0, 0.013694, -0.0194371, -0.422612, 0.0157281, -0.00961473, -0.421925, 0.0026645, -0.0048767, -0.179914, -0.0013539, -0.00132969, -0.00885535, 
        0, 1, 0, 0.0321939, 0.00226492, -0.135736, 0.0330525, 0.00462613, -0.135448, 0.0181491, 0.00758086, -0.122187, 0.00532694, 0.0049221, -0.128917,
        0, 0, 1, 0.423948, 0.130694, 0.0166546, 0.42642, 0.118176, 0.0221309, 0.180296, 0.120584, 0.0105059, 0.0838259, 0.081304, 0.00719314;
    for (int i = 0; i < Jv.rows(); i++) {
        for (int j = 0; j < Jv.cols(); j++) {
            EXPECT_NEAR(Jv(i, j), Jv_truth(i, j), TOLERANCE);
        }
    }

    // Angular Jacobian
    const MatrixXd Jwbody = nodecheck->getWorldTransform().topLeftCorner<3,3>().transpose()
        * nodecheck->getJacobianAngular();
    MatrixXd Jwbody_truth = MatrixXd::Zero(Jwbody.rows(), Jwbody.cols());
    Jwbody_truth << 0, 0, 0, 0.0818662, 0.996527, 0.000504051, 0.110263, 0.993552, 0.0249018, 0.0772274, 0.995683, 0.0423836, 0.648386, 0.628838, 0.0338389,
        0, 0, 0, -0.995079, 0.082001, -0.0518665, -0.992054, 0.111479, -0.0554717, -0.996033, 0.078601, -0.0313861, -0.629201, 0.649145, -0.00994729,
        0, 0, 0, -0.0518265, 0.00391001, 0.998406, -0.0579139, -0.0187244, 0.998021, -0.0343359, -0.0398718, 0.998564, -0.0262454, -0.0235626, 0.999159;

    for (int i = 0; i < Jwbody.rows(); i++) {
        for (int j = 0; j < Jwbody.cols(); j++) {
            EXPECT_NEAR(Jwbody(i, j), Jwbody_truth(i, j), TOLERANCE);
        }
    }
}