C++ Joint::getNumGenCoords方法代码示例

void BodyNode::fitWorldTransformParentJointImpl(
    const Eigen::Isometry3d& _target, bool _jointLimit)
{
  Joint* parentJoint = getParentJoint();
  size_t dof = parentJoint->getNumGenCoords();

  if (dof == 0)
    return;

  optimizer::Problem prob(dof);

  // Use the current joint configuration as initial guess
  prob.setInitialGuess(parentJoint->getConfigs());

  // Objective function
  TransformObjFunc obj(this, _target, mSkeleton);
  prob.setObjective(&obj);

  // Joint limit
  if (_jointLimit)
  {
    prob.setLowerBounds(parentJoint->getConfigsMin());
    prob.setUpperBounds(parentJoint->getConfigsMax());
  }

  // Solve with gradient-free local minima algorithm
  optimizer::NloptSolver solver(&prob, NLOPT_LN_BOBYQA);
  solver.solve();

  // Set optimal configuration of the parent joint
  Eigen::VectorXd jointQ = prob.getOptimalSolution();
  parentJoint->setConfigs(jointQ, true, true, true);
}

void BodyNode::fitWorldAngularVel(const Eigen::Vector3d& _targetAngVel,
                                  BodyNode::InverseKinematicsPolicy /*_policy*/,
                                  bool _jointVelLimit)
{
  // TODO: Only IKP_PARENT_JOINT policy is supported now.

  Joint* parentJoint = getParentJoint();
  size_t dof = parentJoint->getNumGenCoords();

  if (dof == 0)
    return;

  optimizer::Problem prob(dof);

  // Use the current joint configuration as initial guess
  prob.setInitialGuess(parentJoint->getGenVels());

  // Objective function
  VelocityObjFunc obj(this, _targetAngVel, VelocityObjFunc::VT_ANGULAR, mSkeleton);
  prob.setObjective(&obj);

  // Joint limit
  if (_jointVelLimit)
  {
    prob.setLowerBounds(parentJoint->getGenVelsMin());
    prob.setUpperBounds(parentJoint->getGenVelsMax());
  }

  // Solve with gradient-free local minima algorithm
  optimizer::NloptSolver solver(&prob, NLOPT_LN_BOBYQA);
  solver.solve();

  // Set optimal configuration of the parent joint
  Eigen::VectorXd jointDQ = prob.getOptimalSolution();
  parentJoint->setGenVels(jointDQ, true, true);
}

