python_Elnaz_Mohseninezhad

Chapter3/done.txt

@@ -0,0 +1,8 @@
+dc_motor done
+Active_sp done
+train_linear done
+train done
+tank_solver done
+tank model done
+robot model done
+robot solver done

Chapter3/python/Active_s.py

@@ -0,0 +1,55 @@
+import numpy as np
+import control as ctrl
+import matplotlib.pyplot as plt
+
+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 = ctrl.ss(A, b2, C, D)
+
+# Time vector
+t = np.arange(0, 7, 0.01)
+N = len(t)
+
+
+# Initial state
+x0 = np.array([[0.2], [0], [0], [0]])
+
+# Simulate initial response
+response = ctrl.forced_response(active_suspension, T=t, X0=x0)
+
+
+# Plot state variables x1 and x2
+plt.plot(t, response.states[0], 'k', label='x1')
+plt.plot(t, response.states[1], 'k-.', label='x2')
+plt.xlabel('Time (sec)')
+plt.ylabel('State variables')
+plt.grid(True)
+plt.legend()
+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 response of the system
+# Simulate the response of the system with the provided input signal
+response = ctrl.forced_response(active_suspension, T=t, U=u)
+
+import matplotlib.pyplot as plt
+
+# Plot state variables x1 and x2
+plt.plot(t, response.states[0], 'k', label='x1')
+plt.plot(t, response.states[1], 'k-.', label='x2')
+plt.xlabel('Time (sec)')
+plt.ylabel('State variables')
+plt.grid(True)
+plt.legend()
+plt.show()

Chapter3/python/DCmotor.py

@@ -0,0 +1,94 @@
+import numpy as np
+import control as ctrl
+import matplotlib.pyplot as plt
+
+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, 0, 0],
+            #   [0, 0, 0]])
+D = np.zeros((2, 1))
+# Create state-space system
+DC_motor = ctrl.ss(A, b1, C, D)
+
+# Simulation
+t = np.arange(0, 4, 0.01)
+t, yout = ctrl.step_response(DC_motor, T=t)
+
+
+# t = np.arange(0, 4, 0.01)  # Create the time array
+# N = np.size(t)  # Get the number of elements in the array t
+# # Or equivalently
+# N = len(t)  # Get the number of elements in the array t
+
+
+
+# Assuming t is already defined
+N = len(t)
+u = np.zeros((2, N))  # Assuming u has 2 rows (inputs) and N columns (time points)
+
+for i in range(N):
+    if t[i] < 2:
+        u[:, i] = 3  # Set all elements in column i to 3
+    else:
+        u[:, i] = -3  # Set all elements in column i to -3
+
+
+
+
+
+# Generate a square wave signal
+# t = np.arange(0, 4, 0.01)  # Time vector from 0 to 4 with 0.01 step
+u = -6 * np.sign(np.sin(2 * np.pi * 0.25 * t)) + 3  # Square wave with period 4 seconds, scaled and offset
+
+# Simulate the response of the system
+# _, y, _ = ctrl.forced_response(DC_motor, T=t, U=u)
+# y contains the system's output, t contains the time vector, and x contains the states of the system
+response = ctrl.forced_response(DC_motor, T=t, U=u)
+
+# Inspect the response
+print(response)
+
+
+# print("Time array:")
+# print(t)
+# print("\nOutput array:")
+# print(yout)
+
+
+
+# plt.plot(t, response.states[2], 'k', label=r'$\theta$')
+# plt.plot(t, response.states[1], 'k-.', label=r'$\omega$')
+# plt.plot(t, response.states[0], 'k:', label='i')
+# plt.xlabel('Time (sec)')
+# plt.ylabel('State variables')
+# plt.grid(True)
+# plt.legend()
+# plt.show()
+
+plt.plot(t, -response.states[2], 'k', label='i')
+plt.plot(t, -response.states[1], 'k-.', label=r'$\omega$')
+plt.plot(t, -response.states[0], 'k:', label=r'$\theta$')
+plt.xlabel('Time (sec)')
+plt.ylabel('State variables')
+plt.grid(True)
+plt.legend()
+plt.show()
+#############################################
+# no need to use the DCmotor_transfun function
+
+

