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

Updated first 10 doc comment blocks for python source file

This commit is contained in:
Bill Hunt 2018-07-06 08:24:08 -05:00
parent 2a3f75e5ad
commit b2849e84c2
1 changed files with 159 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

272
code/Python/modern_robotics.py
View File

@ -3,7 +3,7 @@
Library of functions written to accompany the algorithms described in Library of functions written to accompany the algorithms described in
Modern Robotics: Mechanics, Planning, and Control. Modern Robotics: Mechanics, Planning, and Control.
*************************************************************************** ***************************************************************************
Author: Mikhail Todes, Huan Weng Author: Mikhail Todes, Huan Weng, Bill Hunt
Date: June 2018 Date: June 2018
*************************************************************************** ***************************************************************************
Language: Python Language: Python
@ -24,25 +24,33 @@ import matplotlib.pyplot as plt
''' '''
def NearZero(z): def NearZero(z):
#Takes a scalar. """Determines whether a scalar is small enough to be treated as zero
#Checks if the scalar is small enough to be neglected.
''' Takes a scalar; checks if the scalar is small enough to be neglected.
Example Input:
z = -1e-7 Example Input:
Output: z = -1e-7
True Output:
''' True
:param z: A scalar input to check
:returns: True if z is close to zero, false otherwise
"""
return abs(z) < 1e-6 return abs(z) < 1e-6
def Normalize(V): def Normalize(V):
#Takes a vector. """Normalizes a vector
#Scales it to a unit vector.
''' Accepts a vector; returns the unit vector pointing in the same direction as the input.
Example Input:
V = np.array([1, 2, 3]) Example Input:
Output: V = np.array([1, 2, 3])
[0.2672612419124244, 0.5345224838248488, 0.8017837257372732] Output:
''' np.array([0.2672612419124244, 0.5345224838248488, 0.8017837257372732])
:param z: A vector
:returns: A unit vector pointing in the same direction as z
"""
return V / np.linalg.norm(V) return V / np.linalg.norm(V)
''' '''
@ -50,75 +58,97 @@ Output:
''' '''
def RotInv(R): def RotInv(R):
#Takes a 3x3 rotation matrix. """Inverts a rotation matrix
#Returns the inverse (transpose).
''' Accepts a rotation matrix; returns the inverse of that matrix.
Example Input:
R = np.array([[0, 0, 1], Example Input:
[1, 0, 0], R = np.array([[0, 0, 1],
[0, 1, 0]]) [1, 0, 0],
Output: [0, 1, 0]])
[[0, 1, 0], Output:
[0, 0, 1], [[0, 1, 0],
[1, 0, 0]] [0, 0, 1],
''' [1, 0, 0]]
:param R: A rotation matrix
:returns: The inverse of R
"""
return np.array(R).T return np.array(R).T
def VecToso3(omg): def VecToso3(omg):
#Takes a 3-vector (angular velocity). """Converts a 3-vector to an so3 representation
#Returns the skew symmetric matrix in so3.
''' Takes a 3-vector (angular velocity).
Example Input: Returns the skew symmetric matrix in so3.
omg = np.array([1, 2, 3])
Output: Example Input:
[[ 0, -3, 2], omg = np.array([1, 2, 3])
[ 3, 0, -1], Output:
[-2, 1, 0]] [[ 0, -3, 2],
''' [ 3, 0, -1],
[-2, 1, 0]]
:param omg: A 3-vector
:returns: The skew symmetric representation of omg
"""
return np.array([[0, -omg[2], omg[1]], return np.array([[0, -omg[2], omg[1]],
[omg[2], 0, -omg[0]], [omg[2], 0, -omg[0]],
[-omg[1], omg[0], 0]]) [-omg[1], omg[0], 0]])
def so3ToVec(so3mat): def so3ToVec(so3mat):
#Takes a 3x3 skew-symmetric matrix (an element of so(3)). """Converts an so3 representation to a 3-vector
#Returns the corresponding vector (angular velocity).
''' Takes a 3x3 skew-symmetric matrix (an element of so(3)).
Example Input: Returns the corresponding vector (angular velocity).
so3mat = np.array([[ 0, -3, 2],
[ 3, 0, -1], Example Input:
[-2, 1, 0]]) so3mat = np.array([[ 0, -3, 2],
Output: [ 3, 0, -1],
[1, 2, 3] [-2, 1, 0]])
''' Output:
[1, 2, 3]
:param so3mat: A 3x3 skew-symmetric matrix
:returns: The 3-vector corresponding to so3mat
"""
return np.array([so3mat[2][1], so3mat[0][2], so3mat[1][0]]) return np.array([so3mat[2][1], so3mat[0][2], so3mat[1][0]])
def AxisAng3(expc3): def AxisAng3(expc3):
#Takes A 3-vector of exponential coordinates for rotation. """Converts a 3-vector of exponential coordinates for rotation into axis-angle form
#Returns unit rotation axis omghat and the corresponding rotation angle
#theta. Takes A 3-vector of exponential coordinates for rotation.
''' Returns unit rotation axis omghat and the corresponding rotation angle theta.
Example Input:
expc3 = np.array([1, 2, 3]) Example Input:
Output: expc3 = np.array([1, 2, 3])
(array([0.2672612419124244, 0.5345224838248488, 0.8017837257372732]), Output:
3.7416573867739413) (array([0.2672612419124244, 0.5345224838248488, 0.8017837257372732]), 3.7416573867739413)
'''
:param expc3: A 3-vector of exponential coordinates for rotation
:returns: A unit rotation axis and a rotation angle
"""
return (Normalize(expc3), np.linalg.norm(expc3)) return (Normalize(expc3), np.linalg.norm(expc3))
def MatrixExp3(so3mat): def MatrixExp3(so3mat):
#Takes a so(3) representation of exponential coordinates. """Computes the matrix exponential of a matrix in so3
#Returns R in SO(3) that is achieved by rotating about omghat by theta from
#an initial orientation R = I. Takes a so(3) representation of exponential coordinates.
''' Returns R in SO(3) that is achieved by rotating about omghat by theta from
Example Input: an initial orientation R = I.
so3mat = np.array([[ 0, -3, 2],
[ 3, 0, -1], Example Input:
[-2, 1, 0]]) so3mat = np.array([[ 0, -3, 2],
Output: [ 3, 0, -1],
[[-0.69492056, 0.71352099, 0.08929286], [-2, 1, 0]])
[-0.19200697, -0.30378504, 0.93319235], Output:
[ 0.69297817, 0.6313497 , 0.34810748]] [[-0.69492056, 0.71352099, 0.08929286],
''' [-0.19200697, -0.30378504, 0.93319235],
[ 0.69297817, 0.6313497 , 0.34810748]]
:param so3mat: A 3x3 skew-symmetric matrix
:returns: The matrix exponential of so3mat
"""
omgtheta = so3ToVec(so3mat) omgtheta = so3ToVec(so3mat)
if NearZero(np.linalg.norm(omgtheta)): if NearZero(np.linalg.norm(omgtheta)):
return np.eye(3) return np.eye(3)
@ -129,18 +159,23 @@ Output:
+ (1 - np.cos(theta)) * np.dot(omgmat, omgmat) + (1 - np.cos(theta)) * np.dot(omgmat, omgmat)
def MatrixLog3(R): def MatrixLog3(R):
#Takes R (rotation matrix). """Computes the matrix logarithm of a rotation matrix
#Returns the corresponding so(3) representation of exponential coordinates.
''' Takes R (rotation matrix).
Example Input: Returns the corresponding so(3) representation of exponential coordinates.
R = np.array([[0, 0, 1],
[1, 0, 0], Example Input:
[0, 1, 0]]) R = np.array([[0, 0, 1],
Output: [1, 0, 0],
[[ 0, -1.20919958, 1.20919958], [0, 1, 0]])
[ 1.20919958, 0, -1.20919958], Output:
[-1.20919958, 1.20919958, 0]] [[ 0, -1.20919958, 1.20919958],
''' [ 1.20919958, 0, -1.20919958],
[-1.20919958, 1.20919958, 0]]
:param R: A 3x3 rotation matrix
:returns: The matrix logarithm of R
"""
if NearZero(np.linalg.norm(R - np.eye(3))): if NearZero(np.linalg.norm(R - np.eye(3))):
return np.zeros((3, 3)) return np.zeros((3, 3))
elif NearZero(np.trace(R) + 1): elif NearZero(np.trace(R) + 1):
@ -164,38 +199,49 @@ Output:
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):
#Takes rotation matrix R and position p. """Converts a rotation matrix and a position vector into homogeneous tranformation matrix
#Returns corresponding homogeneous transformation matrix T in SE(3).
''' Takes rotation matrix R and position p.
Example Input: Returns corresponding homogeneous transformation matrix T in SE(3).
R = np.array([[1, 0, 0],
[0, 0, -1], Example Input:
[0, 1, 0]]) R = np.array([[1, 0, 0],
p = np.array([1, 2, 5]) [0, 0, -1],
Output: [0, 1, 0]])
[[1, 0, 0, 1], p = np.array([1, 2, 5])
[0, 0, -1, 2], Output:
[0, 1, 0, 5], [[1, 0, 0, 1],
[0, 0, 0, 1]] [0, 0, -1, 2],
''' [0, 1, 0, 5],
[0, 0, 0, 1]]
:param R: A 3x3 rotation matrix
:param p: A 3-vector
:returns: A homogeneous transformation matrix corresponding to the inputs
"""
return np.r_[np.c_[R, p], [[0, 0, 0, 1]]] return np.r_[np.c_[R, p], [[0, 0, 0, 1]]]
def TransToRp(T): def TransToRp(T):
#Takes transformation matrix T in SE(3). """Converts a homogeneous transformation matrix into a rotation matrix and position vector
#Returns R: The corresponding rotation matrix,
# p: The corresponding position vector. Takes transformation matrix T in SE(3).
''' Returns R: The corresponding rotation matrix,
Example Input: p: The corresponding position vector.
T = np.array([[1, 0, 0, 0],
[0, 0, -1, 0], Example Input:
[0, 1, 0, 3], T = np.array([[1, 0, 0, 0],
[0, 0, 0, 1]]) [0, 0, -1, 0],
Output: [0, 1, 0, 3],
(array([[1, 0, 0], [0, 0, 0, 1]])
[0, 0, -1], Output:
[0, 1, 0]]), (array([[1, 0, 0],
array([0, 0, 3])) [0, 0, -1],
''' [0, 1, 0]]),
array([0, 0, 3]))
:param T: A homogeneous transformation matrix
:returns: A rotation matrix and a position vector, corresponding to the input
"""
R = np.array([[T[0][0], T[0][1], T[0][2]], R = np.array([[T[0][0], T[0][1], T[0][2]],
[T[1][0], T[1][1], T[1][2]], [T[1][0], T[1][1], T[1][2]],
[T[2][0], T[2][1], T[2][2]]]) [T[2][0], T[2][1], T[2][2]]])