diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp
index 05708191c4e32..a616858012863 100644
--- a/dart/dynamics/BallJoint.cpp
+++ b/dart/dynamics/BallJoint.cpp
@@ -70,6 +70,46 @@ Eigen::Matrix3d BallJoint::convertToRotation(const Eigen::Vector3d& _positions)
   return math::expMapRot(_positions);
 }
 
+//==============================================================================
+void BallJoint::setPositionsStatic(const Eigen::Vector3d& _positions)
+{
+  mR = convertToRotation(_positions);
+
+  MultiDofJoint::setPositionsStatic(_positions);
+}
+
+//==============================================================================
+Eigen::Vector3d BallJoint::getPositionDifferencesStatic(
+    const Eigen::Vector3d& _q0, const Eigen::Vector3d& _q1) const
+{
+  Eigen::Vector3d dq;
+
+  const Eigen::Matrix3d Jw  = getLocalJacobianStatic(_q0).topRows<3>();
+  const Eigen::Matrix3d R0T = math::expMapRot(-_q0);
+  const Eigen::Matrix3d R1  = math::expMapRot( _q1);
+
+  dq = Jw.inverse() * math::logMap(R0T * R1);
+
+  return dq;
+}
+
+//==============================================================================
+Eigen::Matrix<double, 6, 3> BallJoint::getLocalJacobianStatic(
+    const Eigen::Vector3d& _positions) const
+{
+  // Jacobian expressed in the Joint frame
+  Eigen::Matrix<double, 6, 3> J;
+  J.topRows<3>()    = math::expMapJac(-_positions);
+  J.bottomRows<3>() = Eigen::Matrix3d::Zero();
+
+  // Transform the reference frame to the child BodyNode frame
+  J = math::AdTJacFixed(mT_ChildBodyToJoint, J);
+
+  assert(!math::isNan(J));
+
+  return J;
+}
+
 //==============================================================================
 void BallJoint::integratePositions(double _dt)
 {
@@ -104,13 +144,7 @@ void BallJoint::updateLocalTransform() const
 //==============================================================================
 void BallJoint::updateLocalJacobian(bool) const
 {
-  Eigen::Matrix<double, 6, 3> J;
-  J.topRows<3>()    = math::expMapJac(getPositionsStatic()).transpose();
-  J.bottomRows<3>() = Eigen::Matrix3d::Zero();
-
-  mJacobian = math::AdTJacFixed(mT_ChildBodyToJoint, J);
-
-  assert(!math::isNan(mJacobian));
+  mJacobian = getLocalJacobian(getPositionsStatic());
 }
 
 //==============================================================================
diff --git a/dart/dynamics/BallJoint.h b/dart/dynamics/BallJoint.h
index a4e1f997da18f..ac5a14b23de07 100644
--- a/dart/dynamics/BallJoint.h
+++ b/dart/dynamics/BallJoint.h
@@ -72,7 +72,21 @@ class BallJoint : public MultiDofJoint<3>
   /// Convert a BallJoint-style position vector into a rotation matrix
   static Eigen::Matrix3d convertToRotation(const Eigen::Vector3d& _positions);
 
+  // Documentation inherited
+  void setPositionsStatic(const Eigen::Vector3d& _positions) override;
+
+  // Documentation inherited
+  Eigen::Vector3d getPositionDifferencesStatic(
+      const Eigen::Vector3d& _q0, const Eigen::Vector3d& _q1) const override;
+
+  // Documentation inherited
+  Eigen::Matrix<double, 6, 3> getLocalJacobianStatic(
+      const Eigen::Vector3d& _positions) const override;
+
 protected:
+
+  using MultiDofJoint::getLocalJacobianStatic;
+
   // Documentation inherited
   virtual void integratePositions(double _dt);
 
diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp
index 366637b37c12b..9beaf5d15d1a7 100644
--- a/dart/dynamics/EulerJoint.cpp
+++ b/dart/dynamics/EulerJoint.cpp
@@ -116,52 +116,14 @@ Eigen::Matrix3d EulerJoint::convertToRotation(const Eigen::Vector3d& _positions)
 }
 
 //==============================================================================
-void EulerJoint::updateDegreeOfFreedomNames()
+Eigen::Matrix<double, 6, 3> EulerJoint::getLocalJacobianStatic(
+    const Eigen::Vector3d& _positions) const
 {
-  std::vector<std::string> affixes;
-  switch (mAxisOrder)
-  {
-    case AO_ZYX:
-      affixes.push_back("_z");
-      affixes.push_back("_y");
-      affixes.push_back("_x");
-      break;
-    case AO_XYZ:
-      affixes.push_back("_x");
-      affixes.push_back("_y");
-      affixes.push_back("_z");
-      break;
-    default:
-      dterr << "Unsupported axis order in EulerJoint named '" << mName
-            << "' (" << mAxisOrder << ")\n";
-  }
+  Eigen::Matrix<double, 6, 3> J;
 
-  if (affixes.size() == 3)
-  {
-    for (size_t i = 0; i < 3; ++i)
-    {
-      if(!mDofs[i]->isNamePreserved())
-        mDofs[i]->setName(mName + affixes[i], false);
-    }
-  }
-}
-
-//==============================================================================
-void EulerJoint::updateLocalTransform() const
-{
-  mT = mT_ParentBodyToJoint * convertToTransform(getPositionsStatic())
-       * mT_ChildBodyToJoint.inverse();
-
-  assert(math::verifyTransform(mT));
-}
-
-//==============================================================================
-void EulerJoint::updateLocalJacobian(bool) const
-{
-  // double q0 = mPositions[0];
-  const Eigen::Vector3d& positions = getPositionsStatic();
-  double q1 = positions[1];
-  double q2 = positions[2];
+  // double q0 = _positions[0];
+  const double q1 = _positions[1];
+  const double q2 = _positions[2];
 
   // double c0 = cos(q0);
   double c1 = cos(q1);
@@ -195,9 +157,9 @@ void EulerJoint::updateLocalJacobian(bool) const
       if (fabs(getPositionsStatic()[1]) == DART_PI * 0.5)
         std::cout << "Singular configuration in ZYX-euler joint ["
                   << mName << "]. ("
-                  << positions[0] << ", "
-                  << positions[1] << ", "
-                  << positions[2] << ")"
+                  << _positions[0] << ", "
+                  << _positions[1] << ", "
+                  << _positions[2] << ")"
                   << std::endl;
 #endif
 
@@ -218,12 +180,12 @@ void EulerJoint::updateLocalJacobian(bool) const
       J2 << 1.0,   0.0,   0.0, 0.0, 0.0, 0.0;
 
 #ifndef NDEBUG
-      if (fabs(positions[1]) == DART_PI * 0.5)
+      if (fabs(_positions[1]) == DART_PI * 0.5)
         std::cout << "Singular configuration in ZYX-euler joint ["
                   << mName << "]. ("
-                  << positions[0] << ", "
-                  << positions[1] << ", "
-                  << positions[2] << ")"
+                  << _positions[0] << ", "
+                  << _positions[1] << ", "
+                  << _positions[2] << ")"
                   << std::endl;
 #endif
 
@@ -236,14 +198,14 @@ void EulerJoint::updateLocalJacobian(bool) const
     }
   }
 
