F(tried_random_motion).ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib qt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "QUESTION F "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Ishanya 21329 \n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.patches as mpatches\n",
    "import numpy as np\n",
    "\n",
    "# VTOL Parameter File\n",
    "\n",
    "# Physical parameters of the VTOL known to the controller\n",
    "mc = 1.0  # kg\n",
    "mr = 0.2  # kg\n",
    "Jc = 0.01  # kg m^2\n",
    "d = 0.1  # m\n",
    "mu = 0.1  # kg/s\n",
    "g = 9.81  # m/s^2\n",
    "F_wind = 0.0  # Wind disturbance force is zero initially\n",
    "\n",
    "# Parameters for animation\n",
    "length = 10.0\n",
    "\n",
    "# Initial Conditions\n",
    "z0 = 4.0  # Initial lateral position (m)\n",
    "h0 = 0.0  # Initial altitude (m)\n",
    "theta0 = 0.0  # Initial roll angle (rad)\n",
    "zdot0 = 0.0  # Initial lateral velocity (m/s)\n",
    "hdot0 = 0.0  # Initial climb rate (m/s)\n",
    "thetadot0 = 0.0  # Initial roll rate (rad/s)\n",
    "target0 = 5.5  # Initial target position (m)\n",
    "\n",
    "# Simulation Parameters\n",
    "t_start = 0.0  # Start time of simulation\n",
    "t_end = 10.0  # End time of simulation\n",
    "Ts = 0.01  # Sample time for simulation\n",
    "t_plot = 0.1  # The plotting and animation is updated at this rate\n",
    "\n",
    "# Saturation limits\n",
    "fmax = 5.0  # Max Force, N\n",
    "\n",
    "# Equilibrium force\n",
    "Fe = (mc + 2 * mr) * g\n",
    "\n",
    "# Mixing matrix\n",
    "mixing = np.linalg.inv(np.array([[1.0, 1.0], [d, -d]]))\n",
    "\n",
    "\n",
    "class VTOLAnimation:\n",
    "    def __init__(self):\n",
    "        self.flagInit = True\n",
    "        self.fig, self.ax = plt.subplots()\n",
    "        self.handle = []\n",
    "        self.length = length\n",
    "        plt.plot([0.0, length], [0.0, 0.0], 'k')\n",
    "        plt.axis([-length / 5, length + length / 5, -length / 5, length + length / 5])\n",
    "        self.time = 0.0  # Initialize time for circular motion\n",
    "\n",
    "    def update(self, x, target=0.0):\n",
    "        z = x.item(0)\n",
    "        h = x.item(1)\n",
    "        theta = x.item(2)\n",
    "\n",
    "        self.drawVehicle(z, h, theta)\n",
    "        self.drawTarget(target)\n",
    "\n",
    "        if self.flagInit == True:\n",
    "            self.flagInit = False\n",
    "\n",
    "    def drawVehicle(self, z, h, theta):\n",
    "        x1 = 0.2  # Increased size\n",
    "        x2 = 0.6  # Increased size\n",
    "        x3 = 0.8  # Increased size\n",
    "        y1 = 0.1  # Increased size\n",
    "        y2 = 0.02  # Increased size\n",
    "        pts = np.array([\n",
    "            [x1, y1],\n",
    "            [x1, 0],\n",
    "            [x2, 0],\n",
    "            [x2, y2],\n",
    "            [x3, y2],\n",
    "            [x3, -y2],\n",
    "            [x2, -y2],\n",
    "            [x2, 0],\n",
    "            [x1, 0],\n",
    "            [x1, -y1],\n",
    "            [-x1, -y1],\n",
    "            [-x1, 0],\n",
    "            [-x2, 0],\n",
    "            [-x2, -y2],\n",
    "            [-x3, -y2],\n",
    "            [-x3, y2],\n",
    "            [-x2, y2],\n",
    "            [-x2, 0],\n",
    "            [-x1, 0],\n",
    "            [-x1, y1],\n",
    "            [x1, y1]]).T\n",
    "        R = np.array([\n",
    "            [np.cos(theta), np.sin(theta)],\n",
    "            [-np.sin(theta), np.cos(theta)],\n",
    "        ])\n",
    "        pts = R.T @ pts\n",
    "        pts = pts + np.tile(np.array([[z], [h]]), (1, pts.shape[1]))\n",
    "        xy = np.array(pts.T)\n",
    "\n",
    "        if self.flagInit == True:\n",
    "            self.handle.append(mpatches.Polygon(xy, facecolor='lightblue', edgecolor='black'))\n",
    "            self.ax.add_patch(self.handle[0])\n",
    "        else:\n",
    "            self.handle[0].set_xy(xy)\n",
    "\n",
    "    def drawTarget(self, target=0.0):\n",
    "        w = 0.2  # Increased size\n",
    "        h = 0.1  # Increased size\n",
    "        radius = 1.0  # Radius of the circular path\n",
    "        self.time += Ts  # Increment time for circular motion\n",
    "        x_target = radius * np.cos(self.time) + target  # Circular path in x-direction\n",
    "        y_target = radius * np.sin(self.time)  # Circular path in y-direction\n",
    "        pts = np.matrix([\n",
    "            [x_target + w / 2.0, y_target + h],\n",
    "            [x_target + w / 2.0, y_target],\n",
    "            [x_target - w / 2.0, y_target],\n",
    "            [x_target - w / 2.0, y_target + h],\n",
    "            [x_target + w / 2.0, y_target + h]])\n",
    "\n",
    "        if self.flagInit == True:\n",
    "            self.handle.append(mpatches.Polygon(pts, facecolor='green', edgecolor='black'))\n",
    "            self.ax.add_patch(self.handle[1])\n",
    "        else:\n",
    "            self.handle[1].set_xy(pts)\n",
    "\n",
    "\n",
    "def main():\n",
    "    animation = VTOLAnimation()\n",
    "    t = np.arange(0.0, t_end / 4, Ts)\n",
    "    x = np.array([z0, h0 + 0.1, theta0])\n",
    "    animation.update(x, target0)\n",
    "    plt.pause(10)\n",
    "\n",
    "    for ti in t:\n",
    "        radius = 1.0  # Radius of the circular path\n",
    "        x_target = radius * np.cos(ti) + target0  # Circular path in x-direction\n",
    "        y_target = radius * np.sin(ti)  # Circular path in y-direction\n",
    "        x = np.array([x_target, h0 + 0.1 + y_target, theta0 + ti / 100])\n",
    "        target = target0\n",
    "        animation.update(x, target)\n",
    "        plt.pause(0.01)\n",
    "\n",
    "    plt.show()\n",
    "\n",
    "\n",
    "main()\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}