Amir mohammad saffar python codes for modern control book

3/Active.py

@@ -0,0 +1,53 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.signal import StateSpace, lsim
+
+# Define system matrices
+A = np.array([[0, 0, 1, 0],
+              [0, 0, 0, 1],
+              [-10, 10, -2, 2],
+              [60, -660, 12, -12]])
+
+b1 = np.array([[0], [0], [0.0033], [-0.02]])
+b2 = np.array([[0], [0], [0], [600]])
+B = np.hstack((b1, b2))
+C = np.array([[1, 0, 0, 0]])
+D = np.array([[0]])
+
+# Create state space system
+active_suspension = StateSpace(A, B, C, D)
+
+# Define time vector and initial conditions
+t = np.arange(0, 7.01, 0.01)
+x0 = np.array([0.2, 0, 0, 0])
+
+# Initial response simulation
+t, y, x = lsim(active_suspension, 0, t, X0=x0)
+
+# Plot initial response
+plt.figure()
+plt.plot(t, x[:, 0], 'k', label='x1')
+plt.plot(t, x[:, 1], 'k-.', label='x2')
+plt.grid(True)
+plt.xlabel('Time (sec)')
+plt.ylabel('State variables')
+plt.legend(['x1', 'x2'])
+plt.title('Initial Response')
+plt.show()
+
+# Generate input signal u
+u = 0.1 * (np.sin(5*t) + np.sin(9*t) + np.sin(13*t) + np.sin(17*t) + np.sin(21*t))
+
+# Simulate the system with input u
+t, y, x = lsim(active_suspension, u, t)
+
+# Plot the simulation result
+plt.figure()
+plt.plot(t, x[:, 0], 'k', label='x1')
+plt.plot(t, x[:, 1], 'k-.', label='x2')
+plt.grid(True)
+plt.xlabel('Time (sec)')
+plt.ylabel('State variables')
+plt.legend(['x1', 'x2'])
+plt.title('Simulation Result')
+plt.show()

3/DCmotor.py

@@ -0,0 +1,53 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.signal import lsim, square
+
+# Simulation of the DC motor
+# Copyright Hamid D. Taghirad 2013
+
+# Define system matrices
+A = np.array([[0, 1, 0],
+              [0, 0, 4.438],
+              [0, -12, -24]])
+
+b1 = np.array([[0], [0], [20]])
+b2 = np.array([[0], [-7.396], [0]])
+B = np.hstack((b1, b2))
+
+C = np.array([[1, 0, 0],
+              [0, 1, 0]])
+
+D = np.array([[0]])
+
+# Create state space system
+DC_motor = (A, B, C, D)
+
+# Define time vector
+t = np.arange(0, 4.01, 0.01)
+N = len(t)
+
+# A Simple way to generate input u
+u = np.zeros((2, N))
+for i in range(N):
+    if t[i] < 2:
+        u[0, i] = 3
+    else:
+        u[0, i] = -3
+
+# A Professional way to generate input u
+u = 6 * square(2 * np.pi * 0.25 * t) - 3  # Generate square wave with period 4 and amplitude 6
+
+# Simulate the system
+t, y, x = lsim(DC_motor, u, t)
+
+# Plot the result
+plt.figure()
+plt.plot(t, x[:, 0], 'k', label='theta')
+plt.plot(t, x[:, 1], 'k-.', label='omega')
+plt.plot(t, x[:, 2], 'k:', label='i')
+plt.grid(True)
+plt.xlabel('Time (sec)')
+plt.ylabel('State variables')
+plt.legend(['theta', 'omega', 'i'])
+plt.title('Simulation Result - DC Motor')
+plt.show()

3/DCmotor_transfun.py

@@ -0,0 +1,37 @@
+import numpy as np
+import control
+
+# Transfer functions for the DC motor
+
+# Define the state-space matrices
+A = np.array([[0, 1, 0],
+              [0, 0, 4.438],
+              [0, -12, -24]])
+
+b1 = np.array([[0],
+               [0],
+               [20]])
+
+b2 = np.array([[0],
+               [-7.396],
+               [0]])
+
+B = np.hstack((b1, b2))
+C = np.array([[1, 0, 0]])  # Note only \theta is used as output
+D = np.array([[0, 0]])
+
+# Create state-space system
+DCM = control.ss(A, B, C, D)
+
+# Conversion to transfer function
+DCM_tf = control.tf(DCM)
+
+# Conversion to zero-pole-gain form
+DCM_zpk = control.zpk(DCM)
+
+# Display the results
+print("Transfer Function:")
+print(DCM_tf)
+
+print("\nZero-Pole-Gain:")
+print(DCM_zpk)

3/activate_this.py

