Add files via upload

Chapter3/Chapter3/Active_s/active_s.py

@@ -0,0 +1,74 @@
+# -*- coding: utf-8 -*-
+"""Active_s.ipynb
+
+Automatically generated by Colab.
+
+Original file is located at
+    https://colab.research.google.com/drive/1IgnliJrZLzc28PpJy-l43qlaRRszVBoX
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.integrate import solve_ivp
+
+# Define the state-space model
+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.column_stack((b1, b2))
+C = np.array([[1, 0, 0, 0]])
+D = np.array([0])
+
+# Simulation parameters
+t_span = (0, 7)  # Time range for simulation
+t_eval = np.linspace(0, 7, 701)  # Time points to evaluate
+x0 = [0.2, 0, 0, 0]  # Initial conditions
+
+# Define the system of ODEs for initial response
+def system_ode(t, x):
+    return A @ x
+
+# Simulate initial response using solve_ivp
+sol_initial = solve_ivp(system_ode, t_span, x0, t_eval=t_eval, method='RK45')
+x_initial = sol_initial.y.T
+
+# Plot initial response
+plt.figure()
+plt.plot(t_eval, x_initial[:, 0], 'k', label='$x_1$')
+plt.plot(t_eval, x_initial[:, 1], 'k-.', label='$x_2$')
+plt.grid(True)
+plt.xlabel('Time (sec)')
+plt.ylabel('State variables')
+plt.legend()
+plt.title('Initial Response')
+plt.show()
+
+# Define input signal function
+def input_signal(t):
+    return 0.1 * (np.sin(5 * t) + np.sin(9 * t) + np.sin(13 * t) + np.sin(17 * t) + np.sin(21 * t))
+
+# Define the system of ODEs with input
+def system_ode_with_input(t, x):
+    u = input_signal(t)
+    return A @ x + b2 * u
+
+# Simulate response with input using solve_ivp
+sol_forced = solve_ivp(system_ode_with_input, t_span, x0, t_eval=t_eval, method='RK45')
+x_forced = sol_forced.y.T
+
+# Plot response with input signal
+plt.figure()
+plt.plot(t_eval, x_forced[:, 0], 'k', label='$x_1$')
+plt.plot(t_eval, x_forced[:, 1], 'k-.', label='$x_2$')
+plt.grid(True)
+plt.xlabel('Time (sec)')
+plt.ylabel('State variables')
+plt.legend()
+plt.title('Response with Input Signal')
+plt.show()

Chapter3/Chapter3/DCmotor/dcmotor.py

@@ -0,0 +1,54 @@
+# -*- coding: utf-8 -*-
+"""DCmotor.ipynb
+
+Automatically generated by Colab.
+
+Original file is located at
+    https://colab.research.google.com/drive/1qEtOC5MiIZ7qpwmGILlODjmnK-zZKQb9
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+import control
+from scipy.signal import lsim
+
+# Define the 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], [0]])
+
+# Create state-space system
+DC_motor = control.ss(A, b1, C, D)  # Note only the first input is used
+
+# Define the time vector
+t = np.arange(0, 4.00, 0.01)
+N = t.size
+
+# Generate input u (simple way)
+u_simple = np.zeros(N)
+for i in range(N):
+    if t[i] < 2:
+        u_simple[i] = 3
+    else:
+        u_simple[i] = -3
+
+# Generate input u (professional way)
+u_prof = scipy.signal.square(2 * np.pi * t / 4)
+u_prof = (+6 * u_prof) - 3
+# Simulate the system with the simple input using lsim
+t_out, y, x = lsim((A, b1, C, D), U=u_simple, T=t)
+
+# Plot the result
+plt.plot(t_out, x[:, 0], 'k', label='\u03B8')  # θ
+plt.plot(t_out, x[:, 1], 'k-.', label='\u03C9')  # ω
+plt.plot(t_out, x[:, 2], 'k:', label='i') # i
+plt.grid(True)
+plt.xlabel('Time (sec)')
+plt.ylabel('State variables')
+plt.legend()
+plt.show()

Chapter3/Chapter3/Invpend_solver/invpend_solver.py