-  mJacobian.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
-  mJacobian.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
-  mJacobian.col(2) = math::AdT(mT_ChildBodyToJoint, J2);
+  J.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
+  J.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
+  J.col(2) = math::AdT(mT_ChildBodyToJoint, J2);
 
-  assert(!math::isNan(mJacobian));
+  assert(!math::isNan(J));
 
 #ifndef NDEBUG
-  Eigen::MatrixXd JTJ = mJacobian.transpose() * mJacobian;
+  Eigen::MatrixXd JTJ = J.transpose() * J;
   Eigen::FullPivLU<Eigen::MatrixXd> luJTJ(JTJ);
   //    Eigen::FullPivLU<Eigen::MatrixXd> luS(mS);
   double det = luJTJ.determinant();
@@ -257,6 +219,54 @@ void EulerJoint::updateLocalJacobian(bool) const
     //        std::cout << "mS: \n" << mS << std::endl;
   }
 #endif
+
+  return J;
+}
+
+//==============================================================================
+void EulerJoint::updateDegreeOfFreedomNames()
+{
+  std::vector<std::string> affixes;
+  switch (mAxisOrder)
+  {
+    case AO_ZYX:
+      affixes.push_back("_z");
+      affixes.push_back("_y");
+      affixes.push_back("_x");
+      break;
+    case AO_XYZ:
+      affixes.push_back("_x");
+      affixes.push_back("_y");
+      affixes.push_back("_z");
+      break;
+    default:
+      dterr << "Unsupported axis order in EulerJoint named '" << mName
+            << "' (" << mAxisOrder << ")\n";
+  }
+
+  if (affixes.size() == 3)
+  {
+    for (size_t i = 0; i < 3; ++i)
+    {
+      if(!mDofs[i]->isNamePreserved())
+        mDofs[i]->setName(mName + affixes[i], false);
+    }
+  }
+}
+
+//==============================================================================
+void EulerJoint::updateLocalTransform() const
+{
+  mT = mT_ParentBodyToJoint * convertToTransform(getPositionsStatic())
+       * mT_ChildBodyToJoint.inverse();
+
+  assert(math::verifyTransform(mT));
+}
+
+//==============================================================================
+void EulerJoint::updateLocalJacobian(bool) const
+{
+  mJacobian = getLocalJacobianStatic(getPositionsStatic());
 }
 
 //==============================================================================
diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h
index e6698a7cd3534..0d7b90185b3e8 100644
--- a/dart/dynamics/EulerJoint.h
+++ b/dart/dynamics/EulerJoint.h
@@ -117,7 +117,14 @@ class EulerJoint : public MultiDofJoint<3>
 
   Eigen::Matrix3d convertToRotation(const Eigen::Vector3d& _positions) const;
 
+  // Documentation inherited
+  Eigen::Matrix<double, 6, 3> getLocalJacobianStatic(
+      const Eigen::Vector3d& _positions) const override;
+
 protected:
+
+  using MultiDofJoint::getLocalJacobianStatic;
+
   /// Set the names of this joint's DegreesOfFreedom. Used during construction
   /// and when axis order is changed.
   virtual void updateDegreeOfFreedomNames();
diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp
index 93ef7338caa13..79920657b7314 100644
--- a/dart/dynamics/FreeJoint.cpp
+++ b/dart/dynamics/FreeJoint.cpp
@@ -76,12 +76,60 @@ Eigen::Isometry3d FreeJoint::convertToTransform(
   return tf;
 }
 