Chapter3/python/DCmotor_transfun.py

@@ -0,0 +1,25 @@
+import numpy as np
+from scipy import signal
+
+# 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]])
+D = np.array([[0, 0]])
+
+# Convert to transfer function
+num, den = signal.ss2tf(A, B, C, D)
+DCM_tf = signal.TransferFunction(num[0, :], den[0])
+
+# Convert to zero-pole-gain form
+DCM_zpk = signal.ss2zpk(A, B, C, D)
+
+# Print transfer function
+print("Transfer Function:")
+print(DCM_tf)
+
+# Print zero-pole-gain form
+print("\nZero-Pole-Gain Form:")
+print(DCM_zpk)

Chapter3/python/Invpend_solver.py

@@ -0,0 +1,63 @@
+import numpy as np
+
+tspan = np.array([0, 1])
+x0 = np.array([0, 0.1, 0, 0])
+
+import numpy as np
+
+def inverted_pendulum(x, t):
+    # Parameters
+    g = 9.8
+    l = 1
+    m = 1
+    M = 1
+
+    # Compute intermediate terms
+    d1 = M + m * (1 - np.cos(x[1])**2)
+    d2 = l * d1
+
+    # Input force
+    F = 0  # No input
+
+    # Compute time derivatives of state variables
+    xp = np.array([
+        x[2],  # x'
+        x[3],  # theta'
+        (F + m * l * x[3]**2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1,  # v'
+        (-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  # omega'
+    ])
+
+    return xp
+
+from scipy.integrate import odeint
+
+# Define the time points at which you want the solution
+t_eval = np.linspace(tspan[0], tspan[1], 100)  # Adjust the number of points as needed
+
+# Solve the ODE using odeint
+x = odeint(inverted_pendulum, x0, t_eval)
+
+
+
+import matplotlib.pyplot as plt
+
+# Plot the first and second columns of x against time
+plt.plot(t_eval, x[:, 0], 'k', label='x (m)')
+plt.plot(t_eval, x[:, 1], '-.k', label=r'$\theta$ (rad)')
+
+# Add gridlines
+plt.grid(True)
+
+# Set labels for x-axis and y-axis
+plt.xlabel('Time (sec)')
+plt.ylabel('State Variables')
+
+# Add legend
+plt.legend()
+
+# Set linewidth for all lines in the plot
+plt.setp(plt.gca().lines, linewidth=2)
+
+# Show the plot
+plt.show()

Chapter3/python/inverted_pendulum.py

@@ -0,0 +1,25 @@
+
+def inverted_pendulum(x, t):
+    # Parameters
+    g = 9.8
+    l = 1
+    m = 1
+    M = 1
+
+    # Compute intermediate terms
+    d1 = M + m * (1 - np.cos(x[1])**2)
+    d2 = l * d1
+
+    # Input force
+    F = 0  # No input
+
+    # Compute time derivatives of state variables
+    xp = np.array([
+        x[2],  # x'
+        x[3],  # theta'
+        (F + m * l * x[3]**2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1,  # v'
+        (-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  # omega'
+    ])
+
+    return xp

Chapter3/python/robot_model.py

@@ -0,0 +1,36 @@
+import numpy as np
+
+def robot_model(t, x):
+    # Parameters
+    g = 9.81
+    l1 = 1
+    l2 = 0.5
+    m1 = 2
+    m2 = 1
+    I1 = 1e-2
+    I2 = 5e-3
+    D = 2
+
+    # Mass matrix M
+    M = np.array([[m1*(l1/2)**2 + m2*(l1**2 + (l2/2)**2) + m2*l1*l2*np.cos(x[1]) + I1 + I2,
+                   m2*(l2/2)**2 + 0.5*m2*l1*l2*np.cos(x[1]) + I2],
+                  [m2*(l2/2)**2 + 0.5*m2*l1*l2*np.cos(x[1]) + I2,
+                   m2*(l2/2)**2 + I2]])
+
+    # Coriolis and centrifugal force vector V
+    V = np.array([-m2*l1*l2*np.sin(x[1])*x[2]*x[3] - 0.5*m2*l1*l2*np.sin(x[1])*x[3]**2,
+                  -0.5*m2*l1*l2*np.sin(x[1])*x[2]*x[3]])
+
+    # Gravitational force vector G
+    G = np.array([(m1*l1/2 + m2*l1)*g*np.cos(x[0]) + m2*g*l2/2*np.cos(x[0] + x[1]),
+                  m2*g*l2/2*np.cos(x[0] + x[1])])
+
+    # Input vector Q
+    Q = np.array([0, 0])  # No input
+    Q -= D * np.array([x[2], x[3]])
+
+    # Compute acceleration
+    xy = np.linalg.pinv(M).dot(Q - V - G)
+
+    # Return time derivatives of state variables
+    return np.array([x[2], x[3], xy[0], xy[1]])

Chapter3/python/robot_solver.py

@@ -0,0 +1,63 @@
+import numpy as np
+from scipy.integrate import odeint
+import matplotlib.pyplot as plt
+
+# Define the robot model differential equation
+def robot_model(x, t):
+    # Parameters
+    g = 9.81
+    l1 = 1
+    l2 = 0.5
+    m1 = 2
+    m2 = 1
+    I1 = 1e-2
+    I2 = 5e-3
+    D = 2
+
+    # Mass matrix M
+    M = np.array([[m1*(l1/2)**2 + m2*(l1**2 + (l2/2)**2) + m2*l1*l2*np.cos(x[1]) + I1 + I2,
+                   m2*(l2/2)**2 + 0.5*m2*l1*l2*np.cos(x[1]) + I2],
+                  [m2*(l2/2)**2 + 0.5*m2*l1*l2*np.cos(x[1]) + I2,
+                   m2*(l2/2)**2 + I2]])
+
+    # Coriolis and centrifugal force vector V
+    V = np.array([-m2*l1*l2*np.sin(x[1])*x[2]*x[3] - 0.5*m2*l1*l2*np.sin(x[1])*x[3]**2,
+                  -0.5*m2*l1*l2*np.sin(x[1])*x[2]*x[3]])
+
+    # Gravitational force vector G
+    G = np.array([(m1*l1/2 + m2*l1)*g*np.cos(x[0]) + m2*g*l2/2*np.cos(x[0] + x[1]),
+                  m2*g*l2/2*np.cos(x[0] + x[1])])
+
+    # Input vector Q
+    # Q = np.array([0, 0])  # No input
+    # Q -= D * np.array([x[2], x[3]])
+    # Input vector Q
+    Q = np.array([0.0, 0.0])  # No input, ensure floats
+    Q -= D * np.array([x[2], x[3]], dtype=float)  # Ensure floats
+
+
+
+    # Compute acceleration
+    xy = np.linalg.pinv(M).dot(Q - V - G)
+
+    # Return time derivatives of state variables
+    return np.array([x[2], x[3], xy[0], xy[1]])
+
+# Define the time span
+tspan = np.linspace(0, 5, 1000)
+
+# Define the initial condition
+x0 = [-np.pi/3, np.pi/3, 0, 0]
+
+# Solve the ODE
+x = odeint(robot_model, x0, tspan)
+
+# Plot the joint angles against time
+plt.plot(tspan, np.degrees(x[:, 0]), 'k', label=r'$\theta_1$ (degrees)')
+plt.plot(tspan, np.degrees(x[:, 1]), '-.k', label=r'$\theta_2$ (degrees)')
+plt.grid(True)
+plt.xlabel('Time (sec)')
+plt.ylabel('State Variables')
+plt.legend()
+plt.setp(plt.gca().lines, linewidth=2)  # Set the linewidth of all lines in the current plot
+plt.show()