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

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

@ -173,15 +173,12 @@ 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 theta / 2.0 / np.sin(theta) * (R - np.array(R).T)
return (R - np.array(R).T)/2
else:
return theta / 2.0 / np.sin(theta) * (R - np.array(R).T)
def RpToTrans(R, p): def RpToTrans(R, p):
"""Converts a rotation matrix and a position vector into homogeneous """Converts a rotation matrix and a position vector into homogeneous
@ -393,25 +390,20 @@ 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))): acosinput = (np.trace(R) - 1) / 2.0
return np.r_[np.c_[np.zeros((3, 3)), if acosinput >= 1:
[T[0][3], T[1][3], T[2][3]]], acosinput = 0.99999
[[0, 0, 0, 0]]] elif acosinput < -1:
else: acosinput = -1
acosinput = (np.trace(R) - 1) / 2.0 theta = np.arccos(acosinput)
if acosinput > 1: omgmat = np.array(MatrixLog3(R), dtype=np.float64)
acosinput = 1 return np.r_[np.c_[omgmat,
elif acosinput < -1: np.dot(np.eye(3) - omgmat / 2.0 \
acosinput = -1 + (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \
theta = np.arccos(acosinput) * np.dot(omgmat,omgmat) / theta,[T[0][3],
omgmat = np.array(MatrixLog3(R), dtype=np.float64) T[1][3],
return np.r_[np.c_[omgmat, T[2][3]])],
np.dot(np.eye(3) - omgmat / 2.0 \ [[0, 0, 0, 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): def ProjectToSO3(mat):