Differences of generalized coordinates

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());
 }
 
 //==============================================================================

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);
 

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());
 }
 
 //==============================================================================

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();

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());
 }
 
 //==============================================================================

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);