Updated first 10 doc comment blocks for python source file

code/Python/modern_robotics.py

@@ -3,7 +3,7 @@
 Library of functions written to accompany the algorithms described in
 Modern Robotics: Mechanics, Planning, and Control.
 ***************************************************************************
-Author: Mikhail Todes, Huan Weng  
+Author: Mikhail Todes, Huan Weng, Bill Hunt
 Date: June 2018
 ***************************************************************************
 Language: Python
@@ -24,25 +24,33 @@ import matplotlib.pyplot as plt
 '''
 
 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
    
 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)
 
 '''
@@ -50,75 +58,97 @@ Output:
 '''
 
 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: 
+        R = np.array([[0, 0, 1],
                       [1, 0, 0],
                       [0, 1, 0]])
-Output:
+    Output:
-[[0, 1, 0], 
+        [[0, 1, 0], 
          [0, 0, 1],
          [1, 0, 0]]
-    '''
+    
+    :param R: A rotation matrix
+    :returns: The inverse of R
+    """
     return np.array(R).T
 
 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])
+    Output:
+        [[ 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]], 
 	             [omg[2],       0, -omg[0]], 
 	             [-omg[1], omg[0],       0]])
 
 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],
+    
+    Example Input: 
+        so3mat = np.array([[ 0, -3,  2],
                            [ 3,  0, -1],
                            [-2,  1,  0]])
-Output:
+    Output:
-[1, 2, 3]
+        [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]])
 
 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))
 
 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],
+    
+    Example Input:
+        so3mat = np.array([[ 0, -3,  2],
                            [ 3,  0, -1],
                            [-2,  1,  0]])
-Output:
+    Output:
-[[-0.69492056,  0.71352099,  0.08929286],
+        [[-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)
     if NearZero(np.linalg.norm(omgtheta)):
         return np.eye(3)
@@ -129,18 +159,23 @@ Output:
                + (1 - np.cos(theta)) * np.dot(omgmat, omgmat)
 
 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],
+    
+    Example Input: 
+        R = np.array([[0, 0, 1],
                       [1, 0, 0],
                       [0, 1, 0]])
-Output:
+    Output:
-[[          0, -1.20919958,  1.20919958],
+        [[          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))):
         return np.zeros((3, 3))
     elif NearZero(np.trace(R) + 1):
@@ -164,38 +199,49 @@ Output:
         return theta / 2.0 / np.sin(theta) * (R - np.array(R).T)
 
 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], 
+        
+    Example Input: 
+        R = np.array([[1, 0,  0], 
                       [0, 0, -1], 
                       [0, 1,  0]])
-p = np.array([1, 2, 5])
+        p = np.array([1, 2, 5])
-Output:
+    Output:
-[[1, 0,  0, 1],
+        [[1, 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]]]    
 
 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],
+            
+    Example Input: 
+        T = np.array([[1, 0,  0, 0],
                       [0, 0, -1, 0],
                       [0, 1,  0, 3],
                       [0, 0,  0, 1]])
-Output:
+    Output:
-(array([[1, 0,  0], 
+        (array([[1, 0,  0], 
                 [0, 0, -1], 
                 [0, 1,  0]]),  
-array([0, 0, 3]))
+         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]],
                   [T[1][0], T[1][1], T[1][2]],
                   [T[2][0], T[2][1], T[2][2]]])