Merge pull request #192 from dartsim/gen_coord

Remove GenCoord and GenCoordSystem

apps/atlasRobot/Controller.cpp

@@ -40,7 +40,6 @@
 #include "dart/math/Helpers.h"
 #include "dart/dynamics/Skeleton.h"
 #include "dart/dynamics/BodyNode.h"
-#include "dart/dynamics/GenCoord.h"
 #include "dart/dynamics/Shape.h"
 #include "dart/dynamics/Joint.h"
 #include "dart/constraint/ConstraintSolver.h"
@@ -209,7 +208,7 @@ void Controller::printDebugInfo() const
 {
   std::cout << "[ATLAS Robot]"  << std::endl
             << " NUM NODES : " << mAtlasRobot->getNumBodyNodes() << std::endl
-            << " NUM DOF   : " << mAtlasRobot->getNumGenCoords() << std::endl
+            << " NUM DOF   : " << mAtlasRobot->getDof() << std::endl
             << " NUM JOINTS: " << mAtlasRobot->getNumBodyNodes() << std::endl;
 
   for(int i = 0; i < mAtlasRobot->getNumBodyNodes(); ++i)
@@ -220,7 +219,7 @@ void Controller::printDebugInfo() const
 
     std::cout << "  Joint [" << i << "]: "
               << joint->getName()
-              << " (" << joint->getNumGenCoords() << ")"
+              << " (" << joint->getDof() << ")"
               << std::endl;
     if (parentBody != NULL)
     {
@@ -310,8 +309,8 @@ void Controller::unharnessRightFoot()
 //==============================================================================
 void Controller::resetRobot()
 {
-  int dof = mAtlasRobot->getNumGenCoords();
-  mAtlasRobot->setState(mInitialState, true, true, false);
+  mAtlasRobot->setState(mInitialState);
+  mAtlasRobot->computeForwardKinematics(true, true, false);
 
   dtmsg << "Robot is reset." << std::endl;
 }
@@ -818,9 +817,9 @@ void Controller::_setJointDamping()
   for (int i = 1; i < mAtlasRobot->getNumBodyNodes(); ++i)
   {
     Joint* joint = mAtlasRobot->getJoint(i);
-    if (joint->getNumGenCoords() > 0)
+    if (joint->getDof() > 0)
     {
-      for (int j = 0; j < joint->getNumGenCoords(); ++j)
+      for (int j = 0; j < joint->getDof(); ++j)
         joint->setDampingCoefficient(j, 80.0);
     }
   }

apps/atlasRobot/Humanoid.cpp

@@ -40,7 +40,6 @@
 #include "dart/math/Helpers.h"
 #include "dart/dynamics/Skeleton.h"
 #include "dart/dynamics/BodyNode.h"
-#include "dart/dynamics/GenCoord.h"
 #include "dart/dynamics/Shape.h"
 //#include "dart/constraint/OldConstraintDynamics.h"
 #include "dart/collision/CollisionDetector.h"

apps/atlasRobot/Main.cpp

@@ -72,9 +72,10 @@ int main(int argc, char* argv[])
   myWorld->addSkeleton(ground);
 
   // Set initial configuration for Atlas robot
-  VectorXd q = atlas->getConfigs();
+  VectorXd q = atlas->getPositions();
   q[0] = -0.5 * DART_PI;
-  atlas->setConfigs(q, true, true, false);
+  atlas->setPositions(q);
+  atlas->computeForwardKinematics(true, true, false);
 
   // Set gravity of the world
   myWorld->setGravity(Vector3d(0.0, -9.81, 0.0));

apps/atlasRobot/State.cpp

@@ -41,7 +41,6 @@
 #include "dart/dynamics/Skeleton.h"
 #include "dart/dynamics/BodyNode.h"
 #include "dart/dynamics/Joint.h"
-#include "dart/dynamics/GenCoord.h"
 #include "dart/dynamics/Shape.h"
 //#include "dart/constraint/OldConstraintDynamics.h"
 #include "dart/collision/CollisionDetector.h"
@@ -74,7 +73,7 @@ State::State(Skeleton* _skeleton, const std::string& _name)
     mDesiredGlobalPelvisAngleOnSagital(0.0),
     mDesiredGlobalPelvisAngleOnCoronal(0.0)
 {
-  int dof = mSkeleton->getNumGenCoords();
+  int dof = mSkeleton->getDof();
 
   mDesiredJointPositions        = Eigen::VectorXd::Zero(dof);
   mDesiredJointPositionsBalance = Eigen::VectorXd::Zero(dof);
@@ -154,9 +153,9 @@ void State::computeControlForce(double _timestep)
 {
   assert(mNextState != NULL && "Next state should be set.");
 
-  int dof = mSkeleton->getNumGenCoords();
-  VectorXd q = mSkeleton->getConfigs();
-  VectorXd dq = mSkeleton->getGenVels();
+  int dof = mSkeleton->getDof();
+  VectorXd q = mSkeleton->getPositions();
+  VectorXd dq = mSkeleton->getVelocities();
 
   // Compute relative joint angles from desired global angles of the pelvis and
   // the swing leg
@@ -207,7 +206,7 @@ void State::computeControlForce(double _timestep)
   _updateTorqueForStanceLeg();
 
   // Apply control torque to the skeleton
-  mSkeleton->setInternalForceVector(mTorque);
+  mSkeleton->setForces(mTorque);
 
   mElapsedTime += _timestep;
   mFrame++;

apps/atlasRobot/StateMachine.cpp

@@ -40,7 +40,6 @@
 #include "dart/math/Helpers.h"
 #include "dart/dynamics/Skeleton.h"
 #include "dart/dynamics/BodyNode.h"
-#include "dart/dynamics/GenCoord.h"
 #include "dart/dynamics/Shape.h"
 //#include "dart/constraint/OldConstraintDynamics.h"
 #include "dart/collision/CollisionDetector.h"

apps/atlasRobot/TerminalCondition.cpp

@@ -42,7 +42,6 @@
 #include "dart/dynamics/BodyNode.h"
 #include "dart/dynamics/SoftBodyNode.h"
 #include "dart/dynamics/PointMass.h"
-#include "dart/dynamics/GenCoord.h"
 #include "dart/dynamics/Shape.h"
 //#include "dart/constraint/OldConstraintDynamics.h"
 #include "dart/collision/CollisionDetector.h"

apps/balance/Controller.cpp

@@ -40,7 +40,6 @@
 #include "dart/math/Helpers.h"
 #include "dart/dynamics/Skeleton.h"
 #include "dart/dynamics/BodyNode.h"
-#include "dart/dynamics/GenCoord.h"
 #include "dart/dynamics/Shape.h"
 //#include "dart/constraint/OldConstraintDynamics.h"
 #include "dart/collision/CollisionDetector.h"
@@ -52,7 +51,7 @@ Controller::Controller(dart::dynamics::Skeleton* _skel,
   mCollisionHandle = _collisionHandle;
   mTimestep = _t;
   mFrame = 0;
-  int nDof = mSkel->getNumGenCoords();
+  int nDof = mSkel->getDof();
   mKp = Eigen::MatrixXd::Identity(nDof, nDof);
   mKd = Eigen::MatrixXd::Identity(nDof, nDof);
   mConstrForces = Eigen::VectorXd::Zero(nDof);
@@ -61,7 +60,7 @@ Controller::Controller(dart::dynamics::Skeleton* _skel,
   mDesiredDofs.resize(nDof);
   for (int i = 0; i < nDof; i++) {
     mTorques[i] = 0.0;
-    mDesiredDofs[i] = mSkel->getGenCoord(i)->getPos();
+    mDesiredDofs[i] = mSkel->getPosition(i);
   }
 
   // using SPD results in simple Kp coefficients
@@ -95,7 +94,7 @@ void Controller::setDesiredDof(int _index, double _val) {
 void Controller::computeTorques(const Eigen::VectorXd& _dof,
                                 const Eigen::VectorXd& _dofVel) {
   // SPD tracking
-  int nDof = mSkel->getNumGenCoords();
+  int nDof = mSkel->getDof();
   Eigen::MatrixXd invM = (mSkel->getMassMatrix() + mKd * mTimestep).inverse();
   Eigen::VectorXd p = -mKp * (_dof + _dofVel * mTimestep - mDesiredDofs);
   Eigen::VectorXd d = -mKd * _dofVel;

apps/balance/Main.cpp

@@ -39,7 +39,6 @@
 #include <vector>
 
 #include "dart/utils/Paths.h"
-#include "dart/dynamics/GenCoord.h"
 #include "dart/dynamics/BodyNode.h"
 #include "dart/dynamics/Joint.h"
 #include "dart/dynamics/Skeleton.h"
@@ -57,7 +56,7 @@ int main(int argc, char* argv[]) {
   Eigen::Vector3d gravity(0.0, -9.81, 0.0);
   myWorld->setGravity(gravity);
 
-  std::vector<int> genCoordIds;
+  std::vector<size_t> genCoordIds;
   genCoordIds.push_back(1);
   genCoordIds.push_back(6);   // left hip
   genCoordIds.push_back(14);   // left knee
@@ -68,16 +67,16 @@ int main(int argc, char* argv[]) {
   genCoordIds.push_back(13);  // lower back
   Eigen::VectorXd initConfig(8);
   initConfig << -0.1, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1;
-  myWorld->getSkeleton(1)->setConfigSegs(genCoordIds, initConfig,
-                                         true, true, false);
+  myWorld->getSkeleton(1)->setPositionSegment(genCoordIds, initConfig);
+  myWorld->getSkeleton(1)->computeForwardKinematics(true, true, false);
   dart::dynamics::Skeleton* skeleton = myWorld->getSkeleton(1);
   for (int i = 1; i < skeleton->getNumBodyNodes(); ++i)
   {
     dart::dynamics::Joint* joint = skeleton->getBodyNode(i)->getParentJoint();
-    for (int j = 0; j < joint->getNumGenCoords(); ++j)
+    for (int j = 0; j < joint->getDof(); ++j)
     {
-      joint->getGenCoord(j)->setPosMin(-0.1);
-      joint->getGenCoord(j)->setPosMax(0.1);
+      joint->setPositionLowerLimit(j, -0.1);
+      joint->setPositionUpperLimit(j, 0.1);
     }
   }
 

apps/balance/MyWindow.cpp

@@ -59,8 +59,8 @@ void MyWindow::timeStepping() {
 
 //  mController->setConstrForces(
 //        mWorld->getConstraintHandler()->getTotalConstraintForce(1));
-//  mController->computeTorques(mWorld->getSkeleton(1)->getConfigs(),
-//                              mWorld->getSkeleton(1)->getGenVels());
+//  mController->computeTorques(mWorld->getSkeleton(1)->getPositions(),
+//                              mWorld->getSkeleton(1)->getVelocities());
 //  mWorld->getSkeleton(1)->setInternalForceVector(mController->getTorques());
 
   mWorld->step();

apps/ballJointConstraintTest/Main.cpp

@@ -65,17 +65,18 @@ int main(int argc, char* argv[])
     myWorld->setGravity(gravity);
     myWorld->setTimeStep(1.0/2000);
 
-    int dof =  myWorld->getSkeleton(0)->getNumGenCoords();
+    int dof =  myWorld->getSkeleton(0)->getDof();
     Eigen::VectorXd initPose(dof);
     for (int i = 0; i < dof; i++)
         initPose[i] = random(-0.5, 0.5);
-    myWorld->getSkeleton(0)->setConfigs(initPose, true, true, false);
+    myWorld->getSkeleton(0)->setPositions(initPose);
+    myWorld->getSkeleton(0)->computeForwardKinematics(true, true, false);
 
     // Add damping to every joint
     for (int i = 0; i < myWorld->getSkeleton(0)->getNumBodyNodes(); i++) {
         BodyNode *bd = myWorld->getSkeleton(0)->getBodyNode(i);
         Joint *jt = bd->getParentJoint();
-        for (int j = 0; j < jt->getNumGenCoords(); j++)
+        for (int j = 0; j < jt->getDof(); j++)
             jt->setDampingCoefficient(j, 0.02);
     }
 

apps/ballJointConstraintTest/MyWindow.cpp

@@ -54,10 +54,10 @@ void MyWindow::timeStepping()
 
 Eigen::VectorXd MyWindow::computeDamping()
 {
-    int nDof = mWorld->getSkeleton(0)->getNumGenCoords();
+    int nDof = mWorld->getSkeleton(0)->getDof();
     Eigen::VectorXd damping = Eigen::VectorXd::Zero(nDof);
     // add damping to each joint; twist-dof has smaller damping
-    damping = -0.01 * mWorld->getSkeleton(0)->getGenVels();
+    damping = -0.01 * mWorld->getSkeleton(0)->getVelocities();
     for (int i = 0; i < nDof; i++)
         if (i % 3 == 1)
             damping[i] *= 0.1;

apps/closedLoop/Main.cpp

@@ -65,15 +65,16 @@ int main(int argc, char* argv[])
     myWorld->setGravity(gravity);
     myWorld->setTimeStep(1.0/2000);
 
-    int dof =  myWorld->getSkeleton(0)->getNumGenCoords();
+    int dof =  myWorld->getSkeleton(0)->getDof();
 
     Eigen::VectorXd initPose(dof);
     initPose.setZero();
     initPose[20] = 3.14159 * 0.4;
     initPose[23] = 3.14159 * 0.4;
     initPose[26] = 3.14159 * 0.4;
     initPose[29] = 3.14159 * 0.4;
-    myWorld->getSkeleton(0)->setConfigs(initPose, true, true, false);
+    myWorld->getSkeleton(0)->setPositions(initPose);
+    myWorld->getSkeleton(0)->computeForwardKinematics(true, true, false);
 
     // create a ball joint constraint
     BodyNode *bd1 = myWorld->getSkeleton(0)->getBodyNode("link 6");

apps/closedLoop/MyWindow.cpp

@@ -42,16 +42,16 @@
 void MyWindow::timeStepping()
 {
     Eigen::VectorXd damping = computeDamping();
-    mWorld->getSkeleton(0)->setInternalForceVector(damping);
+    mWorld->getSkeleton(0)->setForces(damping);
     mWorld->step();
 }
 
 Eigen::VectorXd MyWindow::computeDamping()
 {
-    int nDof = mWorld->getSkeleton(0)->getNumGenCoords();
+    int nDof = mWorld->getSkeleton(0)->getDof();
     Eigen::VectorXd damping = Eigen::VectorXd::Zero(nDof);
     // add damping to each joint; twist-dof has smaller damping
-    damping = -0.01 * mWorld->getSkeleton(0)->getGenVels();
+    damping = -0.01 * mWorld->getSkeleton(0)->getVelocities();
     for (int i = 0; i < nDof; i++)
         if (i % 3 == 1)
             damping[i] *= 0.1;

apps/forwardSim/Main.cpp

@@ -53,11 +53,12 @@ int main(int argc, char* argv[]) {
   myWorld->setGravity(gravity);
   myWorld->setTimeStep(1.0/2000);
 
-  int dof =  myWorld->getSkeleton(0)->getNumGenCoords();
+  int dof =  myWorld->getSkeleton(0)->getDof();
   Eigen::VectorXd initPose(dof);
   for (int i = 0; i < dof; i++)
     initPose[i] = dart::math::random(-0.5, 0.5);
-  myWorld->getSkeleton(0)->setConfigs(initPose, true, true, false);
+  myWorld->getSkeleton(0)->setPositions(initPose);
+  myWorld->getSkeleton(0)->computeForwardKinematics(true, true, false);
 
   // create a window and link it to the world
   MyWindow window;

apps/forwardSim/MyWindow.cpp

@@ -48,15 +48,17 @@ MyWindow::~MyWindow() {
 
 void MyWindow::timeStepping() {
   Eigen::VectorXd damping = computeDamping();
-  mWorld->getSkeleton(0)->setInternalForceVector(damping);
+  mWorld->getSkeleton(0)->setForces(damping);
   mWorld->step();
 }
 
-Eigen::VectorXd MyWindow::computeDamping() {
-  int nDof = mWorld->getSkeleton(0)->getNumGenCoords();
+//==============================================================================
+Eigen::VectorXd MyWindow::computeDamping()
+{
+  int nDof = mWorld->getSkeleton(0)->getDof();
   Eigen::VectorXd damping = Eigen::VectorXd::Zero(nDof);
   // add damping to each joint; twist-dof has smaller damping
-  damping = -0.01 * mWorld->getSkeleton(0)->getGenVels();
+  damping = -0.01 * mWorld->getSkeleton(0)->getVelocities();
   for (int i = 0; i < nDof; i++)
     if (i % 3 == 1)
       damping[i] *= 0.1;

apps/hanging/Controller.cpp

@@ -39,7 +39,6 @@
 #include "dart/math/Helpers.h"
 #include "dart/dynamics/Skeleton.h"
 #include "dart/dynamics/BodyNode.h"
-#include "dart/dynamics/GenCoord.h"
 #include "dart/dynamics/Shape.h"
 #include "dart/constraint/ConstraintSolver.h"
 #include "dart/collision/CollisionDetector.h"
@@ -58,7 +57,7 @@ Controller::Controller(dynamics::Skeleton* _skel,
   mConstraintSolver = _constraintSolver;
   mTimestep = _t;
   mFrame = 0;
-  int nDof = mSkel->getNumGenCoords();
+  int nDof = mSkel->getDof();
   mKp = MatrixXd::Identity(nDof, nDof);
   mKd = MatrixXd::Identity(nDof, nDof);
   mConstrForces = VectorXd::Zero(nDof);
@@ -67,7 +66,7 @@ Controller::Controller(dynamics::Skeleton* _skel,
   mDesiredDofs.resize(nDof);
   for (int i = 0; i < nDof; i++){
     mTorques[i] = 0.0;
-    mDesiredDofs[i] = mSkel->getGenCoord(i)->getPos();
+    mDesiredDofs[i] = mSkel->getPosition(i);
   }
 
   // using SPD results in simple Kp coefficients
@@ -89,7 +88,7 @@ Controller::Controller(dynamics::Skeleton* _skel,
 
 void Controller::computeTorques(const VectorXd& _dof, const VectorXd& _dofVel) {
   // SPD tracking
-  int nDof = mSkel->getNumGenCoords();
+  int nDof = mSkel->getDof();
   MatrixXd invM = (mSkel->getMassMatrix() + mKd * mTimestep).inverse();
   VectorXd p = -mKp * (_dof + _dofVel * mTimestep - mDesiredDofs);
   VectorXd d = -mKd * _dofVel;

apps/hanging/MyWindow.cpp

@@ -44,7 +44,6 @@
 #include "dart/constraint/ConstraintSolver.h"
 #include "dart/constraint/WeldJointConstraint.h"
 #include "dart/dynamics/BodyNode.h"
-#include "dart/dynamics/GenCoord.h"
 #include "dart/dynamics/Skeleton.h"
 #include "dart/simulation/World.h"
 #include "dart/gui/GLFuncs.h"
@@ -69,11 +68,11 @@ MyWindow::MyWindow(dart::simulation::World* _world)
   mWorld->setGravity(gravity);
 
   //
-  std::vector<int> genCoordIds0;
+  std::vector<size_t> genCoordIds0;
   genCoordIds0.push_back(4);
 
   // default standing pose
-  std::vector<int> genCoordIds1;
+  std::vector<size_t> genCoordIds1;
   genCoordIds1.push_back(1);
   genCoordIds1.push_back(6); // left hip
   genCoordIds1.push_back(9); // left knee
@@ -90,16 +89,16 @@ MyWindow::MyWindow(dart::simulation::World* _world)
   Eigen::VectorXd initConfig1(10);
   initConfig1 << -0.1, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1, 0.5, -0.5;
 
-  mWorld->getSkeleton(0)->setConfigSegs(genCoordIds0, initConfig0);
-  mWorld->getSkeleton(1)->setConfigSegs(genCoordIds1, initConfig1);
+  mWorld->getSkeleton(0)->setPositionSegment(genCoordIds0, initConfig0);
+  mWorld->getSkeleton(1)->setPositionSegment(genCoordIds1, initConfig1);
 
   // create controller
   mController = new Controller(mWorld->getSkeleton(1),
                                mWorld->getConstraintSolver(),
                                mWorld->getTimeStep());
 
-  for (int i = 0; i < mWorld->getSkeleton(1)->getNumGenCoords(); i++)
-    mController->setDesiredDof(i, mController->getSkeleton()->getGenCoord(i)->getPos());
+  for (int i = 0; i < mWorld->getSkeleton(1)->getDof(); i++)
+    mController->setDesiredDof(i, mController->getSkeleton()->getPosition(i));
 
   // initialize constraint on the hand
   BodyNode* bd = mWorld->getSkeleton(1)->getBodyNode("h_hand_left");