From 2e6e4ebb227b03776db189f1a12591b6c1d5466b Mon Sep 17 00:00:00 2001 From: Olivier Martin Date: Wed, 19 Dec 2018 13:01:30 -0500 Subject: [PATCH] Update core.py --- packages/Python/modern_robotics/core.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/packages/Python/modern_robotics/core.py b/packages/Python/modern_robotics/core.py index a123d49..8d505ff 100644 --- a/packages/Python/modern_robotics/core.py +++ b/packages/Python/modern_robotics/core.py @@ -178,7 +178,10 @@ def MatrixLog3(R): elif acosinput < -1: acosinput = -1 theta = np.arccos(acosinput) - return theta / 2.0 / np.sin(theta) * (R - np.array(R).T) + 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): """Converts a rotation matrix and a position vector into homogeneous