Robotics
/
Modern_Robotics
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Modify the bug fix for MatrixLog3 and MatrixLog6 for Python

This commit is contained in:
HuanWeng 2018-12-24 00:21:42 -06:00
parent 20aeea8316
commit 6de81fbb32
1 changed files with 17 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
packages/Python/modern_robotics/core.py
View File

@ -158,9 +158,10 @@ def MatrixLog3(R):
[ 1.20919958, 0, -1.20919958], [ 1.20919958, 0, -1.20919958],
[-1.20919958, 1.20919958, 0]]) [-1.20919958, 1.20919958, 0]])
""" """
if NearZero(np.linalg.norm(R - np.eye(3))): acosinput = (np.trace(R) - 1) / 2.0
if acosinput >= 1:
return np.zeros((3, 3)) return np.zeros((3, 3))
elif NearZero(np.trace(R) + 1): elif acosinput <= -1:
if not NearZero(1 + R[2][2]): if not NearZero(1 + R[2][2]):
omg = (1.0 / np.sqrt(2 * (1 + R[2][2]))) \ omg = (1.0 / np.sqrt(2 * (1 + R[2][2]))) \
* np.array([R[0][2], R[1][2], 1 + R[2][2]]) * np.array([R[0][2], R[1][2], 1 + R[2][2]])
@ -172,11 +173,6 @@ def MatrixLog3(R):
* np.array([1 + R[0][0], R[1][0], R[2][0]]) * np.array([1 + R[0][0], R[1][0], R[2][0]])
return VecToso3(np.pi * omg) return VecToso3(np.pi * omg)
else: else:
acosinput = (np.trace(R) - 1) / 2.0
if acosinput >= 1:
acosinput = 0.99999
elif acosinput < -1:
acosinput = -1
theta = np.arccos(acosinput) theta = np.arccos(acosinput)
return theta / 2.0 / np.sin(theta) * (R - np.array(R).T) return theta / 2.0 / np.sin(theta) * (R - np.array(R).T)
@ -392,13 +388,13 @@ def MatrixLog6(T):
[0, 0, 0, 0]]) [0, 0, 0, 0]])
""" """
R, p = TransToRp(T) R, p = TransToRp(T)
acosinput = (np.trace(R) - 1) / 2.0 omgmat = MatrixLog3(R)
if acosinput >= 1: if np.array_equal(omgmat, np.zeros((3, 3))):
acosinput = 0.99999 return np.r_[np.c_[np.zeros((3, 3)),
elif acosinput < -1: [T[0][3], T[1][3], T[2][3]]],
acosinput = -1 [[0, 0, 0, 0]]]
theta = np.arccos(acosinput) else:
omgmat = np.array(MatrixLog3(R), dtype=np.float64) theta = np.arccos((np.trace(R) - 1) / 2.0)
return np.r_[np.c_[omgmat, return np.r_[np.c_[omgmat,
np.dot(np.eye(3) - omgmat / 2.0 \ np.dot(np.eye(3) - omgmat / 2.0 \
+ (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \ + (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \