Improve interface for forward kinematics

Changelog.md

@@ -10,12 +10,14 @@
 1. Added planar joint
 1. Added initial implementation of soft body dynamics
 1. Added useful kinematical properties pertain to COM of skeleton
-1. Renewed optimizer interface and added nlopt solver
+1. Improved optimizer interface and added nlopt solver
   * [Pull request #152](https://github.com/dartsim/dart/pull/152)
 1. Added initial implementation of inverse kinematics functions
   * [Pull request #154](https://github.com/dartsim/dart/pull/154)
 1. Added optional collision detector: BulletCollision.
   * [Pull request #156](https://github.com/dartsim/dart/pull/156)
+1. Improved kinematics API for skeleton and joint
+  * [Pull request #161](https://github.com/dartsim/dart/pull/161)
 
 ### Version 3.0 (2013-11-04)
 

Migration.md

@@ -13,11 +13,48 @@
   + void fitWorldTransformAncestorJointsImpl(BodyNode* _body, const Eigen::Isometry3d& _target, bool _jointLimit = true)
   + void fitWorldTransformAllJointsImpl(BodyNode* _body, const Eigen::Isometry3d& _target, bool _jointLimit = true)
 
+1. **dart/dynamics/Joint.h**
+  + virtual void setConfig(size_t _idx, double _config, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
+  + virtual void setConfigs(const Eigen::VectorXd& _configs, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
+  + virtual void setGenVel(size_t _idx, double _genVel, bool _updateVels = true, bool _updateAccs = true)
+  + virtual void setGenVels(const Eigen::VectorXd& _genVels, bool _updateVels = true, bool _updateAccs = true)
+  + virtual void setGenAcc(size_t _idx, double _genAcc, bool _updateAccs = true)
+  + virtual void setGenAccs(const Eigen::VectorXd& _genAccs, bool _updateAccs = true)
+
+1. **dart/dynamics/Skeleton.h**
+  + virtual void setGenVels(const Eigen::VectorXd& _genVels, bool _updateVels = true, bool _updateAccs = true)
+  + virtual void setGenAccs(const Eigen::VectorXd& _genAccs, bool _updateAccs = true)
+  + void computeForwardKinematics(bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
+
 ### Deletions
+
+1. **dart/dynamics/Skeleton.h**
+  + Eigen::VectorXd getConfig() const
+
 ### Modifications
 
+1. **dart/dynamics/Skeleton.h**
+
+  + ***From:*** void setConfig(const std::vector<int>& _id, const Eigen::VectorXd& _config)
+  + ***To:*** void setConfigSegs(const std::vector<int>& _id, const Eigen::VectorXd& _configs, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
+
+  + ***From:*** Eigen::VectorXd getConfig(const std::vector<int>& _id) const
+  + ***To:*** Eigen::VectorXd getConfigSegs(const std::vector<int>& _id) const
+
+  + ***From:*** void setConfig(const Eigen::VectorXd& _config)
+  + ***To:*** virtual void setConfigs(const Eigen::VectorXd& _configs, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
+
+  + ***From:*** void setState(const Eigen::VectorXd& _state)
+  + ***To:*** void setState(const Eigen::VectorXd& _state, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
+
+  + ***From:*** Eigen::VectorXd getState()
+  + ***To:*** Eigen::VectorXd getState() const
+
 1. **dart/dynamics/BodyNode.h**
+
   + ***From:*** int getNumContactForces() const
   + ***To:*** int getNumContacts() const
 
 ### New Deprecations
+
+

apps/atlasRobot/Controller.cpp

@@ -347,8 +347,8 @@ void Controller::unharnessRightFoot()
 void Controller::resetRobot()
 {
   int dof = mAtlasRobot->getNumGenCoords();
-  mAtlasRobot->setConfig(mInitialState.head(dof));  // See #122
-  mAtlasRobot->setState(mInitialState);
+  mAtlasRobot->setConfigs(mInitialState.head(dof), true, true, false);  // See #122
+  mAtlasRobot->setState(mInitialState, true, true, false);
 
   dtmsg << "Robot is reset." << std::endl;
 }

apps/atlasRobot/Main.cpp

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

apps/atlasRobot/State.cpp

@@ -155,8 +155,8 @@ void State::computeControlForce(double _timestep)
   assert(mNextState != NULL && "Next state should be set.");
 
   int dof = mSkeleton->getNumGenCoords();
-  VectorXd q = mSkeleton->get_q();
-  VectorXd dq = mSkeleton->get_dq();
+  VectorXd q = mSkeleton->getConfigs();
+  VectorXd dq = mSkeleton->getGenVels();
 
   // Compute relative joint angles from desired global angles of the pelvis and
   // the swing leg

apps/balance/Controller.cpp

@@ -61,7 +61,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)->get_q();
+    mDesiredDofs[i] = mSkel->getGenCoord(i)->getConfig();
   }
 
   // using SPD results in simple Kp coefficients

apps/balance/Main.cpp

@@ -64,7 +64,8 @@ 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)->setConfig(genCoordIds, initConfig);
+  myWorld->getSkeleton(1)->setConfigSegs(genCoordIds, initConfig,
+                                         true, true, false);
 
   // create controller
   Controller* myController = new Controller(myWorld->getSkeleton(1),

apps/balance/MyWindow.cpp

@@ -58,8 +58,8 @@ void MyWindow::timeStepping() {
 
   mController->setConstrForces(
         mWorld->getConstraintHandler()->getTotalConstraintForce(1));
-  mController->computeTorques(mWorld->getSkeleton(1)->get_q(),
-                              mWorld->getSkeleton(1)->get_dq());
+  mController->computeTorques(mWorld->getSkeleton(1)->getConfigs(),
+                              mWorld->getSkeleton(1)->getGenVels());
   mWorld->getSkeleton(1)->setInternalForceVector(mController->getTorques());
 
   mWorld->step();

apps/ballJointConstraintTest/Main.cpp

@@ -69,7 +69,7 @@ int main(int argc, char* argv[])
     Eigen::VectorXd initPose(dof);
     for (int i = 0; i < dof; i++)
         initPose[i] = random(-0.5, 0.5);
-    myWorld->getSkeleton(0)->setConfig(initPose);
+    myWorld->getSkeleton(0)->setConfigs(initPose, true, true, false);
 
     // Add damping to every joint
     for (int i = 0; i < myWorld->getSkeleton(0)->getNumBodyNodes(); i++) {

apps/ballJointConstraintTest/MyWindow.cpp

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

apps/closedLoop/Main.cpp

@@ -73,7 +73,7 @@ int main(int argc, char* argv[])
     initPose[23] = 3.14159 * 0.4;
     initPose[26] = 3.14159 * 0.4;
     initPose[29] = 3.14159 * 0.4;
-    myWorld->getSkeleton(0)->setConfig(initPose);
+    myWorld->getSkeleton(0)->setConfigs(initPose, true, true, false);
 
     // create a ball joint constraint
     BodyNode *bd1 = myWorld->getSkeleton(0)->getBodyNode("link 6");

apps/closedLoop/MyWindow.cpp

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

apps/forwardSim/Main.cpp

@@ -57,7 +57,7 @@ int main(int argc, char* argv[]) {
   Eigen::VectorXd initPose(dof);
   for (int i = 0; i < dof; i++)
     initPose[i] = dart::math::random(-0.5, 0.5);
-  myWorld->getSkeleton(0)->setConfig(initPose);
+  myWorld->getSkeleton(0)->setConfigs(initPose, true, true, false);
 
   // create a window and link it to the world
   MyWindow window;

apps/forwardSim/MyWindow.cpp

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

apps/hardcodedDesign/Main.cpp

@@ -87,9 +87,9 @@ dart::dynamics::Joint* create1DOFJoint(double val, double min, double max,
                  Eigen::Vector3d(1.0, 0.0, 0.0));
   // Add the transformation to the joint, set the min/max values and set it to
   // the skeleton
-  newJoint->getGenCoord(0)->set_q(val);
-  newJoint->getGenCoord(0)->set_qMin(min);
-  newJoint->getGenCoord(0)->set_qMax(max);
+  newJoint->getGenCoord(0)->setConfig(val);
+  newJoint->getGenCoord(0)->setConfigMin(min);
+  newJoint->getGenCoord(0)->setConfigMax(max);
 
   return newJoint;
 }

apps/hardcodedDesign/MyWindow.cpp

@@ -61,9 +61,9 @@ void MyWindow::keyboard(unsigned char _key, int _x, int _y) {
     case '2':
     case '3': {
       size_t dofIdx = _key - 49;
-      Eigen::VectorXd pose = skel->get_q();
+      Eigen::VectorXd pose = skel->getConfigs();
       pose(dofIdx) = pose(dofIdx) + (inverse ? -dDOF : dDOF);
-      skel->setConfig(pose);
+      skel->setConfigs(pose, true, true, false);
       std::cout << "Updated pose DOF " << dofIdx << ": " << pose.transpose()
                 << std::endl;
       glutPostRedisplay();

apps/harnessTest/Controller.cpp

@@ -62,7 +62,7 @@ Controller::Controller(dynamics::Skeleton* _skel, constraint::ConstraintDynamics
     mDesiredDofs.resize(nDof);
     for (int i = 0; i < nDof; i++){
         mTorques[i] = 0.0;
-        mDesiredDofs[i] = mSkel->getGenCoord(i)->get_q();
+        mDesiredDofs[i] = mSkel->getGenCoord(i)->getConfig();
     }
 
     // using SPD results in simple Kp coefficients

apps/harnessTest/Main.cpp

@@ -68,7 +68,8 @@ int main(int argc, char* argv[])
     genCoordIds.push_back(13);  // lower back
     Eigen::VectorXd initConfig(9);
     initConfig << -0.1, 0.2, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1;
-    myWorld->getSkeleton(1)->setConfig(genCoordIds, initConfig);
+    myWorld->getSkeleton(1)->setConfigSegs(genCoordIds, initConfig,
+                                           true, true, false);
 
     // create controller
     Controller* myController = new Controller(myWorld->getSkeleton(1),

apps/harnessTest/MyWindow.cpp

@@ -58,7 +58,7 @@ void MyWindow::timeStepping()
     mWorld->getSkeleton(1)->getBodyNode("h_spine")->addExtForce(mForce);
 
     mController->setConstrForces(mWorld->getConstraintHandler()->getTotalConstraintForce(1));
-    mController->computeTorques(mWorld->getSkeleton(1)->get_q(), mWorld->getSkeleton(1)->get_dq());
+    mController->computeTorques(mWorld->getSkeleton(1)->getConfigs(), mWorld->getSkeleton(1)->getGenVels());
     mWorld->getSkeleton(1)->setInternalForceVector(mController->getTorques());
 
     mWorld->step();

apps/meshCollision/Main.cpp

@@ -92,7 +92,7 @@ int main(int argc, char* argv[]) {
   // Verify that our skeleton has something inside :)
   std::printf("Our skeleton has %d nodes \n", MeshSkel->getNumBodyNodes());
   // std::printf("Our skeleton has %d joints \n", MeshSkel->getNumJoints());
-  std::printf("Our skeleton has %d DOFs \n", MeshSkel->getNumGenCoords());
+  std::printf("Our skeleton has %zu DOFs \n", MeshSkel->getNumGenCoords());
 
   MyWindow window;
   window.setWorld(myWorld);

apps/softArticulatedBodiesTest/Main.cpp

@@ -63,7 +63,7 @@ int main(int argc, char* argv[])
   Eigen::VectorXd initPose = Eigen::VectorXd::Zero(dof);
   for (int i = 0; i < 3; i++)
     initPose[i] = dart::math::random(-0.5, 0.5);
-  myWorld->getSkeleton(1)->setConfig(initPose);
+  myWorld->getSkeleton(1)->setConfigs(initPose, true, true, false);
 
   // create a window and link it to the world
   MyWindow window;

apps/softOpenChain/Main.cpp

@@ -63,7 +63,7 @@ int main(int argc, char* argv[])
   Eigen::VectorXd initPose = Eigen::VectorXd::Zero(dof);
   for (int i = 0; i < 3; i++)
     initPose[i] = dart::math::random(-0.5, 0.5);
-  myWorld->getSkeleton("skeleton 1")->setConfig(initPose);
+  myWorld->getSkeleton("skeleton 1")->setConfigs(initPose, true, true, false);
 
   // create a window and link it to the world
   MyWindow window;

apps/vehicle/MyWindow.cpp

@@ -58,9 +58,9 @@ void MyWindow::timeStepping() {
   dart::dynamics::Skeleton* vehicle = mWorld->getSkeleton("car_skeleton");
   assert(vehicle != 0);
 
-  Eigen::VectorXd q   = vehicle->get_q();
-  Eigen::VectorXd dq  = vehicle->get_dq();
-  Eigen::VectorXd tau = vehicle->get_tau();
+  Eigen::VectorXd q   = vehicle->getConfigs();
+  Eigen::VectorXd dq  = vehicle->getGenVels();
+  Eigen::VectorXd tau = vehicle->getGenForces();
   tau.setZero();
 
   if (true)

dart/constraint/BallJointConstraint.cpp

@@ -46,7 +46,7 @@ void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::VectorXd
     getJacobian();
     _J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
     Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
-    Eigen::VectorXd qDot1 = mBodyNode1->getSkeleton()->get_dq();
+    Eigen::VectorXd qDot1 = mBodyNode1->getSkeleton()->getGenVels();
     _C.segment(_rowIndex, 3) = worldP1 - mOffset2;
     _CDot.segment(_rowIndex, 3) = mJ1 * qDot1;
 }
@@ -58,9 +58,9 @@ void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::MatrixXd
     _J2.block(_rowIndex, 0, 3, mBodyNode2->getSkeleton()->getNumGenCoords()) += mJ2;
 
     Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
-    Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->get_dq();
+    Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->getGenVels();
     Eigen::VectorXd worldP2 = mBodyNode2->getWorldTransform() * mOffset2;
-    Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->get_dq();
+    Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->getGenVels();
 
     _C.segment(_rowIndex, 3) = worldP1 - worldP2;
     _CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;

dart/constraint/ConstraintDynamics.cpp

@@ -113,9 +113,9 @@ void ConstraintDynamics::computeConstraintForces()
       for (int k = 0; k < mSkeletons[i]->getJoint(j)->getNumGenCoords(); k++)
       {
         dynamics::GenCoord* genCoord = joint->getGenCoord(k);
-        double val = genCoord->get_q();
-        double ub  = genCoord->get_qMax();
-        double lb  = genCoord->get_qMin();
+        double val = genCoord->getConfig();
+        double ub  = genCoord->getConfigMax();
+        double lb  = genCoord->getConfigMin();
         double violation = 0.0;
 
         if (val >= ub)
@@ -1057,7 +1057,7 @@ void ConstraintDynamics::updateTauStar()
                           + mSkeletons[i]->getInternalForceVector()
                           + mSkeletons[i]->getDampingForceVector();
     Eigen::VectorXd tauStar =
-        (mSkeletons[i]->getAugMassMatrix() * mSkeletons[i]->get_dq())
+        (mSkeletons[i]->getAugMassMatrix() * mSkeletons[i]->getGenVels())
         - (mDt * (mSkeletons[i]->getCombinedVector() - tau));
     mTauStar.block(startRow, 0, tauStar.rows(), 1) = tauStar;
     startRow += tauStar.rows();
@@ -1324,7 +1324,7 @@ void ConstraintDynamics::updateConstraintTerms()
   {
     if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0)
       continue;
-    Eigen::VectorXd qDot = mSkeletons[i]->get_dq();
+    Eigen::VectorXd qDot = mSkeletons[i]->getGenVels();
     mTauHat.noalias() += -(mJ[i] - mPreJ[i]) / mDt * qDot;
     mTauHat.noalias() -= mJMInv[i] * (mSkeletons[i]->getInternalForceVector()
                                       + mSkeletons[i]->getExternalForceVector()

dart/constraint/RevoluteJointConstraint.cpp

@@ -37,7 +37,7 @@ void RevoluteJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::Vecto
     getJacobian();
     _J1.block(_rowIndex, 0, mNumRows, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
     Eigen::Vector3d v1 = mBodyNode1->getWorldTransform().rotation() * mAxis1;
-    Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->get_dq();
+    Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->getGenVels();
     _C.segment(_rowIndex, mNumRows) = v1 - mAxis2;
     _CDot.segment(_rowIndex, mNumRows) = mJ1 * qDot1;
 }
@@ -49,9 +49,9 @@ void RevoluteJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::Matri
     _J2.block(_rowIndex, 0, mNumRows, mBodyNode2->getSkeleton()->getNumGenCoords()) += mJ2;
 
     Eigen::Vector3d v1 = mBodyNode1->getWorldTransform().rotation() * mAxis1;
-    Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->get_dq();
+    Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->getGenVels();
     Eigen::VectorXd v2 = mBodyNode2->getWorldTransform().rotation() * mAxis2;
-    Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->get_dq();
+    Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->getGenVels();
 
     _C.segment(_rowIndex, mNumRows) = v1 - v2;
     _CDot.segment(_rowIndex, mNumRows) = mJ1 * qDot1 + mJ2 * qDot2;