Update core.py

2018-12-19 13:01:30 -05:00 · 2018-12-19 13:01:30 -05:00 · 2e6e4ebb22
parent b5525b0f4e
commit 2e6e4ebb22
1 changed files with 4 additions and 1 deletions
--- a/packages/Python/modern_robotics/core.py
+++ b/packages/Python/modern_robotics/core.py
@ -178,6 +178,9 @@ def MatrixLog3(R):
        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)

 def RpToTrans(R, p):