Merge branch 'master' of github.com:florisvb/PyNumDiff into hack-the-optimizer-more

Author: pavelkomarov

pynumdiff/utils/_pi_cruise_control.py

@@ -1,7 +1,4 @@
-"""
-Simulation and Control of a cruise ?
-"""
-
+"""Simulation and Control of cruise control, in all its nonlinear glory"""
 import numpy as np
 from pynumdiff.utils import utility
 
@@ -21,18 +18,12 @@
 
 
 def triangle(iterations, dt):
-    """
-    Create sawtooth pattern of hills for the car to drive over.
-
-    :param iterations: number of time points in time series
-    :type iterations: int
+    """Create sawtooth pattern of hills for the car to drive over.
 
-    :param dt: time step in seconds
-    :type dt: float
-
-    :return: time series of hills (angle of road as a function of time)
-    :rtype: np.matrix
+    :param int iterations: number of time points in time series
+    :param float dt: time step in seconds
 
+    :return: (np.array) -- time series of hills (angle of road as a function of time)
     """
     t = np.arange(0, iterations*dt, dt)
     continuous_x = np.sin(0.02*t*np.sqrt(t))
@@ -59,34 +50,25 @@ def triangle(iterations, dt):
     reversal_vals = np.array(reversal_vals)[idx]
     reversal_ts = t[reversal_idxs]
 
-    x = np.interp(t, reversal_ts, reversal_vals)
-    x = np.matrix(x)
-
-    return x
+    return np.interp(t, reversal_ts, reversal_vals)
 
 
 def effective_wheel_radius(v=20):
-    """
-    Allow effective wheel radius to be a function of velocity.
+    """Allow effective wheel radius to be a function of velocity.
 
-    :param v: ignored for now
-    :type v:
+    :param float v: velocity
 
-    :return: effective wheel radius, constant for now
-    :rtype: float
+    :return: (float) -- effective wheel radius, constant for now
     """
     return v
 
 
 def torque(omega):
-    """
-    Convert throttle input to Torque. See Astrom and Murray 2008 Chapter 3.
+    """Convert throttle input to Torque. See Astrom and Murray 2008 Chapter 3.
 
-    :param omega: throttle
-    :type omega: float
+    :param float omega: throttle
 
-    :return: motor torque
-    :rtype: float
+    :return: **torque** (float) -- motor torque
     """
     omega_m = parameters['omega_m']
     t_m = parameters['T_m']
@@ -96,25 +78,17 @@ def torque(omega):
 
 # pylint: disable-msg=too-many-locals
 def step_forward(state_vals, disturbances, desired_v, dt):
-    """
-    One-step Euler integrator that takes the current state, disturbances, and desired velocity and calculates the subsequent state.
-
-    :param state_vals: current state [position, velocity, road_angle]
-    :type state_vals: np.matrix
-
-    :param disturbances: current hill angle
-    :type disturbances: np.matrix
-
-    :param desired_velocity: current desired velocity
-    :type desired_velocity: np.matrix
+    """One-step Euler integrator that takes the current state, disturbances, and
+    desired velocity and calculates the subsequent state.
 
-    :param dt: time step (seconds)
-    :type dt: float
+    :param np.array state_vals: current state [position, velocity, road_angle]
+    :param np.array disturbances: current hill angle
+    :param np.array desired_v: current desired velocity
+    :param float dt: time step (seconds)
 
-    :return: a tuple consisting of:
-            - new_state: new state
-            - u: control inputs
-    :rtype: tuple -> (np.matrix, np.matrix)
+    :return: tuple[np.array, np.array] of\n
+            - **new_state** -- new state
+            - **u** -- control inputs
     """
     p = state_vals[0, -1]
     v = state_vals[1, -1]
@@ -125,9 +99,9 @@ def step_forward(state_vals, disturbances, desired_v, dt):
     rho = parameters['rho']
     Cd = parameters['Cd']
     A = parameters['A']
-    v_r = desired_v[0, -1]
+    v_r = desired_v[-1]
     alpha_n = effective_wheel_radius(v)
-    z = np.sum(desired_v[0, :] - state_vals[1, :])*dt
+    z = np.sum(desired_v - state_vals[1, :])*dt
     k_p = parameters['k_p']
     k_i = parameters['k_i']
     u = k_p*(v_r-v) + k_i*z
@@ -141,87 +115,63 @@ def step_forward(state_vals, disturbances, desired_v, dt):
     # driving force
     Fd = alpha_n*u*torque(alpha_n*v)
     vdot = 1/m*(Fd - (Fr + Fa + Fg))
-    new_state = np.matrix([[p + dt*v], [v + vdot*dt], [theta]])
-    return new_state, np.matrix(u)
+    new_state = np.array([[p + dt*v], [v + vdot*dt], [theta]])
+    return new_state, u
 
 
 # disturbance
 def hills(iterations, dt, factor):