+//==============================================================================
+void FreeJoint::setPositionsStatic(const Eigen::Vector6d& _positions)
+{
+  mQ = convertToTransform(_positions);
+
+  MultiDofJoint::setPositionsStatic(_positions);
+}
+
+//==============================================================================
+Eigen::Vector6d FreeJoint::getPositionDifferencesStatic(
+    const Eigen::Vector6d& _q0,
+    const Eigen::Vector6d& _q1) const
+{
+  Eigen::Vector6d dq;
+
+  const Eigen::Matrix3d Jw  = getLocalJacobianStatic(_q0).topLeftCorner<3,3>();
+  const Eigen::Matrix3d R0T = math::expMapRot(-_q0.head<3>());
+  const Eigen::Matrix3d R1  = math::expMapRot( _q1.head<3>());
+
+  dq.head<3>() = Jw.inverse() * math::logMap(R0T * R1);
+  dq.tail<3>() = _q1.tail<3>() - _q0.tail<3>();
+
+  return dq;
+}
+
+//==============================================================================
+Eigen::Matrix6d FreeJoint::getLocalJacobianStatic(
+    const Eigen::Vector6d& _positions) const
+{
+  // Jacobian expressed in the Joint frame
+  Eigen::Matrix6d J = Eigen::Matrix6d::Identity();
+  J.topLeftCorner<3,3>() = math::expMapJac(-_positions.head<3>());
+
+  // Transform the reference frame to the child BodyNode frame
+  J.leftCols<3>()  = math::AdTJacFixed(mT_ChildBodyToJoint, J.leftCols<3>());
+  J.bottomRightCorner<3,3>()
+      = mT_ChildBodyToJoint.linear() * math::expMapRot(-_positions.head<3>());
+
+  // Note that the top right 3x3 block of J is always zero
+  assert((J.topRightCorner<3,3>()) == Eigen::Matrix3d::Zero());
+
+  assert(!math::isNan(J));
+
+  return J;
+}
+
 //==============================================================================
 void FreeJoint::integratePositions(double _dt)
 {
   const Eigen::Vector6d& velocities = getVelocitiesStatic();
-  mQ.linear()      = mQ.linear() * math::expMapRot(
-        getLocalJacobianStatic().topRows<3>() * velocities * _dt);
+
+  mQ.linear() = mQ.linear()
+                * math::expMapRot(getLocalJacobianStatic().topLeftCorner<3,3>()
+                                  * velocities.head<3>() * _dt);
   mQ.translation() = mQ.translation() + velocities.tail<3>() * _dt;
 
   setPositionsStatic(convertToPositions(mQ));
@@ -117,18 +165,7 @@ void FreeJoint::updateLocalTransform() const
 //==============================================================================
 void FreeJoint::updateLocalJacobian(bool) const
 {
-  const Eigen::Vector6d& positions = getPositionsStatic();
-  Eigen::Matrix6d J = Eigen::Matrix6d::Identity();
-  J.topLeftCorner<3,3>() = math::expMapJac(positions.head<3>()).transpose();
-
-  mJacobian.leftCols<3>()
-      = math::AdTJacFixed(mT_ChildBodyToJoint, J.leftCols<3>());
-  mJacobian.rightCols<3>()
-      = math::AdTJacFixed(mT_ChildBodyToJoint
-                          * math::expAngular(-positions.head<3>()),
-                          J.rightCols<3>());
-
-  assert(!math::isNan(mJacobian));
+  mJacobian = getLocalJacobian(getPositionsStatic());
 }
 
 //==============================================================================
diff --git a/dart/dynamics/FreeJoint.h b/dart/dynamics/FreeJoint.h
index c506498474b08..7ecaf7a1e0781 100644
--- a/dart/dynamics/FreeJoint.h
+++ b/dart/dynamics/FreeJoint.h
@@ -67,7 +67,21 @@ class FreeJoint : public MultiDofJoint<6>
   /// Convert a FreeJoint-style 6D vector into a transform
   static Eigen::Isometry3d convertToTransform(const Eigen::Vector6d& _positions);
 
+  // Documentation inherited
+  void setPositionsStatic(const Eigen::Vector6d& _positions) override;
+
+  // Documentation inherited
+  Eigen::Matrix6d getLocalJacobianStatic(
+      const Eigen::Vector6d& _positions) const override;
+
+  // Documentation inherited
+  Eigen::Vector6d getPositionDifferencesStatic(
+      const Eigen::Vector6d& _q0, const Eigen::Vector6d& _q1) const override;
+
 protected:
+
+  using MultiDofJoint::getLocalJacobianStatic;
+
   // Documentation inherited
   virtual void integratePositions(double _dt);
 
diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h
index a6f2e39ba3aff..529cb31beda20 100644
--- a/dart/dynamics/Joint.h
+++ b/dart/dynamics/Joint.h
@@ -410,7 +410,7 @@ class Joint : public virtual common::Subject
   /// \}
 
   //----------------------------------------------------------------------------
-  /// \{ \name Integration
+  /// \{ \name Integration and finite difference
   //----------------------------------------------------------------------------
 
   /// Integrate positions using Euler method
@@ -419,6 +419,11 @@ class Joint : public virtual common::Subject
   /// Integrate velocities using Euler method
   virtual void integrateVelocities(double _dt) = 0;
 
+  /// Return the difference of two generalized coordinates which are measured in
+  /// the configuration space of this Skeleton.
+  virtual Eigen::VectorXd getPositionsDifference(
+      const Eigen::VectorXd& _q0, const Eigen::VectorXd& _q1) const = 0;
+
   /// \}
 
   //----------------------------------------------------------------------------
@@ -487,6 +492,11 @@ class Joint : public virtual common::Subject
   /// w.r.t. local generalized coordinate
   virtual const math::Jacobian getLocalJacobian() const = 0;
 
+  /// Get generalized Jacobian from parent body node to child body node
+  /// w.r.t. local generalized coordinate
+  virtual math::Jacobian getLocalJacobian(
+      const Eigen::VectorXd& _positions) const = 0;
+
   /// Get time derivative of generalized Jacobian from parent body node
   /// to child body node w.r.t. local generalized coordinate
   virtual const math::Jacobian getLocalJacobianTimeDeriv() const = 0;
diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h
index 806e3a70da194..c9d3d0c9eef3c 100644
--- a/dart/dynamics/MultiDofJoint.h
+++ b/dart/dynamics/MultiDofJoint.h
@@ -211,7 +211,7 @@ class MultiDofJoint : public Joint
   // code.
 
   /// Fixed-size version of setPositions()
-  void setPositionsStatic(const Eigen::Matrix<double, DOF, 1>& _positions);
+  virtual void setPositionsStatic(const Eigen::Matrix<double, DOF, 1>& _positions);
 
   /// Fixed-size version of getPositions()
   const Eigen::Matrix<double, DOF, 1>& getPositionsStatic() const;
@@ -286,7 +286,7 @@ class MultiDofJoint : public Joint
   virtual void resetConstraintImpulses();
 
   //----------------------------------------------------------------------------
-  // Integration
+  // Integration and finite difference
   //----------------------------------------------------------------------------
 
   // Documentation inherited
@@ -295,6 +295,15 @@ class MultiDofJoint : public Joint
   // Documentation inherited
   virtual void integrateVelocities(double _dt);
 
+  // Documentation inherited
+  Eigen::VectorXd getPositionsDifference(
+      const Eigen::VectorXd& _q0, const Eigen::VectorXd& _q1) const override;
+
+  /// Fixed-size version of getPositionsDifference()
+  virtual Eigen::Matrix<double, DOF, 1> getPositionDifferencesStatic(
+      const Eigen::Matrix<double, DOF, 1>& _q0,
+      const Eigen::Matrix<double, DOF, 1>& _q1) const;
+
   //----------------------------------------------------------------------------
   /// \{ \name Passive forces - spring, viscous friction, Coulomb friction
   //----------------------------------------------------------------------------
@@ -344,6 +353,14 @@ class MultiDofJoint : public Joint
   /// Fixed-size version of getLocalJacobian()
   const Eigen::Matrix<double, 6, DOF>& getLocalJacobianStatic() const;
 
+  // Documentation inherited
+  math::Jacobian getLocalJacobian(
+      const Eigen::VectorXd& _positions) const override;
+
+  /// Fixed-size version of getLocalJacobian()
+  virtual Eigen::Matrix<double, 6, DOF> getLocalJacobianStatic(
+      const Eigen::Matrix<double, DOF, 1>& _positions) const = 0;
+
   // Documentation inherited
   const math::Jacobian getLocalJacobianTimeDeriv() const override;
 
@@ -1504,6 +1521,32 @@ void MultiDofJoint<DOF>::integrateVelocities(double _dt)
   setVelocitiesStatic(getVelocitiesStatic() + getAccelerationsStatic() * _dt);
 }
 