@@ -0,0 +1,26 @@
+import numpy as np
+import scipy.io.wavfile
+import matplotlib.pyplot as plt
+
+sampling_rate, data = scipy.io.wavfile.read('voice1.wav')
+print(f"sampling rate: {sampling_rate}hz") # frequency (sample per second)
+print('data type:', data.dtype)
+print('data shape:', data.shape)
+# TODO: Calculate length of the signal in seconds
+# print(f"length = {length}s")
+N, no_channels = data.shape # signal length and no. of channels
+print('signal length:', N)
+channel0 = data[:,0]
+channel1 = data[:,1]
+scale = np.linspace(-2,4,N)
+plt.plot(np.arange(N), scale)
+plt.show()
+print('shape_old:', scale.shape)
+scale.shape = (N,1)
+print('shape_new:', scale.shape)
+data_new = data[::-1]
+data_new1 = np.int16(scale * data)
+scipy.io.wavfile.write('reverse.wav', sampling_rate , data_new)
+scipy.io.wavfile.write('scale.wav', sampling_rate , data_new1)
+scipy.io.wavfile.write('fast.wav', sampling_rate * 2 , data_new1)
+scipy.io.wavfile.write('slow.wav', sampling_rate // 2 , data_new1)

3/inverted_pendulum.py

@@ -0,0 +1,53 @@
+import numpy as np
+from scipy.integrate import solve_ivp
+import matplotlib.pyplot as plt
+
+
+def inverted_pendulum(t, x):
+    # State variable x = [x, theta, v, omega]
+    g = 9.8
+    l = 1
+    m = 1
+    M = 1
+    d1 = M + m * (1 - np.cos(x[1]) ** 2)
+    d2 = l * d1
+
+    F = 0  # No input
+
+    xp = np.zeros(4)
+    xp[0] = x[2]
+    xp[1] = x[3]
+    xp[2] = (F + m * l * x[3] ** 2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1
+    xp[3] = (-F * np.cos(x[1]) - m * l * x[3] ** 2 * np.sin(x[1]) * np.cos(x[1]) + (M + m) * g * np.sin(x[1])) / d2
+
+    return xp
+
+
+# Initial conditions: x, theta, v, omega
+x0 = [0, np.pi / 4, 0, 0]
+
+# Time span for the simulation
+t_span = (0, 10)
+t_eval = np.linspace(*t_span, 300)
+
+# Solve the differential equation
+sol = solve_ivp(inverted_pendulum, t_span, x0, t_eval=t_eval)
+
+# Plotting the results
+plt.figure(figsize=(10, 8))
+plt.subplot(2, 1, 1)
+plt.plot(sol.t, sol.y[0], label='x (position)')
+plt.plot(sol.t, sol.y[1], label='theta (angle)')
+plt.legend()
+plt.xlabel('Time [s]')
+plt.ylabel('Position/Angle')
+
+plt.subplot(2, 1, 2)
+plt.plot(sol.t, sol.y[2], label='v (velocity)')
+plt.plot(sol.t, sol.y[3], label='omega (angular velocity)')
+plt.legend()
+plt.xlabel('Time [s]')
+plt.ylabel('Velocity/Angular Velocity')
+
+plt.tight_layout()
+plt.show()

3/invpend_solver.py

@@ -0,0 +1,45 @@
+import numpy as np
+from scipy.integrate import solve_ivp
+import matplotlib.pyplot as plt
+
+
+def inverted_pendulum(t, x):
+    # State variable x = [x, theta, v, omega]
+    g = 9.8
+    l = 1
+    m = 1
+    M = 1
+    d1 = M + m * (1 - np.cos(x[1]) ** 2)
+    d2 = l * d1
+
+    F = 0  # No input
+
+    xp = np.zeros(4)
+    xp[0] = x[2]
+    xp[1] = x[3]
+    xp[2] = (F + m * l * x[3] ** 2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1
+    xp[3] = (-F * np.cos(x[1]) - m * l * x[3] ** 2 * np.sin(x[1]) * np.cos(x[1]) + (M + m) * g * np.sin(x[1])) / d2
+
+    return xp
+
+
+# Initial conditions: x, theta, v, omega
+x0 = [0, 0.1, 0, 0]
+
+# Time span for the simulation
+t_span = (0, 1)
+t_eval = np.linspace(*t_span, 300)  # Generating 300 time points within the span
+
+# Solve the differential equation
+sol = solve_ivp(inverted_pendulum, t_span, x0, t_eval=t_eval, max_step=1e-2)
+
+# Plotting the results
+plt.figure(figsize=(10, 6))
+plt.plot(sol.t, sol.y[0], 'k', label='x (m)')
+plt.plot(sol.t, sol.y[1], '-.k', label='theta (rad)')
+plt.grid(True)
+plt.xlabel('Time (sec)')
+plt.ylabel('State Variables')
+plt.legend()
+plt.title('Simulation of Inverted Pendulum')
+plt.show()

3/robot_model.py

@@ -0,0 +1,60 @@
+import numpy as np
+
+def robot_model(t, x):
+    # State variable x = [theta_1, theta_2, omega_1, omega_2]
+    g = 9.81
+    l1 = 1
+    l2 = 0.5
+    m1 = 2
+    m2 = 1
+    I1 = 1e-2
+    I2 = 5e-3
+    D = 2
+
+    M = np.zeros((2, 2))
+    M[0, 0] = m1 * (l1 / 2)**2 + m2 * (l1**2 + (l2 / 2)**2) + m2 * l1 * l2 * np.cos(x[1]) + I1 + I2
+    M[0, 1] = m2 * (l2 / 2)**2 + 0.5 * m2 * l1 * l2 * np.cos(x[1]) + I2
+    M[1, 0] = M[0, 1]
+    M[1, 1] = m2 * (l2 / 2)**2 + I2
+
+    V = np.zeros((2, 1))
+    V[0, 0] = -m2 * l1 * l2 * np.sin(x[1]) * x[2] * x[3] - 0.5 * m2 * l1 * l2 * np.sin(x[1]) * x[3]**2
+    V[1, 0] = -0.5 * m2 * l1 * l2 * np.sin(x[1]) * x[2] * x[3]
+
+    G = np.zeros((2, 1))
+    G[0, 0] = (m1 * l1 / 2 + m2 * l1) * g * np.cos(x[0]) + m2 * g * l2 / 2 * np.cos(x[0] + x[1])
+    G[1, 0] = m2 * g * l2 / 2 * np.cos(x[0] + x[1])
+
+    Q = np.zeros((2, 1))  # No input
+    Q = Q - D * np.array([[x[2]], [x[3]]])
+
+    # Solve for the accelerations
+    xy = np.linalg.pinv(M).dot(Q - V - G)
+
+    xp = np.array([x[2], x[3], xy[0, 0], xy[1, 0]])
+    return xp
+
+# Example usage: integrate over time using solve_ivp
+from scipy.integrate import solve_ivp
+import matplotlib.pyplot as plt
+
+# Initial conditions: theta1, theta2, omega1, omega2
+x0 = [0, 0.1, 0, 0]
+
+# Time span for the simulation
+t_span = (0, 10)
+t_eval = np.linspace(*t_span, 300)
+
+# Solve the differential equation
+sol = solve_ivp(robot_model, t_span, x0, t_eval=t_eval)
+
+# Plotting the results
+plt.figure(figsize=(10, 6))
+plt.plot(sol.t, sol.y[0], label='theta_1 (rad)')
+plt.plot(sol.t, sol.y[1], label='theta_2 (rad)')
+plt.grid(True)
+plt.xlabel('Time (sec)')
+plt.ylabel('State Variables')
+plt.legend()
+plt.title('Simulation of 2R Robot')
+plt.show()

3/robot_solver.py

@@ -0,0 +1,58 @@
+import numpy as np
+from scipy.integrate import solve_ivp
+import matplotlib.pyplot as plt
+
+def robot_model(t, x):
+    # State variable x = [theta_1, theta_2, omega_1, omega_2]
+    g = 9.81
+    l1 = 1
+    l2 = 0.5
+    m1 = 2
+    m2 = 1
+    I1 = 1e-2
+    I2 = 5e-3
+    D = 2
+
+    M = np.zeros((2, 2))
+    M[0, 0] = m1 * (l1 / 2)**2 + m2 * (l1**2 + (l2 / 2)**2) + m2 * l1 * l2 * np.cos(x[1]) + I1 + I2
+    M[0, 1] = m2 * (l2 / 2)**2 + 0.5 * m2 * l1 * l2 * np.cos(x[1]) + I2
+    M[1, 0] = M[0, 1]
+    M[1, 1] = m2 * (l2 / 2)**2 + I2
+
+    V = np.zeros((2, 1))
+    V[0, 0] = -m2 * l1 * l2 * np.sin(x[1]) * x[2] * x[3] - 0.5 * m2 * l1 * l2 * np.sin(x[1]) * x[3]**2
+    V[1, 0] = -0.5 * m2 * l1 * l2 * np.sin(x[1]) * x[2] * x[3]
+
+    G = np.zeros((2, 1))
+    G[0, 0] = (m1 * l1 / 2 + m2 * l1) * g * np.cos(x[0]) + m2 * g * l2 / 2 * np.cos(x[0] + x[1])
+    G[1, 0] = m2 * g * l2 / 2 * np.cos(x[0] + x[1])
+
+    Q = np.zeros((2, 1))  # No input
+    Q = Q - D * np.array([[x[2]], [x[3]]])
+
+    # Solve for the accelerations
+    xy = np.linalg.pinv(M).dot(Q - V - G)
+
+    xp = np.array([x[2], x[3], xy[0, 0], xy[1, 0]])
+    return xp
+
+# Initial conditions: theta1, theta2, omega1, omega2
+x0 = [-np.pi / 3, np.pi / 3, 0, 0]
+
+# Time span for the simulation
+t_span = (0, 5)
+t_eval = np.linspace(*t_span, 300)  # Generate 300 time points within the span
+
+# Solve the differential equation
+sol = solve_ivp(robot_model, t_span, x0, t_eval=t_eval)
+
+# Plotting the results
+plt.figure(figsize=(10, 6))
+plt.plot(sol.t, np.degrees(sol.y[0]), 'k', label='theta_1 (degrees)')
+plt.plot(sol.t, np.degrees(sol.y[1]), '-.k', label='theta_2 (degrees)')
+plt.grid(True)
+plt.xlabel('Time (sec)')
+plt.ylabel('State Variables')
+plt.legend()
+plt.title('Simulation of 2R Robot')
+plt.show()