-    """
-    Wrapper for creating a hill profile for the car to drive over that has an appropriate length and magnitude
-
-    :param iterations: number of time points to simulate
-    :type iterations: int
-
-    :param dt: timestep of simulation in seconds
-    :type dt: float
+    """Wrapper for creating a hill profile for the car to drive over that has an appropriate length and magnitude
 
-    :param factor: determines magnitude of the hills
-    :type factor: int
+    :param int iterations: number of time points to simulate
+    :param float dt: timestep of simulation in seconds
+    :param int factor: determines magnitude of the hills
 
-
-    :return: hills, the output of the triangle function, a [1,M] matrix where M is the number of time points simulated
-    :rtype: np.matrix
+    :return: **hills** (np.array) -- the output of the triangle function, a shape (M,) array where M is the number
+        of time points simulated
     """
     return triangle(iterations, dt)*0.3/factor
 
 
 # desired velocity
 def desired_velocity(n, factor):
-    """
-    Wrapper for defining the desired velocity as a matrix with size [1, M], where M is the number of time points to simulate
-    See function "run" for how this function gets used.
-
-    :param n: number of time points to simulate
-    :type n: int
+    """Wrapper for defining the desired velocity as an array of shape (M,), where M is the number of time points
+    to simulate.
 
-    :param factor: factor that determines the magnitude of the desired velocity
-    :type factor: float
+    :param int n: number of time points to simulate
+    :param float factor: factor that determines the magnitude of the desired velocity
 
-    :return: desired velocity as function of time, a [1,M] matrix, M is the number of time points to simulate
-    :rtype: np.matrix
+    :return: (np.array) -- desired velocity as function of time
     """
-    return np.matrix([2/factor]*n)
+    return np.array([2/factor]*n)
 
 
-def run(timeseries_length=4, dt=0.01):
-    """
-    Simulate proportional integral control of a car attempting to maintain constant velocity while going up and down hills.
-    This function is used for testing differentiation methods.
-
-    See Astrom and Murray 2008 Chapter 3.
-
-    :param timeseries_length: number of seconds to simulate
-    :type timeseries_length: float
+def run(duration=4, dt=0.01):
+    """Simulate proportional integral control of a car attempting to maintain constant velocity while going up
+    and down hills. This function can be used for testing differentiation methods. See Astrom and Murray 2008
+    Chapter 3.
 
-    :param dt: timestep in seconds
-    :type dt: float
+    :param float duration: number of seconds to simulate
+    :param float dt: timestep in seconds
 
-    :return: a tuple consisting of arrays of size [N, M], where M is the number of time steps.:
-            - state_vals: state of the car, i.e. position and velocity as a function of time
-            - disturbances: disturbances, ie. hills, that the car is subjected to
-            - controls: control inputs applied by the car
-    :rtype: tuple -> (np.array, np.array, np.array)
+    :return: tuple[np.array, np.array, np.array] of\n
+            - **state_vals** -- state of the car, i.e. position and velocity as a function of time, shape (N,M)
+            - **disturbances** -- disturbances from hills that the car is subjected to, shape (M,)
+            - **controls** -- control inputs applied by the car, shape (M+1,)
     """
-    t = np.arange(0, timeseries_length, dt)
+    t = np.arange(0, duration, dt)
     iterations = len(t)
 
-    # hills
-    disturbances = np.matrix(np.zeros([3, iterations+1]))
-    h = hills(iterations+1, dt, factor=0.5*timeseries_length/2)
-    disturbances[2, :] = h[:, 0:disturbances.shape[1]]
-
-    # controls
-    controls = np.matrix([[0]])
-
-    # initial condition
-    state_vals = np.matrix([[0], [0], [0]])
+    disturbances = np.array(np.zeros([3, iterations+1])) # disturbance from hills
+    h = hills(iterations+1, dt, factor=0.5*duration/2)
+    disturbances[2, :] = h[0:disturbances.shape[1]]
 
-    # desired vel
-    v_r = desired_velocity(iterations, factor=0.5*iterations*dt/2)
+    controls = np.array([0])
+    state_vals = np.array([[0], [0], [0]]) # initial condition
+    v_r = desired_velocity(iterations, factor=0.5*iterations*dt/2) # desired, reference velocity
 
     for i in range(1, iterations+1):
-        new_state, u = step_forward(state_vals, disturbances[:, 0:i], v_r[:, 0:i], dt)
+        new_state, u = step_forward(state_vals, disturbances[:, 0:i], v_r[0:i], dt)
         state_vals = np.hstack((state_vals, new_state))
         controls = np.hstack((controls, u))