+//==============================================================================
+template <size_t DOF>
+Eigen::VectorXd MultiDofJoint<DOF>::getPositionsDifference(
+    const Eigen::VectorXd& _q0, const Eigen::VectorXd& _q1) const
+{
+  if (static_cast<size_t>(_q0.size()) != getNumDofs()
+      || static_cast<size_t>(_q1.size()) != getNumDofs())
+  {
+    dterr << "MultiDofJoint::getPositionsDifference: q0's size[" << _q0.size()
+          << "] or q1's size[" << _q1.size() << "is different with the dof ["
+          << getNumDofs() << "]." << std::endl;
+    return Eigen::VectorXd::Zero(getNumDofs());
+  }
+
+  return getPositionDifferencesStatic(_q0, _q1);
+}
+
+//==============================================================================
+template <size_t DOF>
+Eigen::Matrix<double, DOF, 1> MultiDofJoint<DOF>::getPositionDifferencesStatic(
+    const Eigen::Matrix<double, DOF, 1>& _q0,
+    const Eigen::Matrix<double, DOF, 1>& _q1) const
+{
+  return _q1 - _q0;
+}
+
 //==============================================================================
 template <size_t DOF>
 void MultiDofJoint<DOF>::setSpringStiffness(size_t _index, double _k)
@@ -1657,12 +1700,7 @@ Eigen::Vector6d MultiDofJoint<DOF>::getBodyConstraintWrench() const
 template <size_t DOF>
 const math::Jacobian MultiDofJoint<DOF>::getLocalJacobian() const
 {
-  if(mIsLocalJacobianDirty)
-  {
-    updateLocalJacobian(false);
-    mIsLocalJacobianDirty = false;
-  }
-  return mJacobian;
+  return getLocalJacobianStatic();
 }
 
 //==============================================================================
@@ -1678,6 +1716,14 @@ MultiDofJoint<DOF>::getLocalJacobianStatic() const
   return mJacobian;
 }
 
+//==============================================================================
+template <size_t DOF>
+math::Jacobian MultiDofJoint<DOF>::getLocalJacobian(
+    const Eigen::VectorXd& _positions) const
+{
+  return getLocalJacobianStatic(_positions);
+}
+
 //==============================================================================
 template <size_t DOF>
 const math::Jacobian MultiDofJoint<DOF>::getLocalJacobianTimeDeriv() const
diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp
index 3ecc4b67e5b04..95b7ce1c2fe0e 100644
--- a/dart/dynamics/PlanarJoint.cpp
+++ b/dart/dynamics/PlanarJoint.cpp
@@ -151,6 +151,27 @@ const Eigen::Vector3d& PlanarJoint::getTranslationalAxis2() const
   return mTransAxis2;
 }
 
