4-Controller.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "bf90bd1f-dc8d-4552-bf78-1e123c184657",
   "metadata": {},
   "source": [
    "import numpy as np\n",
    "import scipy as sp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "47c62ee5-09a2-4626-8ef1-6120077e61f1",
   "metadata": {},
   "outputs": [],
   "source": [
    "from quadrotor.dynamics import QuadrotorDynamicsBase, QuadrotorState\n",
    "from quadrotor.controller import ControllerBase, QuadrotorCommands\n",
    "from quadrotor.trajectory import TrajectoryBase, TrajectoryState"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "56fed8cc-e617-41c6-8eb0-6a13c03ab833",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sym import Rot3\n",
    "\n",
    "def dRot3(R: Rot3, omega: np.ndarray) -> Rot3:\n",
    "    \"\"\"\n",
    "    NOTE(roy): If interested, we can discuss the math here later. There are other\n",
    "    ways of doing this but to keep the ode_int formulation, we specifically require\n",
    "    dq/dt. Refer to 'Baseile Graf - Quaternions and Dynamics' or (for intuition)\n",
    "    https://math.stackexchange.com/questions/1896379/how-to-use-the-quaternion-derivative\n",
    "    \"\"\"\n",
    "    quat = np.array(R.to_storage())  # Convert to raw quaternion\n",
    "    (q0, q1, q2, q3) = quat  # xyzw storage, see https://symforce.org/api-gen-cpp/class/classsym_1_1Rot3.html\n",
    "\n",
    "    G = np.array([[ q3,  q2, -q1, -q0],\n",
    "                [-q2,  q3,  q0, -q1],\n",
    "                [ q1, -q0,  q3, -q2]])\n",
    "    quat_dot = (G.T @ omega) / 2\n",
    "    return Rot3.from_storage(quat_dot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "789f07d7-e28a-439f-b71e-94862ce52596",
   "metadata": {},
   "outputs": [],
   "source": [
    "from dataclasses import dataclass, field\n",
    "\n",
    "@dataclass\n",
    "class Params:\n",
    "    \"\"\"A little utility class to hold our quadrotor parameters\"\"\"\n",
    "    mass: float = 1.352  # [kg]\n",
    "\n",
    "    # NOTE(roy) this is a matrix now as discussed!\n",
    "    # NOTE(roy) don't get scared by the field/default_factory thing! It's just a way to do default field initialization for dataclasses,\n",
    "    # see https://docs.python.org/3/library/dataclasses.html. Don't worry about it!\n",
    "    inertia: np.ndarray = field(default_factory=lambda: np.array([\n",
    "        [9.8e-3, 0, 0],\n",
    "        [0, 10.02e-3, 0],\n",
    "        [0, 0, 18.6e-3],\n",
    "    ]))\n",
    "\n",
    "    rotor_diameter: float = 10 * 0.0254  # [m] 10 inches\n",
    "\n",
    "    static_thrust_coefficient: float = 0.14553  # [-] Thrust coefficient\n",
    "    static_torque_coefficient: float = 0.01047  # [-] Torque coefficient\n",
    "\n",
    "    # NOTE(roy) assume this is the same front-to-back and left-to-right\n",
    "    arm_length: float = 0.3814 / 2.0  # [m] Arm length\n",
    "\n",
    "    # Universal constants\n",
    "    g: float = 9.80665  # [m/s2] Gravity\n",
    "    rho: float = 1.225  # [kg/m3] Sea-level density\n",
    "\n",
    "    def rotor_model(self, static_coefficient: float) -> float:\n",
    "        # NOTE(roy) I'm just including this for completeness sake, this means we can e.g. see how air density\n",
    "        # influences our simulation. Refer to this source for derivation:\n",
    "        # https://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/node86.html\n",
    "        return self.rho * static_coefficient * (self.rotor_diameter ** 4) / (4 * np.pi ** 2)\n",
    "\n",
    "    # NOTE(roy) if you're unsure what @property decorators do: they make it so that we can call k_thrust as if it was just a class\n",
    "    # variable. I.e. we'd write params.k_thrust now, rather than params.k_thrust()\n",
    "    @property\n",
    "    def k_thrust(self):\n",
    "        return self.rotor_model(self.static_thrust_coefficient)\n",
    "\n",
    "    @property\n",
    "    def k_torque(self):\n",
    "        return self.rotor_model(self.static_torque_coefficient)\n",
    "\n",
    "# Simply treat as a global\n",
    "p = Params()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4bcc7644-032e-4c48-bd4b-83791954e4c6",
   "metadata": {},
   "source": [
    "### Writing a controller for our drone\n",
    "In this notebook, we'll start writing a feedback controller for our model! Again, I will be leaving you guys alone a little bit more.\n",
    "\n",
    "#### Recommended steps\n",
    "- Copy your 3D quadrotor model from the previous lecture\n",
    "- Work your way down in the `Controller` class\n",
    "    - I implemented a basic structure, but feel free to diverge from this if you like!\n",
    "    - Write the inverse mixing matrix formulation to from **u** to rotor rates\n",
    "    - As discussed, the first week I expect you to just set up control in *z* to warm up!"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "937c0d60-2ee6-4e32-9db9-36121e53cf1a",
   "metadata": {},
   "source": [
    "### The dynamics"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0b26efd2-d381-4491-9751-38b510e08139",
   "metadata": {},
   "outputs": [],
   "source": [
    "# NOTE(roy) Copy your 3D Quadrotor here!"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3566ef9f-ba46-4888-bccd-e2310f724cea",
   "metadata": {},
   "source": [
    "### The controller"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d97b6cd4-7870-4f30-af64-184148bd4265",
   "metadata": {},
   "outputs": [],
   "source": [
    "@dataclass\n",
    "class ControllerParams:\n",
    "    \"\"\"The same as above but for the controller gains\"\"\"\n",
    "\n",
    "    # NOTE(roy) diag just means we get a matrix with these values on the diagonal,\n",
    "    # so these are 3x3 matrices (print them if you're unsure!)\n",
    "    # We use this formulation because it makes the math a bit more readable later on!\n",
    "    K_p: np.ndarray = field(default_factory=lambda: np.diag([0, 0, 100]))\n",
    "    K_d: np.ndarray = field(default_factory=lambda: np.diag([0, 0, 20]))\n",
    "\n",
    "    # Min/max rotor rates!\n",
    "    rotor_rate_min: float = 180\n",
    "    rotor_rate_max: float = 600\n",
    "\n",
    "controller_p = ControllerParams()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "237e1331-e9ac-46f8-96b3-a4a24c2b031e",
   "metadata": {},
   "outputs": [],
   "source": [
    "class Controller(ControllerBase):\n",
    "    @staticmethod\n",
    "    def rotor_rates_from_u(u: np.ndarray) -> np.ndarray:\n",
    "        # NOTE(roy) implement your conversion from u (so that's [u1, u2_1, u2_2, u2_3] like we discussed, a 4-sized vector)\n",
    "        # to rotor rates (which is also a four-sized vector)\n",
    "\n",
    "        # NOTE(roy) the steps here are:\n",
    "        #  1. convert u -> forces with the inverse mixing matrix\n",
    "        #  2. convert forces -> rotor rates by inverting the rotor rate model\n",
    "        #  3. limit the rotor rates with rotor_rate_min/rotor_rate_max above!\n",
    "        raise NotImplementedError(\"rotor_rates_from_u not implemented!\")\n",
    "\n",
    "    def step(self, t: float, trajectory: TrajectoryState, state: QuadrotorState) -> QuadrotorCommands:\n",
    "        desired_position, desired_velocity = trajectory.position, trajectory.velocity\n",
    "        actual_position, actual_velocity = state.position, state.velocity\n",
    "        \n",
    "        # NOTE(roy) first compute the desired acceleration using the equation from the lectures!\n",
    "        # I already extracted the values you need to compute it above\n",
    "        commanded_acceleration = np.zeros(3)  # Your equation here!\n",
    "\n",
    "        # NOTE(roy) now compute u_1 from the commanded acceleration using the linearized equation from the lectures!\n",
    "        u_1 = 0  # Your equation here!\n",
    "\n",
    "        # NOTE(roy) we're just doing thrust this week, set u_2 to just be zeros!\n",
    "        u_2 = np.zeros(3)\n",
    "        u = np.array([u_1, *u_2])\n",
    "\n",
    "        rotor_rates = Controller.rotor_rates_from_u(u)\n",
    "        return QuadrotorCommands(rotor_rates)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b21933dc-327e-475f-9547-ea88ddefef3d",
   "metadata": {},
   "source": [
    "### The trajectory\n",
    "Because we're now finally doing closed-loop control, we also need to set desired states in the trajectory!\n",
    "I've set up something simple, please mess around with it!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "db8f15e4-ff49-420b-8c8b-c23693b1f49a",
   "metadata": {},
   "outputs": [],
   "source": [
    "class JumpTrajectory(TrajectoryBase):\n",
    "    def eval(self, t: float):\n",
    "        altitude = 0 if t < 1.0 else 1.0\n",
    "        position = np.array([0, 0, altitude])\n",
    "\n",
    "        return TrajectoryState(t, position=position)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1a3db211-6d28-4e56-aa1b-b5847bbd9157",
   "metadata": {},
   "source": [
    "### Running it!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e8238d97-8dbc-4c5a-99c2-afbf92d7ed6d",
   "metadata": {},
   "outputs": [],
   "source": [
    "from quadrotor.simulator import SimulatorBase, SimulatorState\n",
    "\n",
    "# This might be slow to run the first time!\n",
    "from quadrotor.renderer import animate_k3d"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a57398f9-538d-43e0-8345-906ac3734e7b",
   "metadata": {},
   "outputs": [],
   "source": [
    "def run_and_render(initial_state: QuadrotorState, t_total: int = 2.0):\n",
    "    sim = SimulatorBase(\n",
    "        dt=0.01,\n",
    "        dynamics=FullQuadrotorDynamics(),\n",
    "        controller=Controller(),\n",
    "        trajectory=JumpTrajectory(),\n",
    "        initial_state=initial_state,\n",
    "        t_total=t_total,\n",
    "    )\n",
    "    \n",
    "    # Run the simulator\n",
    "    output = sim.simulate()\n",
    "\n",
    "    # K3D renderer\n",
    "    plot = animate_k3d(output)\n",
    "    \n",
    "    plot.display()\n",
    "    plot.start_auto_play()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b7a1d6b7-b4dc-49c5-8fd0-df855a37cade",
   "metadata": {},
   "outputs": [],
   "source": [
    "# NOTE(roy) play with this variable and see what happens!\n",
    "initial_state = QuadrotorState(\n",
    "    position=np.zeros(3),\n",
    "    velocity=np.array([0, 0, 0]),\n",
    "    orientation=Rot3.from_yaw_pitch_roll(0, 0, 0),\n",
    "    angular_velocity=np.zeros(3),\n",
    ")\n",
    "\n",
    "run_and_render(initial_state, 5.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8c3a2631-3a31-4b58-ac0b-d3b7ffdcf5b9",
   "metadata": {},
   "source": [
    "### Looking at the controller behavior\n",
    "If all is well, the quadrotor should have a stable hover and after 1 second jump up 1 meter (with the `JumpTrajectory`)\n",
    "\n",
    "Try some experiments:\n",
    "- What happens if you increase/descrease `K_p`?\n",
    "- What happens if you increase/decrease `K_d`?\n",
    "- What happens if you use a slightly different mass in the controller (so the controller and dynamics have a different estimate for the quadrotor mass)? How do you think you could solve the behavior you see?"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "10af4693-4881-49d9-b457-5037273b3e8e",
   "metadata": {},
   "source": [
    "### Commit your updated notebook to your fork!"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.12.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}