Compute actual Jacobian of BallJoint instead of returning identity

ci/manylinux/tests/run_tests.sh

@@ -3,6 +3,7 @@ set -e
 
 VERSION=$(cat ../../../VERSION.txt)
 GIT_HASH=$(git log --pretty=format:'%H' -n 1)
+echo "Testing commit: $GIT_HASH"
 docker build . --build-arg VERSION=${VERSION} --build-arg GIT_HASH=${GIT_HASH}
 IMAGE_ID=$(docker images | awk '{print $3}' | awk 'NR==2')
 echo "Build image ID $IMAGE_ID"

dart/dynamics/BallJoint.cpp

@@ -95,8 +95,7 @@ Eigen::Matrix3d BallJoint::convertToRotation(const Eigen::Vector3d& _positions)
 
 //==============================================================================
 BallJoint::BallJoint(const Properties& properties)
-  : Base(properties),
-    mR(Eigen::Isometry3d::Identity())
+  : Base(properties)
 {
   mJacobianDeriv = Eigen::Matrix<double, 6, 3>::Zero();
 
@@ -114,9 +113,18 @@ Joint* BallJoint::clone() const
 
 //==============================================================================
 Eigen::Matrix<double, 6, 3> BallJoint::getRelativeJacobianStatic(
-    const Eigen::Vector3d& /*positions*/) const
+    const Eigen::Vector3d& positions) const
 {
-  return mJacobian;
+  Eigen::Matrix<double, 6, 3> J;
+
+  const Eigen::Vector3d& q = positions;
+  const Eigen::Isometry3d& T = Joint::mAspectProperties.mT_ChildBodyToJoint;
+
+  J.topRows(3).noalias() = T.rotation() * math::so3RightJacobian(q);
+  J.bottomRows(3).noalias()
+      = math::makeSkewSymmetric(T.translation()) * J.topRows(3);
+
+  return J;
 }
 
 //==============================================================================
@@ -125,15 +133,20 @@ Eigen::Vector3d BallJoint::getPositionDifferencesStatic(
 {
   const Eigen::Matrix3d R1 = convertToRotation(_q1);
   const Eigen::Matrix3d R2 = convertToRotation(_q2);
+  const Eigen::Matrix3d S = math::so3RightJacobian(_q1);
 
-  return convertToPositions(R1.transpose() * R2);
+  return S.inverse() * convertToPositions(R1.transpose() * R2);
 }
 
 //==============================================================================
-void BallJoint::integratePositions(double _dt)
+void BallJoint::integratePositions(double dt)
 {
-  Eigen::Matrix3d Rnext
-      = getR().linear() * convertToRotation(getVelocitiesStatic() * _dt);
+  const auto& q = getPositionsStatic();
+  const auto& dq = getVelocitiesStatic();
+
+  const Eigen::Matrix3d S = math::so3RightJacobian(q);
+  const Eigen::Matrix3d Rnext
+      = convertToRotation(q) * convertToRotation(S * dq * dt);
 
   setPositionsStatic(convertToPositions(Rnext));
 }
@@ -212,28 +225,33 @@ void BallJoint::updateDegreeOfFreedomNames()
 //==============================================================================
 void BallJoint::updateRelativeTransform() const
 {
-  mR.linear() = convertToRotation(getPositionsStatic());
+  Eigen::Isometry3d R = Eigen::Isometry3d::Identity();
+  R.linear() = convertToRotation(getPositionsStatic());
 
-  mT = Joint::mAspectProperties.mT_ParentBodyToJoint * mR
+  mT = Joint::mAspectProperties.mT_ParentBodyToJoint * R
       * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse();
 
   assert(math::verifyTransform(mT));
 }
 
 //==============================================================================
-void BallJoint::updateRelativeJacobian(bool _mandatory) const
+void BallJoint::updateRelativeJacobian(bool /*_mandatory*/) const
 {
-  if (_mandatory)
-  {
-    mJacobian = math::getAdTMatrix(
-          Joint::mAspectProperties.mT_ChildBodyToJoint).leftCols<3>();
-  }
+  mJacobian = getRelativeJacobianStatic(getPositionsStatic());
 }
 
 //==============================================================================
 void BallJoint::updateRelativeJacobianTimeDeriv() const
 {
-  assert(Eigen::Matrix6d::Zero().leftCols<3>() == mJacobianDeriv);
+  const auto& q = getPositionsStatic();
+  const auto& dq = getVelocitiesStatic();
+  const Eigen::Isometry3d& T = Joint::mAspectProperties.mT_ChildBodyToJoint;
+
+  const Eigen::Matrix3d dJ = math::so3RightJacobianTimeDeriv(q, dq);
+
+  mJacobianDeriv.topRows(3).noalias() = T.rotation() * dJ;
+  mJacobianDeriv.bottomRows(3).noalias()
+      = math::makeSkewSymmetric(T.translation()) * mJacobianDeriv.topRows(3);
 }
 
 //==============================================================================
@@ -327,18 +345,5 @@ Eigen::Vector6d BallJoint::getScrewAxisGradientForForce(
     parentTransform * Joint::mAspectProperties.mT_ParentBodyToJoint, grad);
 }
 
-//==============================================================================
-const Eigen::Isometry3d& BallJoint::getR() const
-{
-  if(mNeedTransformUpdate)
-  {
-    updateRelativeTransform();
-    mNeedTransformUpdate = false;
-  }
-
-  return mR;
-}
-
 }  // namespace dynamics
 }  // namespace dart
-

dart/dynamics/BallJoint.hpp

@@ -167,16 +167,6 @@ class BallJoint : public GenericJoint<math::SO3Space>
   // Documentation inherited
   void updateRelativeJacobianTimeDeriv() const override;
 