@@ -0,0 +1,50 @@
+# -*- coding: utf-8 -*-
+"""Invpend_solver.ipynb
+
+Automatically generated by Colab.
+
+Original file is located at
+    https://colab.research.google.com/drive/16Y1vhzJ8L5TfRFYvMGy5a2lBbMNVpzGT
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.integrate import solve_ivp
+
+# Define the inverted pendulum system
+def inverted_pendulum(t, x):
+    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
+
+    dxdt = np.zeros(4)
+    dxdt[0] = x[2]
+    dxdt[1] = x[3]
+    dxdt[2] = (F + m * l * x[3]**2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1
+    dxdt[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 dxdt
+
+# Simulation parameters
+t_span = (0, 1)
+
+x0 = [0, 0.1, 0, 0]  # Initial state: [x; theta; v; omega]
+# Solve the system
+sol = solve_ivp(inverted_pendulum, t_span, x0,t_eval = np.linspace(0, 1, 1000), method='RK45')
+
+# Plot the results
+plt.figure()
+plt.plot(sol.t, sol.y[0],'k', label = 'x (m)')
+plt.plot(sol.t, sol.y[1], '-.k', label = r'$\theta$ (rad)')
+plt.xlabel('Time (sec)')
+plt.ylabel('State variables')
+plt.legend()
+plt.grid()
+plt.title('Inverted Pendulum System Response')
+plt.show()

Chapter3/Chapter3/inverted_pendulum/inverted_pendulum.ipynb

@@ -0,0 +1,46 @@
+{
+  "nbformat": 4,
+  "nbformat_minor": 0,
+  "metadata": {
+    "colab": {
+      "provenance": []
+    },
+    "kernelspec": {
+      "name": "python3",
+      "display_name": "Python 3"
+    },
+    "language_info": {
+      "name": "python"
+    }
+  },
+  "cells": [
+    {
+      "cell_type": "code",
+      "execution_count": 1,
+      "metadata": {
+        "id": "C8SX677qxGVN"
+      },
+      "outputs": [],
+      "source": [
+        "def inverted_pendulum(t, x):\n",
+        "    g = 9.8\n",
+        "    l = 1\n",
+        "    m = 1\n",
+        "    M = 1\n",
+        "\n",
+        "    d1 = M + m * (1 - np.cos(x[1])**2)\n",
+        "    d2 = l * d1\n",
+        "\n",
+        "    F = 0  # No input\n",
+        "\n",
+        "    dxdt = np.zeros(4)\n",
+        "    dxdt[0] = x[2]\n",
+        "    dxdt[1] = x[3]\n",
+        "    dxdt[2] = (F + m * l * x[3]**2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1\n",
+        "    dxdt[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\n",
+        "\n",
+        "    return dxdt"
+      ]
+    }
+  ]
+}

Chapter3/Chapter3/inverted_pendulum/inverted_pendulum.py

@@ -0,0 +1,27 @@
+# -*- coding: utf-8 -*-
+"""inverted_pendulum.ipynb
+
+Automatically generated by Colab.
+
+Original file is located at
+    https://colab.research.google.com/drive/1PENBSi2mj9KfvFKPHHVcz9qx76lBY1ct
+"""
+
+def inverted_pendulum(t, x):
+    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
+
+    dxdt = np.zeros(4)
+    dxdt[0] = x[2]
+    dxdt[1] = x[3]
+    dxdt[2] = (F + m * l * x[3]**2 * np.sin(x[1]) - m * g * np.sin(x[1]) * np.cos(x[1])) / d1
+    dxdt[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 dxdt

Chapter3/Chapter3/robot_model/robot_model.ipynb

@@ -0,0 +1,66 @@
+{
+  "nbformat": 4,
+  "nbformat_minor": 0,
+  "metadata": {
+    "colab": {
+      "provenance": []
+    },
+    "kernelspec": {
+      "name": "python3",
+      "display_name": "Python 3"
+    },
+    "language_info": {
+      "name": "python"
+    }
+  },
+  "cells": [
+    {
+      "cell_type": "code",
+      "execution_count": 2,
+      "metadata": {
+        "id": "ZKtZsRte1jiO"
+      },
+      "outputs": [],
+      "source": [
+        "import numpy as np\n",
+        "# Define robot model function\n",
+        "def robot_model(t, x):\n",
+        "    # Robot parameters\n",
+        "    g = 9.81\n",
+        "    l1 = 1\n",
+        "    l2 = 0.5\n",
+        "    m1 = 2\n",
+        "    m2 = 1\n",
+        "    I1 = 1e-2\n",
+        "    I2 = 5e-3\n",
+        "    D = 2\n",
+        "\n",
+        "    # Mass matrix (M)\n",
+        "    M = np.array([[m1*(l1/2)**2 + m2*(l1**2 + (l2/2)**2) + m2*l1*l2*np.cos(x[1]) + I1 + I2,\n",
+        "                   m2*(l2/2)**2 + 0.5*m2*l1*l2*np.cos(x[1]) + I2],\n",
+        "                  [m2*(l2/2)**2 + 0.5*m2*l1*l2*np.cos(x[1]) + I2,\n",
+        "                   m2*(l2/2)**2 + I2]])\n",
+        "\n",
+        "    # Coriolis and centrifugal terms (V)\n",
+        "    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],\n",
+        "                  [-0.5*m2*l1*l2*np.sin(x[1])*x[2]*x[3]]])\n",
+        "\n",
+        "    # Gravitational terms (G)\n",
+        "    G = np.array([[ (m1*l1/2 + m2*l1)*g*np.cos(x[0]) + m2*g*l2/2*np.cos(x[0] + x[1])],\n",
+        "                  [ m2*g*l2/2*np.cos(x[0] + x[1])]])\n",
+        "\n",
+        "    # Input (Q) - currently no external torques\n",
+        "    Q = np.array([[-D*x[2]],  # Damping term for joint 1\n",
+        "                  [-D*x[3]]])  # Damping term for joint 2\n",
+        "\n",
+        "    # System dynamics\n",
+        "    xy = np.linalg.pinv(M) @ (Q - V - G)\n",
+        "\n",
+        "    # Output - angular velocities and accelerations\n",
+        "    xp = np.vstack((x[2:], xy.flatten()))\n",
+        "\n",
+        "    return xp.flatten()\n"
+      ]
+    }
+  ]
+}

Chapter3/Chapter3/robot_model/robot_model.py

@@ -0,0 +1,47 @@
+# -*- coding: utf-8 -*-
+"""robot_model.ipynb
+
+Automatically generated by Colab.
+
+Original file is located at
+    https://colab.research.google.com/drive/11g2x9Re3j6hFtHCNM_XnZiTL7LTW7s0r
+"""
+
+import numpy as np
+# Define robot model function
+def robot_model(t, x):
+    # Robot 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 terms (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 terms (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 (Q) - currently no external torques
+    Q = np.array([[-D*x[2]],  # Damping term for joint 1
+                  [-D*x[3]]])  # Damping term for joint 2
+
+    # System dynamics
+    xy = np.linalg.pinv(M) @ (Q - V - G)
+
+    # Output - angular velocities and accelerations
+    xp = np.vstack((x[2:], xy.flatten()))
+
+    return xp.flatten()