Updated remaining classes to simplify

Author: botprof

MPC_linear_tracking.py

@@ -9,8 +9,6 @@
 
 import numpy as np
 import matplotlib.pyplot as plt
-from mobotpy import models
-from mobotpy import integration
 from scipy import signal
 from numpy.linalg import matrix_power
 from numpy.linalg import inv

ackermann_kinematic.py

@@ -9,8 +9,8 @@
 
 import numpy as np
 import matplotlib.pyplot as plt
-from mobotpy import integration
-from mobotpy import models
+from mobotpy.integration import rk_four
+from mobotpy.models import Ackermann
 
 # Set the simulation time [s] and the sample period [s]
 SIM_TIME = 15.0
@@ -42,12 +42,11 @@
 u[1, 0] = 0
 
 # Let's now use the class Ackermann for plotting
-vehicle = models.Ackermann(ELL_W, ELL_T)
+vehicle = Ackermann(ELL_W, ELL_T)
 
 # Run the simulation
 for k in range(1, N):
-    x[:, k] = integration.rk_four(
-        vehicle.f, x[:, k - 1], u[:, k - 1], T, ELL_W)
+    x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T)
     phi_L[k] = np.arctan(
         2 * ELL_W * np.tan(x[3, k]) / (2 * ELL_W - ELL_T * np.tan(x[3, k]))
     )
@@ -100,15 +99,15 @@
 plt.plot(x[0, :], x[1, :])
 plt.axis("equal")
 X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = vehicle.draw(
-    x[0, 0], x[1, 0], x[2, 0], phi_L[0], phi_R[0], ELL_W, ELL_T
+    x[0, 0], x[1, 0], x[2, 0], phi_L[0], phi_R[0]
 )
 plt.fill(X_BL, Y_BL, "k")
 plt.fill(X_BR, Y_BR, "k")
 plt.fill(X_FR, Y_FR, "k")
 plt.fill(X_FL, Y_FL, "k")
 plt.fill(X_BD, Y_BD, "C2", alpha=0.5, label="Start")
 X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = vehicle.draw(
-    x[0, N - 1], x[1, N - 1], x[2, N - 1], phi_L[N - 1], phi_R[N - 1], ELL_W, ELL_T
+    x[0, N - 1], x[1, N - 1], x[2, N - 1], phi_L[N - 1], phi_R[N - 1]
 )
 plt.fill(X_BL, Y_BL, "k")
 plt.fill(X_BR, Y_BR, "k")
@@ -134,8 +133,6 @@
     T,
     phi_L,
     phi_R,
-    ELL_W,
-    ELL_T,
     True,
     "../agv-book/gifs/ch3/ackermann_kinematic.gif",
 )

control_approx_linearization.py

@@ -9,8 +9,8 @@
 
 import numpy as np
 import matplotlib.pyplot as plt
-from mobotpy import models
-from mobotpy import integration
+from mobotpy.models import DiffDrive
+from mobotpy.integration import rk_four
 from scipy import signal
 
 # Set the simulation time [s] and the sample period [s]
@@ -47,7 +47,7 @@
 ELL = 1.0
 
 # Create a vehicle object of type DiffDrive
-vehicle = models.DiffDrive(ELL)
+vehicle = DiffDrive(ELL)
 
 # %%
 # SIMULATE THE CLOSED-LOOP SYSTEM
@@ -81,10 +81,10 @@
 
     # Compute the controls (v, omega) and convert to wheel speeds (v_L, v_R)
     u_unicycle = -K.gain_matrix @ (x[:, k - 1] - x_d[:, k - 1]) + u_d[:, k - 1]
-    u[:, k] = vehicle.uni2diff(u_unicycle, ELL)
+    u[:, k] = vehicle.uni2diff(u_unicycle)
 
     # Simulate the differential drive vehicle motion
-    x[:, k] = integration.rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T, ELL)
+    x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T)
 
 # %%
 # MAKE PLOTS
@@ -133,13 +133,13 @@
 plt.plot(x_d[0, :], x_d[1, :], "C1--", label="Desired")
 plt.plot(x[0, :], x[1, :], "C0", label="Actual")
 plt.axis("equal")
-X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0], ELL)
+X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0])
 plt.fill(X_L, Y_L, "k")
 plt.fill(X_R, Y_R, "k")
 plt.fill(X_C, Y_C, "k")
 plt.fill(X_B, Y_B, "C2", alpha=0.5, label="Start")
 X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(
-    x[0, N - 1], x[1, N - 1], x[2, N - 1], ELL
+    x[0, N - 1], x[1, N - 1], x[2, N - 1]
 )
 plt.fill(X_L, Y_L, "k")
 plt.fill(X_R, Y_R, "k")
@@ -160,7 +160,7 @@
 
 # Create and save the animation
 ani = vehicle.animate_trajectory(
-    x, x_d, T, ELL, True, "../agv-book/gifs/ch4/control_approx_linearization.gif"
+    x, x_d, T, True, "../agv-book/gifs/ch4/control_approx_linearization.gif"
 )
 
 # Show the movie to the screen

diffdrive_kinematic.py

@@ -9,7 +9,7 @@
 
 import numpy as np
 import matplotlib.pyplot as plt
-from mobotpy import models
+from mobotpy.models import DiffDrive
 
 # Set the simulation time [s] and the sample period [s]
 SIM_TIME = 30.0
@@ -19,35 +19,38 @@
 t = np.arange(0.0, SIM_TIME, T)
 N = np.size(t)
 
+# Set the track of the vehicle [m]
+ELL = 0.35
+
 # %%
 # FUNCTION DEFINITIONS
 
 
-def rk_four(f, x, u, T, P):
+def rk_four(f, x, u, T):
     """Fourth-order Runge-Kutta numerical integration."""
-    k_1 = f(x, u, P)
-    k_2 = f(x + T * k_1 / 2.0, u, P)
-    k_3 = f(x + T * k_2 / 2.0, u, P)
-    k_4 = f(x + T * k_3, u, P)
+    k_1 = f(x, u)
+    k_2 = f(x + T * k_1 / 2.0, u)
+    k_3 = f(x + T * k_2 / 2.0, u)
+    k_4 = f(x + T * k_3, u)
     x_new = x + T / 6.0 * (k_1 + 2.0 * k_2 + 2.0 * k_3 + k_4)
     return x_new
 
 
-def diffdrive_f(x, u, ell):
+def diffdrive_f(x, u):
     """Differential drive kinematic vehicle model."""
     f = np.zeros(3)
     f[0] = 0.5 * (u[0] + u[1]) * np.cos(x[2])
     f[1] = 0.5 * (u[0] + u[1]) * np.sin(x[2])
-    f[2] = 1.0 / ell * (u[1] - u[0])
+    f[2] = 1.0 / ELL * (u[1] - u[0])
     return f
 
 
-def uni2diff(u, ell):
+def uni2diff(u):
     """Convert speed and angular rate to wheel speeds."""
     v = u[0]
     omega = u[1]
-    v_L = v - ell / 2 * omega
-    v_R = v + ell / 2 * omega
+    v_L = v - ELL / 2 * omega
+    v_R = v + ELL / 2 * omega
     return np.array([v_L, v_R])
 
 
@@ -65,19 +68,16 @@ def openloop(t):
 x = np.zeros((3, N))
 u = np.zeros((2, N))
 
-# Set the track of the vehicle [m]
-ELL = 0.35
-
 # Set the initial pose [m, m, rad], velocities [m/s, m/s]
 x[0, 0] = 0.0
 x[1, 0] = 0.0
 x[2, 0] = np.pi / 2.0
-u[:, 0] = uni2diff(openloop(t[0]), ELL)
+u[:, 0] = uni2diff(openloop(t[0]))
 
 # Run the simulation
 for k in range(1, N):
-    x[:, k] = rk_four(diffdrive_f, x[:, k - 1], u[:, k - 1], T, ELL)
-    u[:, k] = uni2diff(openloop(t[k]), ELL)
+    x[:, k] = rk_four(diffdrive_f, x[:, k - 1], u[:, k - 1], T)
+    u[:, k] = uni2diff(openloop(t[k]))
 
 # %%
 # MAKE PLOTS
@@ -118,20 +118,19 @@ def openloop(t):
 plt.savefig("../agv-book/figs/ch3/diffdrive_kinematic_fig1.pdf")
 
 # Let's now use the class DiffDrive for plotting
