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

Update core.py 

I think this should about be right.
This commit is contained in:
Olivier Martin 2018-12-21 15:16:49 -05:00 committed by GitHub
parent 2e6e4ebb22
commit fcaac5c5c7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 17 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -173,14 +173,11 @@ def MatrixLog3(R):
return VecToso3(np.pi * omg) return VecToso3(np.pi * omg)
else: else:
acosinput = (np.trace(R) - 1) / 2.0 acosinput = (np.trace(R) - 1) / 2.0
if acosinput > 1: if acosinput >= 1:
acosinput = 1 acosinput = 0.99999
elif acosinput < -1: elif acosinput < -1:
acosinput = -1 acosinput = -1
theta = np.arccos(acosinput) 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): def RpToTrans(R, p):
@ -393,14 +390,9 @@ def MatrixLog6(T):
[0, 0, 0, 0]]) [0, 0, 0, 0]])
""" """
R, p = TransToRp(T) 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 acosinput = (np.trace(R) - 1) / 2.0
if acosinput > 1: if acosinput >= 1:
acosinput = 1 acosinput = 0.99999
elif acosinput < -1: elif acosinput < -1:
acosinput = -1 acosinput = -1
theta = np.arccos(acosinput) theta = np.arccos(acosinput)