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

15
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,18 +392,13 @@ 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)
omgmat = MatrixLog3(R) omgmat = np.array(MatrixLog3(R), dtype=np.float64)
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) \