Beispiel #1
0
//==============================================================================
void Controller::_setJointDamping()
{
  for (std::size_t i = 1; i < mAtlasRobot->getNumBodyNodes(); ++i)
  {
    Joint* joint = mAtlasRobot->getJoint(i);
    if (joint->getNumDofs() > 0)
    {
      for (std::size_t j = 0; j < joint->getNumDofs(); ++j)
        joint->setDampingCoefficient(j, 80.0);
    }
  }
}
Beispiel #2
0
void check_self_consistency(SkeletonPtr skeleton)
{
  for(size_t i=0; i<skeleton->getNumBodyNodes(); ++i)
  {
    BodyNode* bn = skeleton->getBodyNode(i);
    EXPECT_TRUE(bn->getIndexInSkeleton() == i);
    EXPECT_TRUE(skeleton->getBodyNode(bn->getName()) == bn);

    Joint* joint = bn->getParentJoint();
    EXPECT_TRUE(skeleton->getJoint(joint->getName()) == joint);

    for(size_t j=0; j<joint->getNumDofs(); ++j)
    {
      DegreeOfFreedom* dof = joint->getDof(j);
      EXPECT_TRUE(dof->getIndexInJoint() == j);
      EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
    }
  }

  for(size_t i=0; i<skeleton->getNumDofs(); ++i)
  {
    DegreeOfFreedom* dof = skeleton->getDof(i);
    EXPECT_TRUE(dof->getIndexInSkeleton() == i);
    EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
  }
}
Beispiel #3
0
//==============================================================================
void Controller::printDebugInfo() const
{
  std::cout << "[ATLAS Robot]" << std::endl
            << " NUM NODES : " << mAtlasRobot->getNumBodyNodes() << std::endl
            << " NUM DOF   : " << mAtlasRobot->getNumDofs() << std::endl
            << " NUM JOINTS: " << mAtlasRobot->getNumBodyNodes() << std::endl;

  for (std::size_t i = 0; i < mAtlasRobot->getNumBodyNodes(); ++i)
  {
    Joint* joint = mAtlasRobot->getJoint(i);
    BodyNode* body = mAtlasRobot->getBodyNode(i);
    BodyNode* parentBody = mAtlasRobot->getBodyNode(i)->getParentBodyNode();

    std::cout << "  Joint [" << i << "]: " << joint->getName() << " ("
              << joint->getNumDofs() << ")" << std::endl;
    if (parentBody != nullptr)
    {
      std::cout << "    Parent body: " << parentBody->getName() << std::endl;
    }

    std::cout << "    Child body : " << body->getName() << std::endl;
  }
}
Beispiel #4
0
//==============================================================================
void SoftDynamicsTest::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
  size_t nRandomItr = 1;
#else
  size_t nRandomItr = 1;
