From b5525b0f4ed5217075a2056d5c2741267d664116 Mon Sep 17 00:00:00 2001 From: Olivier Martin Date: Fri, 7 Dec 2018 23:12:35 -0500 Subject: [PATCH 1/3] upadte Matrix6Log somehow, in certain cases, I was unable to generate a trajectory without getting an error that the operator / can't work between list (omgmat) and float... so changing that fixed it for me. --- packages/Python/modern_robotics/core.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packages/Python/modern_robotics/core.py b/packages/Python/modern_robotics/core.py index 925751c..a123d49 100644 --- a/packages/Python/modern_robotics/core.py +++ b/packages/Python/modern_robotics/core.py @@ -401,7 +401,7 @@ def MatrixLog6(T): elif acosinput < -1: acosinput = -1 theta = np.arccos(acosinput) - omgmat = MatrixLog3(R) + 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) \ From 2e6e4ebb227b03776db189f1a12591b6c1d5466b Mon Sep 17 00:00:00 2001 From: Olivier Martin Date: Wed, 19 Dec 2018 13:01:30 -0500 Subject: [PATCH 2/3] 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 From fcaac5c5c78b4c3ada8ee6060786064a2f170b2d Mon Sep 17 00:00:00 2001 From: Olivier Martin Date: Fri, 21 Dec 2018 15:16:49 -0500 Subject: [PATCH 3/3] 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):