//==============================================================================
void DynamicsTest::testImpulseBasedDynamics(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 --------------------------------------
  // Number of random state tests for each skeletons
#ifndef NDEBUG  // Debug mode
  int nRandomItr = 1;
#else
  int nRandomItr = 100;
#endif

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

  simulation::World* myWorld = NULL;

  //----------------------------- Tests ----------------------------------------
  // Check whether multiplication of mass matrix and its inverse is identity
  // matrix.
  myWorld = utils::SkelParser::readWorld(_fileName);
  EXPECT_TRUE(myWorld != NULL);

  for (int i = 0; i < myWorld->getNumSkeletons(); ++i)
  {
    dynamics::Skeleton* skel = myWorld->getSkeleton(i);

    int dof            = skel->getNumGenCoords();
//    int nBodyNodes     = skel->getNumBodyNodes();

    if (dof == 0 || !skel->isMobile())
    {
      dtdbg << "Skeleton [" << skel->getName() << "] is skipped since it has "
            << "0 DOF or is immobile." << endl;
      continue;
    }

    for (int j = 0; j < nRandomItr; ++j)
    {
      // Set random configurations
      for (int k = 0; k < skel->getNumBodyNodes(); ++k)
      {
        BodyNode* body     = skel->getBodyNode(k);
        Joint*    joint    = body->getParentJoint();
        int       localDof = joint->getNumGenCoords();

        for (int l = 0; l < localDof; ++l)
        {
          double lbRP = joint->getGenCoord(l)->getPosMin();
          double ubRP = joint->getGenCoord(l)->getPosMax();
          if (lbRP < -DART_PI)
            lbRP = -DART_PI;
          if (ubRP > DART_PI)
            ubRP = DART_PI;
          joint->setConfig(l, random(lbRP, ubRP), true, false, false);
        }
      }
      skel->setConfigs(VectorXd::Zero(dof));

      // TODO(JS): Just clear what should be
      skel->clearExternalForces();
      skel->clearConstraintImpulses();

      // Set random impulses
      VectorXd impulses = VectorXd::Zero(dof);
      for (size_t k = 0; k < impulses.size(); ++k)
        impulses[k] = random(lb, ub);
      skel->setConstraintImpulses(impulses);

      // Compute impulse-based forward dynamics
      skel->computeImpulseForwardDynamics();

      // Compare resultant velocity change and invM * impulses
      VectorXd deltaVel1 = skel->getVelsChange();
      MatrixXd invM = skel->getInvMassMatrix();
      VectorXd deltaVel2 = invM * impulses;

      EXPECT_TRUE(equals(deltaVel1, deltaVel2, 1e-6));
      if (!equals(deltaVel1, deltaVel2, 1e-6))
      {
        cout << "deltaVel1: " << deltaVel1.transpose()  << endl;
        cout << "deltaVel2: " << deltaVel2.transpose() << endl;
      }
    }
  }

  delete myWorld;
}

