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

Merge pull request #12 from omartin2010/patch-2 

Update MatrixLog6 and MatrixLog3
This commit is contained in:
HuanWeng 2018-12-23 23:48:37 -06:00 committed by GitHub
parent 64bba07040 fcaac5c5c7
commit 20aeea8316
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 16 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -173,8 +173,8 @@ 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)
@ -392,25 +392,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 = MatrixLog3(R) 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):