+//==============================================================================
+Eigen::Matrix<double, 6, 3> PlanarJoint::getLocalJacobianStatic(
+    const Eigen::Vector3d& _positions) const
+{
+  Eigen::Matrix<double, 6, 3> J = Eigen::Matrix<double, 6, 3>::Zero();
+  J.block<3, 1>(3, 0) = mTransAxis1;
+  J.block<3, 1>(3, 1) = mTransAxis2;
+  J.block<3, 1>(0, 2) = mRotAxis;
+
+  J.leftCols<2>()
+      = math::AdTJacFixed(mT_ChildBodyToJoint
+                          * math::expAngular(mRotAxis * -_positions[2]),
+                          J.leftCols<2>());
+  J.col(2) = math::AdTJac(mT_ChildBodyToJoint, J.col(2));
+
+  // Verification
+  assert(!math::isNan(J));
+
+  return J;
+}
+
 //==============================================================================
 void PlanarJoint::updateDegreeOfFreedomNames()
 {
@@ -205,19 +226,7 @@ void PlanarJoint::updateLocalTransform() const
 //==============================================================================
 void PlanarJoint::updateLocalJacobian(bool) const
 {
-  Eigen::Matrix<double, 6, 3> J = Eigen::Matrix<double, 6, 3>::Zero();
-  J.block<3, 1>(3, 0) = mTransAxis1;
-  J.block<3, 1>(3, 1) = mTransAxis2;
-  J.block<3, 1>(0, 2) = mRotAxis;
-
-  mJacobian.leftCols<2>()
-      = math::AdTJacFixed(mT_ChildBodyToJoint
-                        * math::expAngular(mRotAxis * -getPositionsStatic()[2]),
-                        J.leftCols<2>());
-  mJacobian.col(2) = math::AdTJac(mT_ChildBodyToJoint, J.col(2));
-
-  // Verification
-  assert(!math::isNan(mJacobian));
+  mJacobian = getLocalJacobianStatic(getPositionsStatic());
 }
 
 //==============================================================================
diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h
index 82efe1f1244b5..49bc69f96c905 100644
--- a/dart/dynamics/PlanarJoint.h
+++ b/dart/dynamics/PlanarJoint.h
@@ -103,7 +103,14 @@ class PlanarJoint : public MultiDofJoint<3>
   /// Return second translational axis
   const Eigen::Vector3d& getTranslationalAxis2() const;
 
+  // Documentation inherited
+  Eigen::Matrix<double, 6, 3> getLocalJacobianStatic(
+      const Eigen::Vector3d& _positions) const override;
+
 protected:
+
+  using MultiDofJoint::getLocalJacobianStatic;
+
   /// Set the names of this joint's DegreesOfFreedom. Used during construction
   /// and when the Plane type is changed.
   virtual void updateDegreeOfFreedomNames();
diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp
index 0090d0954993d..d826a181d1668 100644
--- a/dart/dynamics/SingleDofJoint.cpp
+++ b/dart/dynamics/SingleDofJoint.cpp
@@ -745,6 +745,30 @@ void SingleDofJoint::integrateVelocities(double _dt)
   setVelocityStatic(getVelocityStatic() + getAccelerationStatic() * _dt);
 }
 
+//==============================================================================
+Eigen::VectorXd SingleDofJoint::getPositionsDifference(
+    const Eigen::VectorXd& _q0, const Eigen::VectorXd& _q1) const
+{
+  if (static_cast<size_t>(_q0.size()) != getNumDofs()
+      || static_cast<size_t>(_q1.size()) != getNumDofs())
+  {
+    dterr << "SingleDofJoint::getPositionsDifference: q0's size[" << _q0.size()
+          << "] or q1's size[" << _q1.size() << "is different with the dof ["
+          << getNumDofs() << "]." << std::endl;
+    return Eigen::Matrix<double, 1, 1>::Zero();
+  }
+
+  return Eigen::Matrix<double, 1, 1>::Constant(
+        getPositionDifferenceStatic(_q0[0], _q1[0]));
+}
+
+//==============================================================================
+double SingleDofJoint::getPositionDifferenceStatic(double _q0,
+                                                    double _q1) const
+{
+  return _q1 - _q0;
+}
+
 //==============================================================================
 void SingleDofJoint::setSpringStiffness(size_t _index, double _k)
 {
@@ -913,12 +937,7 @@ Eigen::Vector6d SingleDofJoint::getBodyConstraintWrench() const
 //==============================================================================
 const math::Jacobian SingleDofJoint::getLocalJacobian() const
 {
-  if(mIsLocalJacobianDirty)
-  {
-    updateLocalJacobian(false);
-    mIsLocalJacobianDirty = false;
-  }
-  return mJacobian;
+  return getLocalJacobianStatic();
 }
 
 //==============================================================================
@@ -932,6 +951,14 @@ const Eigen::Vector6d& SingleDofJoint::getLocalJacobianStatic() const
   return mJacobian;
 }
 
+//==============================================================================
+math::Jacobian SingleDofJoint::getLocalJacobian(
+    const Eigen::VectorXd& /*_positions*/) const
+{
+  // The Jacobian is always constant w.r.t. the generalized coordinates.
+  return getLocalJacobianStatic();
+}
+
 //==============================================================================
 const math::Jacobian SingleDofJoint::getLocalJacobianTimeDeriv() const
 {
diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h
index a413f81b3ff40..7e0b8b62d6f05 100644
--- a/dart/dynamics/SingleDofJoint.h
+++ b/dart/dynamics/SingleDofJoint.h
@@ -277,7 +277,7 @@ class SingleDofJoint : public Joint
   virtual void resetConstraintImpulses();
 
   //----------------------------------------------------------------------------
-  // Integration
+  // Integration and finite difference
   //----------------------------------------------------------------------------
 
   // Documentation inherited
@@ -286,6 +286,13 @@ class SingleDofJoint : public Joint
   // Documentation inherited
   virtual void integrateVelocities(double _dt);
 
+  // Documentation inherited
+  Eigen::VectorXd getPositionsDifference(
+      const Eigen::VectorXd& _q0, const Eigen::VectorXd& _q1) const override;
+
+  /// Fixed-size version of getPositionsDifference()
+  double getPositionDifferenceStatic(double _q0, double _q1) const;
+
   //----------------------------------------------------------------------------
   /// \{ \name Passive forces - spring, viscous friction, Coulomb friction
   //----------------------------------------------------------------------------
@@ -348,6 +355,10 @@ class SingleDofJoint : public Joint
   /// Fixed-size version of getLocalJacobian()
   const Eigen::Vector6d& getLocalJacobianStatic() const;
 
+  // Documentation inherited
+  math::Jacobian getLocalJacobian(
+      const Eigen::VectorXd& _positions) const override;
+
   // Documentation inherited
   const math::Jacobian getLocalJacobianTimeDeriv() const override;
 
diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp
index 0ff2256648d99..c76993eedccfd 100644
--- a/dart/dynamics/Skeleton.cpp
+++ b/dart/dynamics/Skeleton.cpp
@@ -1159,6 +1159,55 @@ void Skeleton::integrateVelocities(double _dt)
   }
 }
 