-vehicle = models.DiffDrive(ELL)
+vehicle = DiffDrive(ELL)
 
 # Plot the position of the vehicle in the plane
 fig2 = plt.figure(2)
 plt.plot(x[0, :], x[1, :], "C0")
 plt.axis("equal")
-X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(
-    x[0, 0], x[1, 0], x[2, 0], ELL)
+X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0])
 plt.fill(X_L, Y_L, "k")
 plt.fill(X_R, Y_R, "k")
 plt.fill(X_C, Y_C, "k")
 plt.fill(X_B, Y_B, "C2", alpha=0.5, label="Start")
 X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(
-    x[0, N - 1], x[1, N - 1], x[2, N - 1], ELL
+    x[0, N - 1], x[1, N - 1], x[2, N - 1]
 )
 plt.fill(X_L, Y_L, "k")
 plt.fill(X_R, Y_R, "k")
@@ -151,8 +150,7 @@ def openloop(t):
 # MAKE AN ANIMATION
 
 # Create and save the animation
-ani = vehicle.animate(
-    x, T, ELL, True, "../agv-book/gifs/ch3/diffdrive_kinematic.gif")
+ani = vehicle.animate(x, T, True, "../agv-book/gifs/ch3/diffdrive_kinematic.gif")
 
 # Show the movie to the screen
 plt.show()

dynamic_extension_tracking.py

@@ -114,10 +114,10 @@
     u_unicycle[1] = w[1]
 
     # Convert unicycle inputs to differential drive wheel speeds
-    u[:, k] = vehicle.uni2diff(u_unicycle, ELL)
+    u[:, k] = vehicle.uni2diff(u_unicycle)
 
     # Simulate the vehicle motion
-    x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T, ELL)
+    x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T)
 
     # Update the extended system states
     xi[0, k] = x[0, k]
@@ -172,13 +172,13 @@
 plt.plot(x_d[0, :], x_d[1, :], "C1--", label="Desired")
 plt.plot(x[0, :], x[1, :], "C0", label="Actual")
 plt.axis("equal")
-X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0], ELL)
+X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0])
 plt.fill(X_L, Y_L, "k")
 plt.fill(X_R, Y_R, "k")
 plt.fill(X_C, Y_C, "k")
 plt.fill(X_B, Y_B, "C2", alpha=0.5, label="Start")
 X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(
-    x[0, N - 1], x[1, N - 1], x[2, N - 1], ELL
+    x[0, N - 1], x[1, N - 1], x[2, N - 1]
 )
 plt.fill(X_L, Y_L, "k")
 plt.fill(X_R, Y_R, "k")
@@ -199,7 +199,7 @@
 
 # Create and save the animation
 ani = vehicle.animate_trajectory(
-    x, x_d, T, ELL, True, "../agv-book/gifs/ch4/dynamic_extension_tracking.gif"
+    x, x_d, T, True, "../agv-book/gifs/ch4/dynamic_extension_tracking.gif"
 )
 
 # Show the movie to the screen

mobotpy/integration.py

@@ -5,29 +5,29 @@
 """
 
 
-def rk_four(f, x, u, T, params):
+def rk_four(f, x, u, T):
     """
     Perform fourth-order Runge-Kutta numerical integration.
 
     The function to integrate is f(x, u, params), where the state varables are
     collected in the variable x, we assume a constant input vector u over time
     interval T, and params is an array of the system's parameters.
     """
-    k_1 = f(x, u, params)
-    k_2 = f(x + T * k_1 / 2.0, u, params)
-    k_3 = f(x + T * k_2 / 2.0, u, params)
-    k_4 = f(x + T * k_3, u, params)
+    k_1 = f(x, u)
+    k_2 = f(x + T * k_1 / 2.0, u)
+    k_3 = f(x + T * k_2 / 2.0, u)
+    k_4 = f(x + T * k_3, u)
     x_new = x + T / 6.0 * (k_1 + 2.0 * k_2 + 2.0 * k_3 + k_4)
     return x_new
 
 
-def euler_int(f, x, u, T, params):
+def euler_int(f, x, u, T):
     """
     Perform Euler (trapezoidal) numerical integration.
 
     The function to integrate is f(x, u, params), where the state varables are
     collected in the variable x, we assume a constant input vector u over time
     interval T, and params is an array of the system's parameters.
     """
-    x_new = x + T * f(x, u, params)
+    x_new = x + T * f(x, u)
     return x_new