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

Update core.py

This commit is contained in:
Olivier Martin 2018-12-19 13:01:30 -05:00 committed by GitHub
parent b5525b0f4e
commit 2e6e4ebb22
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -178,6 +178,9 @@ def MatrixLog3(R):
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):