#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 (size_t i = 0; i < myWorld->getNumSkeletons(); ++i)
  {
    dynamics::Skeleton* skel = myWorld->getSkeleton(i);
    dynamics::Skeleton* softSkel
        = dynamic_cast<dynamics::Skeleton*>(skel);

    int dof            = skel->getNumDofs();
//    int nBodyNodes     = skel->getNumBodyNodes();
    int nSoftBodyNodes = 0;
    if (softSkel != NULL)
      nSoftBodyNodes = softSkel->getNumSoftBodyNodes();

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

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

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

          double lbRP = joint->getPositionLowerLimit(l);
          double ubRP = joint->getPositionUpperLimit(l);
          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);
      skel->computeForwardKinematics(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);
      EXPECT_EQ(M.cols(), dof);

      // Check mass matrix
      EXPECT_TRUE(equals(M, M2, 1e-6));
      if (!equals(M, M2, 1e-6))
      {
        cout << "M :" << endl << M  << endl << endl;
        cout << "M2:" << endl << M2 << endl << endl;
      }

      // Check augmented mass matrix
      EXPECT_TRUE(equals(AugM, AugM2, 1e-6));
      if (!equals(AugM, AugM2, 1e-6))
      {
        cout << "AugM :" << endl << AugM  << endl << endl;
        cout << "AugM2:" << endl << AugM2 << endl << endl;
      }

      // Check if both of (M * InvM) and (InvM * M) are identity.
      EXPECT_TRUE(equals(M_InvM, I, 1e-6));
      if (!equals(M_InvM, I, 1e-6))
      {
        cout << "InvM  :" << endl << InvM << endl << endl;
      }
      EXPECT_TRUE(equals(InvM_M, I, 1e-6));
      if (!equals(InvM_M, I, 1e-6))
      {
        cout << "InvM_M:" << endl << InvM_M << endl << endl;
      }

      // Check if both of (M * InvM) and (InvM * M) are identity.
      EXPECT_TRUE(equals(AugM_InvAugM, I, 1e-6));
      if (!equals(AugM_InvAugM, I, 1e-6))
      {
        cout << "InvAugM  :" << endl << InvAugM << endl << endl;
        cout << "InvAugM2  :" << endl << AugM.inverse() << endl << endl;
        cout << "AugM_InvAugM  :" << endl << AugM_InvAugM << endl << endl;
      }
      EXPECT_TRUE(equals(InvAugM_AugM, I, 1e-6));
      if (!equals(InvAugM_AugM, I, 1e-6))
      {
        cout << "InvAugM_AugM:" << endl << InvAugM_AugM << endl << endl;
      }

      //------- Coriolis Force Vector and Combined Force Vector Tests --------
      // Get C1, Coriolis force vector using recursive method
      VectorXd C  = skel->getCoriolisForces();
      VectorXd Cg = skel->getCoriolisAndGravityForces();

      // Get C2, Coriolis force vector using inverse dynamics algorithm
      Vector3d oldGravity = skel->getGravity();
      VectorXd oldTau     = skel->getForces();
      VectorXd oldDdq     = skel->getAccelerations();
      // TODO(JS): Save external forces of body nodes
      vector<double> oldKv(nSoftBodyNodes, 0.0);
      vector<double> oldKe(nSoftBodyNodes, 0.0);
      vector<double>  oldD(nSoftBodyNodes, 0.0);
      for (int k = 0; k < nSoftBodyNodes; ++k)
      {
        assert(softSkel != NULL);
        dynamics::SoftBodyNode* sbn = softSkel->getSoftBodyNode(k);
        oldKv[k] = sbn->getVertexSpringStiffness();
        oldKe[k] = sbn->getEdgeSpringStiffness();
        oldD[k]  = sbn->getDampingCoefficient();
      }

      skel->resetForces();
      skel->clearExternalForces();
      skel->setAccelerations(VectorXd::Zero(dof));
      for (int k = 0; k < nSoftBodyNodes; ++k)
      {
        assert(softSkel != NULL);
        dynamics::SoftBodyNode* sbn = softSkel->getSoftBodyNode(k);
        sbn->setVertexSpringStiffness(0.0);
        sbn->setEdgeSpringStiffness(0.0);
        sbn->setDampingCoefficient(0.0);
      }

      EXPECT_TRUE(skel->getForces()         == VectorXd::Zero(dof));
      EXPECT_TRUE(skel->getExternalForces() == VectorXd::Zero(dof));
      EXPECT_TRUE(skel->getAccelerations()  == VectorXd::Zero(dof));

      skel->setGravity(Vector3d::Zero());
      EXPECT_TRUE(skel->getGravity() == Vector3d::Zero());
      skel->computeInverseDynamics(false, false);
      VectorXd C2 = skel->getForces();

      skel->setGravity(oldGravity);
      EXPECT_TRUE(skel->getGravity() == oldGravity);
      skel->computeInverseDynamics(false, false);
      VectorXd Cg2 = skel->getForces();

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

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

      skel->setForces(oldTau);
      skel->setAccelerations(oldDdq);
      // TODO(JS): Restore external forces of body nodes
    }
  }

  delete myWorld;
}
Beispiel #5
0
// Current code only works for the left leg with only one constraint
VectorXd MyWorld::updateGradients() {
    mJ.setZero();
    mC.setZero();

  // compute c(q)
  //std::cout << "HAMYDEBUG: mConstrainedMarker = " << getMarker(mConstrainedMarker) << std::endl;
  mC = getMarker(mConstrainedMarker)->getWorldPosition() - mTarget;
  std::cout << "C(q) = " << mC << std::endl;

  // compute J(q)
  Vector4d offset;
  offset << getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates

  //Setup vars

  BodyNode *node = getMarker(mConstrainedMarker)->getBodyNode();
  Joint *joint = node->getParentJoint();
  Matrix4d worldToParent;
  Matrix4d parentToJoint;
  
  //Declare Vars
  Matrix4d dR; // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
  Matrix4d R;
  Matrix4d R1;
  Matrix4d R2;
  Matrix4d jointToChild;
  Vector4d jCol;
  int colIndex;

  //TODO: Might want to change this to check if root using given root fcn

  //Iterate until we get to the root node
  while(true) {

    //std::cout << "HAMY DEBUG: Beginning new looop" << std::endl;

    if(node->getParentBodyNode() == NULL) {
      worldToParent = worldToParent.setIdentity();
    } else {
      worldToParent = node->getParentBodyNode()->getTransform().matrix();
    }
    parentToJoint = joint->getTransformFromParentBodyNode().matrix();
     // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
    jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();

    //TODO: R1, R2, ... Rn code depending on DOFs
    int nDofs = joint->getNumDofs();
    //std::cout << "HAMY: nDofs=" << nDofs << std::endl;
    //Can only have up to 3 DOFs on any one piece
    switch(nDofs){
      case 1: //std::cout << "HAMY: 1 nDOF" << std::endl;

        dR = joint->getTransformDerivative(0);
        jCol = worldToParent * parentToJoint * dR * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(0);
        mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
        offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset;

        break;
      case 2: //std::cout << "HAMY: 2 nDOF" << std::endl;

        dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
        R = joint->getTransform(1).matrix();
        jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
        jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(0);
        mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol

        dR = joint->getTransformDerivative(1);
        R = joint->getTransform(0).matrix();
        jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(1);
        mJ.col(colIndex) = jCol.head(3);
        offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Upd

        break;
      case 3: //std::cout << "HAMY: 3 nDOF" << std::endl;

        dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
        R1 = joint->getTransform(1).matrix();
        R2 = joint->getTransform(2).matrix();
        jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
        jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(0);
        mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J

        R1 = joint->getTransform(0).matrix();
        dR = joint->getTransformDerivative(1);
        R2 = joint->getTransform(2).matrix();
        jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(1);
        mJ.col(colIndex) = jCol.head(3);

        R1 = joint->getTransform(0).matrix();
        R2 = joint->getTransform(1).matrix();
        dR = joint->getTransformDerivative(2);
        jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(2);
        mJ.col(colIndex) = jCol.head(3);

        offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * joint->getTransform(2).matrix() * jointToChild * offset;

        break;
      default: //std::cout << "HAMY: Unexpected nDOF = " << nDofs << std::endl;
        break;
    }

    if(node != mSkel->getRootBodyNode()) {
      //std::cout << "HAMY DEBUG: Not root, continue loop" << std::endl;
      node = node->getParentBodyNode(); // return NULL if node is the root node
      joint = node->getParentJoint();
    } else {
      break;
    }
  }

  // compute gradients
  VectorXd gradients = 2 * mJ.transpose() * mC;
  return gradients;
}