Merge pull request #407 from dartsim/angleaxis

Using Eigen::AngleAxisd for computing logMaps
dartsim · Jun 16, 2015 · b163b74 · b163b74
2 parents 7f58aca + 3cc0a30
commit b163b74
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 35 deletions.
diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp
@@ -531,33 +531,36 @@ Eigen::Vector3d logMap(const Eigen::Matrix3d& _R) {
   // v = beta*p + gamma*w + 1 / 2*cross(p, w)
   //    , beta = t*(1 + cos(t)) / (2*sin(t)), gamma = <w, p>*(1 - beta) / t^2
   //--------------------------------------------------------------------------
-  double theta =
-      std::acos(
-        std::max(
-          std::min(0.5 * (_R(0, 0) + _R(1, 1) + _R(2, 2) - 1.0), 1.0), -1.0));
-
-  if (theta > DART_PI - DART_EPSILON) {
-    double delta = 0.5 + 0.125*(DART_PI - theta)*(DART_PI - theta);
-
-    return Eigen::Vector3d(
-          _R(2, 1) > _R(1, 2) ? theta*sqrt(1.0 + (_R(0, 0) - 1.0)*delta) :
-                             -theta*sqrt(1.0 + (_R(0, 0) - 1.0)*delta),
-          _R(0, 2) > _R(2, 0) ? theta*sqrt(1.0 + (_R(1, 1) - 1.0)*delta) :
-                             -theta*sqrt(1.0 + (_R(1, 1) - 1.0)*delta),
-          _R(1, 0) > _R(0, 1) ? theta*sqrt(1.0 + (_R(2, 2) - 1.0)*delta) :
-                             -theta*sqrt(1.0 + (_R(2, 2) - 1.0)*delta));
-  } else {
-    double alpha = 0.0;
-
-    if (theta > DART_EPSILON)
-      alpha = 0.5*theta / sin(theta);
-    else
-      alpha = 0.5 + DART_1_12*theta*theta;
-
-    return Eigen::Vector3d(alpha*(_R(2, 1) - _R(1, 2)),
-                           alpha*(_R(0, 2) - _R(2, 0)),
-                           alpha*(_R(1, 0) - _R(0, 1)));
-  }
+//  double theta =
+//      std::acos(
+//        std::max(
+//          std::min(0.5 * (_R(0, 0) + _R(1, 1) + _R(2, 2) - 1.0), 1.0), -1.0));
+
+//  if (theta > DART_PI - DART_EPSILON) {
+//    double delta = 0.5 + 0.125*(DART_PI - theta)*(DART_PI - theta);
+
+//    return Eigen::Vector3d(
+//          _R(2, 1) > _R(1, 2) ? theta*sqrt(1.0 + (_R(0, 0) - 1.0)*delta) :
+//                             -theta*sqrt(1.0 + (_R(0, 0) - 1.0)*delta),
+//          _R(0, 2) > _R(2, 0) ? theta*sqrt(1.0 + (_R(1, 1) - 1.0)*delta) :
+//                             -theta*sqrt(1.0 + (_R(1, 1) - 1.0)*delta),
+//          _R(1, 0) > _R(0, 1) ? theta*sqrt(1.0 + (_R(2, 2) - 1.0)*delta) :
+//                             -theta*sqrt(1.0 + (_R(2, 2) - 1.0)*delta));
+//  } else {
+//    double alpha = 0.0;
+
+//    if (theta > DART_EPSILON)
+//      alpha = 0.5*theta / sin(theta);
+//    else
+//      alpha = 0.5 + DART_1_12*theta*theta;
+
+//    return Eigen::Vector3d(alpha*(_R(2, 1) - _R(1, 2)),
+//                           alpha*(_R(0, 2) - _R(2, 0)),
+//                           alpha*(_R(1, 0) - _R(0, 1)));
+//  }
+
+  Eigen::AngleAxisd aa(_R);
+  return aa.angle()*aa.axis();
 }
 
 Eigen::Vector6d logMap(const Eigen::Isometry3d& _T) {

diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp
@@ -700,6 +700,7 @@ void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates(
   double ddqUB =  0.1 * DART_PI;
   Vector3d gravity(0.0, -9.81, 0.0);
   double timeStep = 1e-3;
+  double TOLERANCE = 5e-4;
 
   // load skeleton
   WorldPtr world = SkelParser::readWorld(_fileName);
@@ -746,27 +747,27 @@ void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates(
       VectorXd ddqFD1 = skeleton->getVelocityDifferences(dq1FD, dq0FD) / timeStep;
       VectorXd ddqFD2 = skeleton->getVelocityDifferences(dq2, dq1) / timeStep;
 
-      EXPECT_TRUE(equals(dq0, dq0FD));
-      EXPECT_TRUE(equals(dq1, dq1FD));
-      EXPECT_TRUE(equals(ddq0, ddqFD1));
-      EXPECT_TRUE(equals(ddq0, ddqFD2));
+      EXPECT_TRUE(equals(dq0, dq0FD, TOLERANCE));
+      EXPECT_TRUE(equals(dq1, dq1FD, TOLERANCE));
+      EXPECT_TRUE(equals(ddq0, ddqFD1, TOLERANCE));
+      EXPECT_TRUE(equals(ddq0, ddqFD2, TOLERANCE));
 
-      if (!equals(dq0FD, dq0))
+      if (!equals(dq0FD, dq0, TOLERANCE))
       {
         std::cout << "dq0  : " << dq0.transpose() << std::endl;
         std::cout << "dq0FD: " << dq0FD.transpose() << std::endl;
       }
-      if (!equals(dq1, dq1FD))
+      if (!equals(dq1, dq1FD, TOLERANCE))
       {
         std::cout << "dq1  : " << dq1.transpose() << std::endl;
         std::cout << "dq1FD: " << dq1FD.transpose() << std::endl;
       }
-      if (!equals(ddq0, ddqFD1))
+      if (!equals(ddq0, ddqFD1, TOLERANCE))
       {
         std::cout << "ddq0  : " << ddq0.transpose() << std::endl;
         std::cout << "ddqFD1: " << ddqFD1.transpose() << std::endl;
       }
-      if (!equals(ddq0, ddqFD2))
+      if (!equals(ddq0, ddqFD2, TOLERANCE))
       {
         std::cout << "ddq0  : " << ddq0.transpose() << std::endl;
         std::cout << "ddqFD2: " << ddqFD2.transpose() << std::endl;