From fcaac5c5c78b4c3ada8ee6060786064a2f170b2d Mon Sep 17 00:00:00 2001
From: Olivier Martin <omartin2010@users.noreply.github.com>
Date: Fri, 21 Dec 2018 15:16:49 -0500
Subject: [PATCH] Update core.py

I think this should about be right.
---
 packages/Python/modern_robotics/core.py | 42 ++++++++++---------------
 1 file changed, 17 insertions(+), 25 deletions(-)

diff --git a/packages/Python/modern_robotics/core.py b/packages/Python/modern_robotics/core.py
index 8d505ff..88645f8 100644
--- a/packages/Python/modern_robotics/core.py
+++ b/packages/Python/modern_robotics/core.py
@@ -173,15 +173,12 @@ def MatrixLog3(R):
         return VecToso3(np.pi * omg)
     else:
         acosinput = (np.trace(R) - 1) / 2.0
-        if acosinput > 1:
-            acosinput = 1
+        if acosinput >= 1:
+            acosinput = 0.99999
         elif acosinput < -1:
             acosinput = -1
         theta = np.arccos(acosinput)
-        if theta == 0:
-            return (R - np.array(R).T)/2
-        else:
-            return theta / 2.0 / np.sin(theta) * (R - np.array(R).T)
+        return theta / 2.0 / np.sin(theta) * (R - np.array(R).T)
 
 def RpToTrans(R, p):
     """Converts a rotation matrix and a position vector into homogeneous
@@ -393,25 +390,20 @@ def MatrixLog6(T):
                   [0,          0,           0,           0]])
     """
     R, p = TransToRp(T)
-    if NearZero(np.linalg.norm(R - np.eye(3))):
-        return np.r_[np.c_[np.zeros((3, 3)),
-                           [T[0][3], T[1][3], T[2][3]]],
-                     [[0, 0, 0, 0]]]
-    else:
-        acosinput = (np.trace(R) - 1) / 2.0
-        if acosinput > 1:
-            acosinput = 1
-        elif acosinput < -1:
-            acosinput = -1
-        theta = np.arccos(acosinput)
-        omgmat = np.array(MatrixLog3(R), dtype=np.float64)
-        return np.r_[np.c_[omgmat,
-                           np.dot(np.eye(3) - omgmat / 2.0 \
-                           + (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \
-                             * np.dot(omgmat,omgmat) / theta,[T[0][3],
-                                                              T[1][3],
-                                                              T[2][3]])],
-                     [[0, 0, 0, 0]]]
+    acosinput = (np.trace(R) - 1) / 2.0
+    if acosinput >= 1:
+        acosinput = 0.99999
+    elif acosinput < -1:
+        acosinput = -1
+    theta = np.arccos(acosinput)
+    omgmat = np.array(MatrixLog3(R), dtype=np.float64)
+    return np.r_[np.c_[omgmat,
+                       np.dot(np.eye(3) - omgmat / 2.0 \
+                       + (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \
+                         * np.dot(omgmat,omgmat) / theta,[T[0][3],
+                                                          T[1][3],
+                                                          T[2][3]])],
+                 [[0, 0, 0, 0]]]
 
 
 def ProjectToSO3(mat):