-protected:
-
-  /// Access mR, which is an auto-updating variable
-  const Eigen::Isometry3d& getR() const;
-
-  /// Rotation matrix dependent on the generalized coordinates
-  ///
-  /// Do not use directly! Use getR() to access this
-  mutable Eigen::Isometry3d mR;
-
 public:
   // To get byte-aligned Eigen vectors
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

dart/math/Geometry.cpp

@@ -1605,6 +1605,71 @@ Eigen::Matrix3d eulerZYZToMatrix(const Eigen::Vector3d& _angle)
   return ret;
 }
 
+//==============================================================================
+Eigen::Matrix3d so3LeftJacobian(const Eigen::Vector3d& w)
+{
+  return expMapJac(w);
+}
+
+//==============================================================================
+Eigen::Matrix3d so3RightJacobian(const Eigen::Vector3d& w)
+{
+  const double theta = w.norm();
+
+  Eigen::Matrix3d J = Eigen::Matrix3d::Zero();
+  const Eigen::Matrix3d qss = math::makeSkewSymmetric(w);
+  const Eigen::Matrix3d qss2 = qss * qss;
+
+  if (theta < EPSILON_EXPMAP_THETA)
+    J = Eigen::Matrix3d::Identity() - 0.5 * qss + (1.0 / 6.0) * qss2;
+  else
+    J = Eigen::Matrix3d::Identity() - ((1 - cos(theta)) / (theta * theta)) * qss
+        + ((theta - sin(theta)) / (theta * theta * theta)) * qss2;
+
+  return J;
+}
+
+//==============================================================================
+Eigen::Matrix3d so3LeftJacobianTimeDeriv(
+    const Eigen::Vector3d& q, const Eigen::Vector3d& dq)
+{
+  return expMapJacDot(q, dq);
+}
+
+//==============================================================================
+Eigen::Matrix3d so3RightJacobianTimeDeriv(
+    const Eigen::Vector3d& q, const Eigen::Vector3d& dq)
+{
+  const double theta = q.norm();
+
+  Eigen::Matrix3d Jdot = Eigen::Matrix3d::Zero();
+  const Eigen::Matrix3d qss = math::makeSkewSymmetric(q);
+  const Eigen::Matrix3d qss2 = qss * qss;
+  const Eigen::Matrix3d qdss = math::makeSkewSymmetric(dq);
+  const double ttdot = q.dot(dq); // theta*thetaDot
+  const double st = sin(theta);
+  const double ct = cos(theta);
+  const double t2 = theta * theta;
+  const double t3 = t2 * theta;
+  const double t4 = t3 * theta;
+  const double t5 = t4 * theta;
+
+  if (theta < EPSILON_EXPMAP_THETA)
+  {
+    Jdot = -0.5 * qdss + (1.0 / 6.0) * (qss * qdss + qdss * qss);
+    Jdot += (1.0 / 12) * ttdot * qss + (-1.0 / 60) * ttdot * qss2;
+  }
+  else
+  {
+    Jdot = -((1 - ct) / t2) * qdss
+           + ((theta - st) / t3) * (qss * qdss + qdss * qss);
+    Jdot += -((theta * st + 2 * ct - 2) / t4) * ttdot * qss
+            + ((3 * st - theta * ct - 2 * theta) / t5) * ttdot * qss2;
+  }
+
+  return Jdot;
+}
+
 // R = Exp(w)
 // p = sin(t) / t*v + (t - sin(t)) / t^3*<w, v>*w + (1 - cos(t)) / t^2*(w X v)
 // , when S = (w, v), t = |w|

dart/math/Geometry.hpp

@@ -159,6 +159,25 @@ Eigen::Vector3d matrixToEulerZYX(const Eigen::Matrix3d& _R);
 // Eigen::Vector3d matrixToEulerZYZ(const Eigen::Matrix3d& R);
 
 //------------------------------------------------------------------------------
+
+/// Returns the Jacobian of an SO(3) element w.r.t. its exponential
+/// coordinates where the Jacobian maps the time derivative of the exponential
+/// coordinates to the angular velocity in the world frame.
+Eigen::Matrix3d so3LeftJacobian(const Eigen::Vector3d& w);
+
+/// Returns the Jacobian of an SO(3) element w.r.t. its exponential
+/// coordinates where the Jacobian maps the time derivative of the exponential
+/// coordinates to the angular velocity in the body frame.
+Eigen::Matrix3d so3RightJacobian(const Eigen::Vector3d& w);
+
+/// Returns the time derivative of the left Jacobian of SO(3).
+Eigen::Matrix3d so3LeftJacobianTimeDeriv(
+    const Eigen::Vector3d& q, const Eigen::Vector3d& dq);
+
+/// Returns the time derivative of the right Jacobian of SO(3).
+Eigen::Matrix3d so3RightJacobianTimeDeriv(
+    const Eigen::Vector3d& q, const Eigen::Vector3d& dq);
+
 /// \brief Exponential mapping
 Eigen::Isometry3d expMap(const Eigen::Vector6d& _S);
 

unittests/comprehensive/test_Joints.cpp

@@ -326,14 +326,10 @@ TEST_F(JOINTS, TRANSLATIONAL_JOINT_2D)
 #endif
 
 // 3-dof joint
-//TEST_F(JOINTS, BALL_JOINT)
-//{
-//  kinematicsTest<BallJoint>();
-//}
-// TODO(JS): Disabled the test compares analytical Jacobian and numerical
-// Jacobian since the meaning of BallJoint Jacobian is changed per
-// we now use angular velocity and angular accertions as BallJoint's generalized
-// velocities and accelerations, repectively.
+TEST_F(JOINTS, BALL_JOINT)
+{
+  kinematicsTest<BallJoint>();
+}
 
 // 3-dof joint
 #ifdef ALL_TESTS