//==============================================================================
void DynamicsTest::centerOfMass(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 --------------------------------------
  // Number of random state tests for each skeletons
#ifndef NDEBUG  // Debug mode
  int nRandomItr = 10;
#else
  int nRandomItr = 100;
#endif

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

  // Lower and upper bound of joint damping and stiffness
  double lbD =  0.0;
  double ubD = 10.0;
  double lbK =  0.0;
  double ubK = 10.0;

  simulation::World* myWorld = NULL;

  //----------------------------- Tests ----------------------------------------
  // Check whether multiplication of mass matrix and its inverse is identity
  // matrix.
  myWorld = utils::SkelParser::readWorld(_fileName);
  EXPECT_TRUE(myWorld != NULL);

  for (int i = 0; i < myWorld->getNumSkeletons(); ++i)
  {
    dynamics::Skeleton* skel = myWorld->getSkeleton(i);

    int dof            = skel->getNumGenCoords();
//    int nBodyNodes     = skel->getNumBodyNodes();

    if (dof == 0)
    {
      dtmsg << "Skeleton [" << skel->getName() << "] is skipped since it has "
            << "0 DOF." << endl;
      continue;
    }

    for (int j = 0; j < nRandomItr; ++j)
    {
      // Random joint stiffness and damping coefficient
      for (int k = 0; k < skel->getNumBodyNodes(); ++k)
      {
        BodyNode* body     = skel->getBodyNode(k);
        Joint*    joint    = body->getParentJoint();
        int       localDof = joint->getNumGenCoords();

        for (int l = 0; l < localDof; ++l)
        {
          joint->setDampingCoefficient(l, random(lbD,  ubD));
          joint->setSpringStiffness   (l, random(lbK,  ubK));

          double lbRP = joint->getGenCoord(l)->getPosMin();
          double ubRP = joint->getGenCoord(l)->getPosMax();
          if (lbRP < -DART_PI)
            lbRP = -DART_PI;
          if (ubRP > DART_PI)
            ubRP = DART_PI;
          joint->setRestPosition      (l, random(lbRP, ubRP));
        }
      }

      // Set random states
      VectorXd x = skel->getState();
      for (int k = 0; k < x.size(); ++k)
        x[k] = random(lb, ub);
      skel->setState(x, true, true, false);

      VectorXd tau = skel->getGenForces();
      for (int k = 0; k < tau.size(); ++k)
        tau[k] = random(lb, ub);
      skel->setGenForces(tau);

      skel->computeForwardDynamics();

      VectorXd q  = skel->getConfigs();
      VectorXd dq = skel->getGenVels();
      VectorXd ddq = skel->getGenAccs();

      VectorXd com   = skel->getWorldCOM();
      VectorXd dcom  = skel->getWorldCOMVelocity();
      VectorXd ddcom = skel->getWorldCOMAcceleration();

      MatrixXd comJ  = skel->getWorldCOMJacobian();
      MatrixXd comdJ = skel->getWorldCOMJacobianTimeDeriv();

      VectorXd dcom2  = comJ * dq;
//.........这里部分代码省略.........

//==============================================================================
void DynamicsTest::compareEquationsOfMotion(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 --------------------------------------
  // Number of random state tests for each skeletons
#ifndef NDEBUG  // Debug mode
  int nRandomItr = 5;
#else
  int nRandomItr = 100;
#endif

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

  // Lower and upper bound of joint damping and stiffness
  double lbD =  0.0;
  double ubD = 10.0;
  double lbK =  0.0;
  double ubK = 10.0;

  simulation::World* myWorld = NULL;

  //----------------------------- Tests ----------------------------------------
  // Check whether multiplication of mass matrix and its inverse is identity
  // matrix.
  myWorld = utils::SkelParser::readWorld(_fileName);
  EXPECT_TRUE(myWorld != NULL);

  for (int i = 0; i < myWorld->getNumSkeletons(); ++i)
  {
    dynamics::Skeleton* skel = myWorld->getSkeleton(i);

    int dof            = skel->getNumGenCoords();
//    int nBodyNodes     = skel->getNumBodyNodes();

    if (dof == 0)
    {
      dtmsg << "Skeleton [" << skel->getName() << "] is skipped since it has "
            << "0 DOF." << endl;
      continue;
    }

    for (int j = 0; j < nRandomItr; ++j)
    {
      // Random joint stiffness and damping coefficient
      for (int k = 0; k < skel->getNumBodyNodes(); ++k)
      {
        BodyNode* body     = skel->getBodyNode(k);
        Joint*    joint    = body->getParentJoint();
        int       localDof = joint->getNumGenCoords();

        for (int l = 0; l < localDof; ++l)
        {
          joint->setDampingCoefficient(l, random(lbD,  ubD));
          joint->setSpringStiffness   (l, random(lbK,  ubK));

          double lbRP = joint->getGenCoord(l)->getPosMin();
          double ubRP = joint->getGenCoord(l)->getPosMax();
          if (lbRP < -DART_PI)
            lbRP = -DART_PI;
          if (ubRP > DART_PI)
            ubRP = DART_PI;
          joint->setRestPosition      (l, random(lbRP, ubRP));
        }
      }

      // Set random states
      VectorXd x = skel->getState();
      for (int k = 0; k < x.size(); ++k)
        x[k] = random(lb, ub);
      skel->setState(x, true, true, false);

      //------------------------ Mass Matrix Test ----------------------------
      // Get matrices
      MatrixXd M      = skel->getMassMatrix();
      MatrixXd M2     = getMassMatrix(skel);
      MatrixXd InvM   = skel->getInvMassMatrix();
      MatrixXd M_InvM = M * InvM;
      MatrixXd InvM_M = InvM * M;

      MatrixXd AugM         = skel->getAugMassMatrix();
      MatrixXd AugM2        = getAugMassMatrix(skel);
      MatrixXd InvAugM      = skel->getInvAugMassMatrix();
      MatrixXd AugM_InvAugM = AugM * InvAugM;
      MatrixXd InvAugM_AugM = InvAugM * AugM;

      MatrixXd I        = MatrixXd::Identity(dof, dof);

      // Check if the number of generalized coordinates and dimension of mass
      // matrix are same.
      EXPECT_EQ(M.rows(), dof);
//.........这里部分代码省略.........