+//==============================================================================
+Eigen::VectorXd Skeleton::getPositionDifferences(
+    const Eigen::VectorXd& _q0, const Eigen::VectorXd& _q1) const
+{
+  if (static_cast<size_t>(_q0.size()) != getNumDofs()
+      || static_cast<size_t>(_q1.size()) != getNumDofs())
+  {
+    dterr << "Skeleton::getPositionsDifference: q0's size[" << _q0.size()
+          << "] or q1's size[" << _q1.size() << "is different with the dof ["
+          << getNumDofs() << "]." << std::endl;
+    return Eigen::VectorXd::Zero(getNumDofs());
+  }
+
+  Eigen::VectorXd dq(getNumDofs());
+
+  for (const auto& bodyNode : mBodyNodes)
+  {
+    const Joint* joint = bodyNode->getParentJoint();
+    const size_t dof   = joint->getNumDofs();
+
+    if (dof)
+    {
+      size_t index = joint->getDof(0)->getIndexInSkeleton();
+      const Eigen::VectorXd& q0Seg = _q0.segment(index, dof);
+      const Eigen::VectorXd& q1Seg = _q1.segment(index, dof);
+      dq.segment(index, dof) = joint->getPositionsDifference(q0Seg, q1Seg);
+    }
+  }
+
+  return dq;
+}
+
+//==============================================================================
+Eigen::VectorXd Skeleton::getVelocityDifferences(
+    const Eigen::VectorXd& _dq0, const Eigen::VectorXd& _dq1) const
+{
+  if (static_cast<size_t>(_dq0.size()) != getNumDofs()
+      || static_cast<size_t>(_dq1.size()) != getNumDofs())
+  {
+    dterr << "Skeleton::getPositionsDifference: dq0's size[" << _dq0.size()
+          << "] or dq1's size[" << _dq1.size() << "is different with the dof ["
+          << getNumDofs() << "]." << std::endl;
+    return Eigen::VectorXd::Zero(getNumDofs());
+  }
+
+  // All the tangent spaces of Joint's configuration spaces are vector spaces.
+  return _dq1 - _dq0;
+}
+
 //==============================================================================
 void Skeleton::computeForwardKinematics(bool _updateTransforms,
                                         bool _updateVels,
diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h
index db2b4148995d5..084cf6417170d 100644
--- a/dart/dynamics/Skeleton.h
+++ b/dart/dynamics/Skeleton.h
@@ -426,7 +426,7 @@ class Skeleton
   Eigen::VectorXd getJointConstraintImpulses() const;
 
   //----------------------------------------------------------------------------
-  // Integration
+  // Integration and finite difference
   //----------------------------------------------------------------------------
 
   // Documentation inherited
@@ -435,6 +435,16 @@ class Skeleton
   // Documentation inherited
   void integrateVelocities(double _dt);
 
+  /// Return the difference of two generalized positions which are measured in
+  /// the configuration space of this Skeleton.
+  Eigen::VectorXd getPositionDifferences(
+      const Eigen::VectorXd& _q0, const Eigen::VectorXd& _q1) const;
+
+  /// Return the difference of two generalized velocities or accelerations which
+  /// are measured in the tangent space at the identity.
+  Eigen::VectorXd getVelocityDifferences(
+      const Eigen::VectorXd& _dq0, const Eigen::VectorXd& _dq1) const;
+
   //----------------------------------------------------------------------------
   // State
   //----------------------------------------------------------------------------
diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp
index 6857975e6a738..792fac24cb302 100644
--- a/dart/dynamics/TranslationalJoint.cpp
+++ b/dart/dynamics/TranslationalJoint.cpp
@@ -57,6 +57,14 @@ TranslationalJoint::~TranslationalJoint()
 {
 }
 
+//==============================================================================
+Eigen::Matrix<double, 6, 3> TranslationalJoint::getLocalJacobianStatic(
+    const Eigen::Vector3d& /*_positions*/) const
+{
+  // The Jacobian is always constant w.r.t. the generalized coordinates.
+  return getLocalJacobianStatic();
+}
+
 //==============================================================================
 void TranslationalJoint::updateDegreeOfFreedomNames()
 {
@@ -82,22 +90,13 @@ void TranslationalJoint::updateLocalTransform() const
 //==============================================================================
 void TranslationalJoint::updateLocalJacobian(bool _mandatory) const
 {
-  if(_mandatory)
+  if (_mandatory)
   {
-    Eigen::Vector6d J0;
-    Eigen::Vector6d J1;
-    Eigen::Vector6d J2;
-
-    J0 << 0, 0, 0, 1, 0, 0;
-    J1 << 0, 0, 0, 0, 1, 0;
-    J2 << 0, 0, 0, 0, 0, 1;
-
-    mJacobian.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
-    mJacobian.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
-    mJacobian.col(2) = math::AdT(mT_ChildBodyToJoint, J2);
+    mJacobian.bottomRows<3>() = mT_ChildBodyToJoint.linear();
 
     // Verification
-    assert(!math::isNan(mJacobian));
+    assert(mJacobian.topRows<3>() == Eigen::Matrix3d::Zero());
+    assert(!math::isNan(mJacobian.bottomRows<3>()));
   }
 }
 
diff --git a/dart/dynamics/TranslationalJoint.h b/dart/dynamics/TranslationalJoint.h
index c6984b1b2c6b7..1737d96dd3763 100644
--- a/dart/dynamics/TranslationalJoint.h
+++ b/dart/dynamics/TranslationalJoint.h
@@ -54,7 +54,14 @@ class TranslationalJoint : public MultiDofJoint<3>
   /// Destructor
   virtual ~TranslationalJoint();
 
+  // Documentation inherited
+  Eigen::Matrix<double, 6, 3> getLocalJacobianStatic(
+      const Eigen::Vector3d& _positions) const override;
+
 protected:
+
+  using MultiDofJoint::getLocalJacobianStatic;
+
   // Documentation inherited
   virtual void updateDegreeOfFreedomNames();
 
@@ -62,7 +69,7 @@ class TranslationalJoint : public MultiDofJoint<3>
   virtual void updateLocalTransform() const;
 
   // Documentation inherited
-  virtual void updateLocalJacobian(bool _mandatory=true) const;
+  virtual void updateLocalJacobian(bool _mandatory = true) const;
 
   // Documentation inherited
   virtual void updateLocalJacobianTimeDeriv() const;
diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp
index fb56b47b8ce8b..c50d8b76a04d5 100644
--- a/dart/dynamics/UniversalJoint.cpp
+++ b/dart/dynamics/UniversalJoint.cpp
@@ -88,6 +88,20 @@ const Eigen::Vector3d& UniversalJoint::getAxis2() const
   return mAxis[1];
 }
 
+//==============================================================================
+Eigen::Matrix<double, 6, 2> UniversalJoint::getLocalJacobianStatic(
+    const Eigen::Vector2d& _positions) const
+{
+  Eigen::Matrix<double, 6, 2> J;
+  J.col(0) = math::AdTAngular(
+               mT_ChildBodyToJoint
+               * math::expAngular(-mAxis[1] * _positions[1]),
+                                  mAxis[0]);
+  J.col(1) = math::AdTAngular(mT_ChildBodyToJoint, mAxis[1]);
+  assert(!math::isNan(J));
+  return J;
+}
+
 //==============================================================================
 void UniversalJoint::updateDegreeOfFreedomNames()
 {
@@ -111,12 +125,7 @@ void UniversalJoint::updateLocalTransform() const
 //==============================================================================
 void UniversalJoint::updateLocalJacobian(bool) const
 {
-  mJacobian.col(0) = math::AdTAngular(
-                       mT_ChildBodyToJoint
-                       * math::expAngular(-mAxis[1] * getPositionsStatic()[1]),
-                                          mAxis[0]);
-  mJacobian.col(1) = math::AdTAngular(mT_ChildBodyToJoint, mAxis[1]);
-  assert(!math::isNan(mJacobian));
+  mJacobian = getLocalJacobianStatic(getPositionsStatic());
 }
 
 //==============================================================================
diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h
index 280587180eef0..a5c0db0023bc9 100644
--- a/dart/dynamics/UniversalJoint.h
+++ b/dart/dynamics/UniversalJoint.h
@@ -70,7 +70,14 @@ class UniversalJoint : public MultiDofJoint<2>
   ///
   const Eigen::Vector3d& getAxis2() const;
 
+  // Documentation inherited
+  Eigen::Matrix<double, 6, 2> getLocalJacobianStatic(
+      const Eigen::Vector2d& _positions) const override;
+
 protected:
+
+  using MultiDofJoint::getLocalJacobianStatic;
+
   // Documentation inherited
   virtual void updateDegreeOfFreedomNames();
 
diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp
index a5c2bcc62a705..60062da17c4e4 100644
--- a/dart/dynamics/ZeroDofJoint.cpp
+++ b/dart/dynamics/ZeroDofJoint.cpp
@@ -395,7 +395,15 @@ void ZeroDofJoint::integratePositions(double _dt)
 //==============================================================================
 void ZeroDofJoint::integrateVelocities(double _dt)
 {
-    // Do nothing
+  // Do nothing
+}
+
+//==============================================================================
+Eigen::VectorXd ZeroDofJoint::getPositionsDifference(
+    const Eigen::VectorXd& /*_q0*/,
+    const Eigen::VectorXd& /*_q1*/) const
+{
+  return Eigen::VectorXd::Zero(0);
 }
 
 //==============================================================================
@@ -471,6 +479,13 @@ const math::Jacobian ZeroDofJoint::getLocalJacobian() const
   return Eigen::Matrix<double, 6, 0>();
 }
 
+//==============================================================================
+math::Jacobian ZeroDofJoint::getLocalJacobian(
+    const Eigen::VectorXd& /*_positions*/) const
+{
+  return Eigen::Matrix<double, 6, 0>();
+}
+
 //==============================================================================
 const math::Jacobian ZeroDofJoint::getLocalJacobianTimeDeriv() const
 {
diff --git a/dart/dynamics/ZeroDofJoint.h b/dart/dynamics/ZeroDofJoint.h
index ae03365ce6a79..28c8a31b31beb 100644
--- a/dart/dynamics/ZeroDofJoint.h
+++ b/dart/dynamics/ZeroDofJoint.h
@@ -250,7 +250,7 @@ class ZeroDofJoint : public Joint
   virtual void resetConstraintImpulses();
 
   //----------------------------------------------------------------------------
-  // Integration
+  // Integration and finite difference
   //----------------------------------------------------------------------------
 
   // Documentation inherited
@@ -259,6 +259,10 @@ class ZeroDofJoint : public Joint
   // Documentation inherited
   virtual void integrateVelocities(double _dt);
 
+  // Documentation inherited
+  Eigen::VectorXd getPositionsDifference(
+      const Eigen::VectorXd& _q0, const Eigen::VectorXd& _q1) const override;
+
   //----------------------------------------------------------------------------
   /// \{ \name Passive forces - spring, viscous friction, Coulomb friction
   //----------------------------------------------------------------------------
@@ -309,6 +313,10 @@ class ZeroDofJoint : public Joint
   // Documentation inherited
   virtual const math::Jacobian getLocalJacobian() const override;
 
+  // Documentation inherited
+  math::Jacobian getLocalJacobian(
+      const Eigen::VectorXd& _positions) const override;
+
   // Documentation inherited
   virtual const math::Jacobian getLocalJacobianTimeDeriv() const override;
 
diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp
index 1d27e8972263a..d64c7f0a9b7b9 100644
--- a/unittests/testDynamics.cpp
+++ b/unittests/testDynamics.cpp
@@ -80,9 +80,13 @@ class DynamicsTest : public ::testing::Test
   // difference.
   void testJacobians(const std::string& _fileName);
 
+  // Compare velocities and accelerations with actual vaules and approximates
+  // using finite differece method.
+  void testFiniteDifferenceGeneralizedCoordinates(const std::string& _fileName);
+
   // Compare accelerations computed by recursive method, Jacobian, and finite
   // difference.
-  void testFiniteDifference(const std::string& _fileName);
+  void testFiniteDifferenceBodyNodeAcceleration(const std::string& _fileName);
 
   // Compare dynamics terms in equations of motion such as mass matrix, mass
   // inverse matrix, Coriolis force vector, gravity force vector, and external
@@ -653,7 +657,111 @@ void DynamicsTest::testJacobians(const std::string& _fileName)
 }
 
 //==============================================================================
-void DynamicsTest::testFiniteDifference(const std::string& _fileName)
+void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates(
+    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 -------------------------------------
+#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.3 * DART_PI;
+  double dqUB  =  0.3 * DART_PI;
+  double ddqLB = -0.1 * DART_PI;
+  double ddqUB =  0.1 * DART_PI;
+  Vector3d gravity(0.0, -9.81, 0.0);
+  double timeStep = 1e-3;
+
+  // load skeleton
+  World* world = SkelParser::readWorld(_fileName);
+  assert(world != NULL);
+  world->setGravity(gravity);
+  world->setTimeStep(timeStep);
+
+  //------------------------------ Tests ---------------------------------------
+  for (size_t i = 0; i < world->getNumSkeletons(); ++i)
+  {
+    Skeleton* skeleton = world->getSkeleton(i);
+    assert(skeleton != NULL);
+    int dof = skeleton->getNumDofs();
+
+    for (int j = 0; j < nRandomItr; ++j)
+    {
+      // Generate a random state and ddq
+      VectorXd q0   = VectorXd(dof);
+      VectorXd dq0  = VectorXd(dof);
+      VectorXd ddq0 = VectorXd(dof);
+      for (int k = 0; k < dof; ++k)
+      {
+        q0[k]   = math::random(qLB,   qUB);
+        dq0[k]  = math::random(dqLB,  dqUB);
+        ddq0[k] = math::random(ddqLB, ddqUB);
+      }
+
+      skeleton->setPositions(q0);
+      skeleton->setVelocities(dq0);
+      skeleton->setAccelerations(ddq0);
+
+      skeleton->integratePositions(timeStep);
+      VectorXd q1 = skeleton->getPositions();
+      skeleton->integrateVelocities(timeStep);
+      VectorXd dq1 = skeleton->getVelocities();
+
+      skeleton->integratePositions(timeStep);
+      VectorXd q2 = skeleton->getPositions();
+      skeleton->integrateVelocities(timeStep);
+      VectorXd dq2 = skeleton->getVelocities();
+
+      VectorXd dq0FD = skeleton->getPositionDifferences(q0, q1) / timeStep;
+      VectorXd dq1FD = skeleton->getPositionDifferences(q1, q2) / timeStep;
+      VectorXd ddqFD1 = skeleton->getVelocityDifferences(dq0FD, dq1FD) / timeStep;
+      VectorXd ddqFD2 = skeleton->getVelocityDifferences(dq1, dq2) / timeStep;
+
+      EXPECT_TRUE(equals(dq0, dq0FD));
+      EXPECT_TRUE(equals(dq1, dq1FD));
+      EXPECT_TRUE(equals(ddq0, ddqFD1));
+      EXPECT_TRUE(equals(ddq0, ddqFD2));
+
+      if (!equals(dq0FD, dq0))
+      {
+        std::cout << "dq0  : " << dq0.transpose() << std::endl;
+        std::cout << "dq0FD: " << dq0FD.transpose() << std::endl;
+      }
+      if (!equals(dq1, dq1FD))
+      {
+        std::cout << "dq1  : " << dq1.transpose() << std::endl;
+        std::cout << "dq1FD: " << dq1FD.transpose() << std::endl;
+      }
+      if (!equals(ddq0, ddqFD1))
+      {
+        std::cout << "ddq0  : " << ddq0.transpose() << std::endl;
+        std::cout << "ddqFD1: " << ddqFD1.transpose() << std::endl;
+      }
+      if (!equals(ddq0, ddqFD2))
+      {
+        std::cout << "ddq0  : " << ddq0.transpose() << std::endl;
+        std::cout << "ddqFD2: " << ddqFD2.transpose() << std::endl;
+      }
+    }
+  }
+
+  delete world;
+}
+
+//==============================================================================
+void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration(
+    const std::string& _fileName)
 {
   using namespace std;
   using namespace Eigen;
@@ -703,10 +811,6 @@ void DynamicsTest::testFiniteDifference(const std::string& _fileName)
         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 qNext  =  q +  dq * timeStep;
@@ -1546,7 +1650,8 @@ TEST_F(DynamicsTest, testFiniteDifference)
 #ifndef NDEBUG
     dtdbg << getList()[i] << std::endl;
 #endif
-    testFiniteDifference(getList()[i]);
+    testFiniteDifferenceGeneralizedCoordinates(getList()[i]);
+    testFiniteDifferenceBodyNodeAcceleration(getList()[i]);
   }
 }