{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "f96fd41d",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "# ATRVjr Driving\n",
    "\n",
    "In this notebook we import the ATRVjr rover model into **kdflex** and drive it around using a simple manual applied torque approach. Contact between the wheels and the ground are currently handled using `CoalScene` which is a wrapper of the open source [Coal](https://github.com/coal-library/coal) project.\n",
    "\n",
    "Requirements:\n",
    "- [2-link Pendulum Collision](../example_pendulum_collision/notebook.ipynb)\n",
    "- [Import/export](../example_import_export/notebook.ipynb)\n",
    "\n",
    "In this tutorial we will:\n",
    "- Import and modify the URDF\n",
    "- Setup the **kdFlex** scene\n",
    "- Attach wheel geometries\n",
    "- Setup StatePropagator and models\n",
    "- Setup the Simulation\n",
    "- Run and clean up\n",
    "\n",
    "**Acknowledgements**  \n",
    "The robot URDF model included in this project is Copyright (c) 2018 diasdm and licensed under the BSD 2‑Clause License [(link)](https://github.com/diasdm/atrvjr_description.git). Some modifications were made to adapt the model for use within the kdflex simulation framework, including simplifying wheel geometries due to current rendering and collision handling limitations.\n",
    "\n",
    " For a more in-depth descriptions of **kdflex** concepts see [usage](usage_page)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "dce602b8",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "import atexit\n",
    "import numpy as np\n",
    "from math import pi\n",
    "from pathlib import Path\n",
    "\n",
    "import Karana.Core as kc\n",
    "import Karana.Math as km\n",
    "import Karana.Integrators as ki\n",
    "import Karana.Frame as kf\n",
    "import Karana.Dynamics as kd\n",
    "import Karana.Dynamics.SOADyn_types as kdt\n",
    "import Karana.Scene.Scene_types as kst\n",
    "import Karana.Scene as ks\n",
    "import Karana.Models as kmdl\n",
    "import Karana.Collision as kcoll\n",
    "from Karana.KUtils.BasicPrefab import BasicPrefabDS, BasicPrefab\n",
    "from Karana.KUtils.DataStruct import DataStruct\n",
    "from Karana.KUtils.Sim import Sim"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bcb35c5b",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "Build the filepath to the URDF file we will import"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "fd13f1bc",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "resources = Path().absolute().parent / \"resources\"\n",
    "urdf_file = resources / \"atrvjr\" / \"atrvjr.urdf\""
   ]
  },
  {
   "cell_type": "markdown",
   "id": "64ddd559",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Import and Modify the URDF\n",
    "\n",
    "Import the URDF file to create a SOADyn_types.SubGraphDS. The \"DS\" is short for \"Data Structure\". This is kdFlex's native, file-format agnostic way of specifying a multibody. We'll use this later on to instantiate the multibody.\n",
    "\n",
    "It's easier to edit the model while it's in the SubGraphDS format before we convert it to a Multibody. In the cell below we the original hinge between the rover and the root frame to a full 6-DOF hinge, which allows it to move freely. We then convert the hinges between the wheel bodies to pin joints so the wheels can properly rotate.\n",
    "\n",
    "Last we set all the visuals in the SubGraphDS to belong to the graphics layer only so that it can't interact with the wheels which we add later or the ground."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "c6ce1a4a",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "sim_info = BasicPrefabDS.fromFile(urdf_file)\n",
    "mb_info = sim_info.params.subtree\n",
    "\n",
    "mb_info.model_dump()\n",
    "\n",
    "# convert to full6DOF hinge between root frame and rover.\n",
    "mb_info.base_bodies[0].body.hinge = kdt.HingeDS(\n",
    "    hinge_type=kd.HingeType.FULL6DOF,\n",
    "    subhinges=[\n",
    "        kdt.Linear3SubhingeDS(prescribed=False),\n",
    "        kdt.SphericalSubhingeDS(prescribed=False),\n",
    "    ],\n",
    ")\n",
    "\n",
    "# unlock locked Hinges for wheels from the URDF\n",
    "for i in [0, 1]:\n",
    "    mb_info.base_bodies[0].children[i].body.hinge = kdt.HingeDS(\n",
    "        hinge_type=kd.HingeType.REVOLUTE,\n",
    "        subhinges=[kdt.PinSubhingeDS(prescribed=False, unit_axis=np.array([0.0, 0.0, 1.0]))],\n",
    "    )\n",
    "for i in [2, 3]:\n",
    "    mb_info.base_bodies[0].children[i].body.hinge = kdt.HingeDS(\n",
    "        hinge_type=kd.HingeType.REVOLUTE,\n",
    "        subhinges=[kdt.PinSubhingeDS(prescribed=False, unit_axis=np.array([0.0, 0.0, -1.0]))],\n",
    "    )\n",
    "\n",
    "\n",
    "# Remove all collision meshes. We want to add these manually.\n",
    "def removeCollision():\n",
    "    \"\"\"Remove all the collision meshes.\"\"\"\n",
    "\n",
    "    def _removeParts(parts: list[kst.ScenePartSpecDS]):\n",
    "        idx = []\n",
    "        for k, p in enumerate(parts):\n",
    "            if p.layers == ks.LAYER_COLLISION:\n",
    "                idx.append(k)\n",
    "        idx.reverse()\n",
    "        for k in idx:\n",
    "            parts.pop(k)\n",
    "\n",
    "    def _recurse(b: kdt.BodyWithContextDS):\n",
    "        _removeParts(b.body.scene_parts)\n",
    "        for c in b.children:\n",
    "            _recurse(c)\n",
    "\n",
    "    for base in mb_info.base_bodies:\n",
    "        _recurse(base)\n",
    "\n",
    "\n",
    "removeCollision()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4fd06632",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "Instantiate and configure the {py:class}`Karana.Dynamics.Multibody` based on the SubGraphDS description."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "8e99d2b9",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "sim, _ = Sim.fromBasicPrefabDS(sim_info)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3c4aeea4",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "Finalize the model to get it ready for use."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "115f56a0",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "LEGEND: [<hinge type[/dofs]> <prescribed subhinges>] <body name>[/num embedded bodies > 0] <flex dofs > 0>\n",
      "|-atrvjr_MBVROOT_\n",
      "   |-[FULL6DOF] center_link\n",
      "      |-[REVOLUTE] left_front_wheel_link\n",
      "      |-[REVOLUTE] left_rear_wheel_link\n",
      "      |-[REVOLUTE] right_front_wheel_link\n",
      "      |-[REVOLUTE] right_rear_wheel_link\n",
      "      |-[LOCKED] cbimu_frame\n",
      "      |-[LOCKED] mpuimu_frame\n",
      "      |-[LOCKED] arm_base_frame\n",
      "      |-[LOCKED] lrf_frame\n",
      "      |-[LOCKED] novatel\n",
      "\n"
     ]
    }
   ],
   "source": [
    "sim.mb.ensureHealthy()\n",
    "sim.mb.resetData()\n",
    "sim.mb.dumpTree()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "97db71f8",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Setup the kdFlex Scene \n",
    "\n",
    "Next we setup **kdflex**'s visualizer by creating a graphics scene and a webserver to view it. We use the helper method setupGraphics() to take care of setting up the scene and webserver for us.\n",
    "\n",
    "See [Visualization](visualization_sec) and [Scene Layer](scene_layer_sec) for more information relating to this section."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "024e4376",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <div style=\"height:300px; resize:vertical; overflow: auto;\">\n",
       "          <iframe src=\"http://newton:42377\" style=\"width:100%; height:100%; border:0; display:block;\"></iframe>\n",
       "        </div>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "_, web_scene = sim.setupGraphics(port=0)\n",
    "web_scene.defaultCamera().pointCameraAt(\n",
    "    offset=np.array([[0.0], [5.0], [10.0]]),\n",
    "    target=np.array([[0.0], [0.0], [0.0]]),\n",
    "    up=np.array([[0.0], [0.0], [1.0]]),\n",
    ")\n",
    "proxy_scene = sim.mb.getScene()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "d6cc4149",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# attach a collision scene\n",
    "col_scene = ks.CoalScene(\"collision_scene\")\n",
    "proxy_scene.registerClientScene(col_scene, sim.mb.virtualRoot(), layers=ks.LAYER_COLLISION)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2b90d05e",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "Display the bodies in the multibody model."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "e9ab8bad",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[WebUI] Web server is running on port 42377\n",
      "You may be able to connect in your browser at:\n",
      "\t\u001b[1mhttp://newton:42377\u001b[0m\n",
      "\n",
      "LEGEND: <body number> <body name> <parent body> <hinge type> [prescribed subhinges] <U dofs/offset> [flex dofs/offset]\n",
      "\n",
      "      Body                     Parent              Hinge    Dofs          \n",
      "      ____                     ______              _____    ____          \n",
      "   1. center_link              atrvjr_MBVROOT_    FULL6DOF  6/0           \n",
      "   2. left_front_wheel_link    center_link        REVOLUTE  1/6           \n",
      "   3. left_rear_wheel_link     center_link        REVOLUTE  1/7           \n",
      "   4. right_front_wheel_link   center_link        REVOLUTE  1/8           \n",
      "   5. right_rear_wheel_link    center_link        REVOLUTE  1/9           \n",
      "   6. cbimu_frame              center_link         LOCKED   0/10          \n",
      "   7. mpuimu_frame             center_link         LOCKED   0/10          \n",
      "   8. arm_base_frame           center_link         LOCKED   0/10          \n",
      "   9. lrf_frame                center_link         LOCKED   0/10          \n",
      "  10. novatel                  center_link         LOCKED   0/10          \n",
      "                                                            ____          \n",
      "                                                            10            \n",
      "\n"
     ]
    }
   ],
   "source": [
    "sim.mb.displayModel()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2feb3376",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Attach Wheel Geometries"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "e4b0d6c6",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "wheel_geom = ks.CylinderGeometry(0.125, 0.075)\n",
    "\n",
    "# create a ground for the rover to drive on\n",
    "ground_geom = ks.BoxGeometry(30, 30, 0.25)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "79747949",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "left_front_wheel = sim.mb.getBody(\"left_front_wheel_link\")\n",
    "# left_front_wheel.addScenePartSpec(wheel_obj.parts[0])\n",
    "# left_front_wheel.addScenePartSpec(wheel_obj.parts[1])\n",
    "left_front_wheel_node = ks.ProxyScenePart(\n",
    "    name=\"left_front_wheel_link_node\", scene=proxy_scene, geometry=wheel_geom\n",
    ")\n",
    "left_front_wheel_node.attachTo(left_front_wheel)\n",
    "left_front_wheel_node.setTranslation([0, 0, 0.03])\n",
    "left_front_wheel_node.setUnitQuaternion(km.UnitQuaternion(pi / 2, [1.0, 0.0, 0.0]))\n",
    "# left_front_wheel_node.graphics().showAxes(1.0)\n",
    "\n",
    "left_rear_wheel = sim.mb.getBody(\"left_rear_wheel_link\")\n",
    "# left_rear_wheel.addScenePartSpec(wheel_obj.parts[0])\n",
    "# left_rear_wheel.addScenePartSpec(wheel_obj.parts[1])\n",
    "left_rear_wheel_node = ks.ProxyScenePart(\n",
    "    \"left_rear_wheel_link_node\", scene=proxy_scene, geometry=wheel_geom\n",
    ")\n",
    "left_rear_wheel_node.attachTo(left_rear_wheel)\n",
    "left_rear_wheel_node.setTranslation([0, 0, 0.03])\n",
    "left_rear_wheel_node.setUnitQuaternion(km.UnitQuaternion(pi / 2, [1.0, 0.0, 0.0]))\n",
    "# left_rear_wheel_node.graphics().showAxes(1.0)\n",
    "\n",
    "right_front_wheel = sim.mb.getBody(\"right_front_wheel_link\")\n",
    "# right_front_wheel.addScenePartSpec(wheel_obj.parts[0])\n",
    "# right_front_wheel.addScenePartSpec(wheel_obj.parts[1])\n",
    "right_front_wheel_node = ks.ProxyScenePart(\n",
    "    \"right_front_wheel_link_node\", scene=proxy_scene, geometry=wheel_geom\n",
    ")\n",
    "right_front_wheel_node.attachTo(right_front_wheel)\n",
    "right_front_wheel_node.setTranslation([0, 0, 0.03])\n",
    "right_front_wheel_node.setUnitQuaternion(km.UnitQuaternion(pi / 2, [1.0, 0.0, 0.0]))\n",
    "# right_front_wheel_node.graphics().showAxes(1.0)\n",
    "\n",
    "right_rear_wheel = sim.mb.getBody(\"right_rear_wheel_link\")\n",
    "# right_rear_wheel.addScenePartSpec(wheel_obj.parts[0])\n",
    "# right_rear_wheel.addScenePartSpec(wheel_obj.parts[1])\n",
    "right_rear_wheel_node = ks.ProxyScenePart(\n",
    "    \"right_rear_wheel_link_node\", scene=proxy_scene, geometry=wheel_geom\n",
    ")\n",
    "right_rear_wheel_node.attachTo(right_rear_wheel)\n",
    "right_rear_wheel_node.setTranslation([0, 0, 0.03])\n",
    "right_rear_wheel_node.setUnitQuaternion(km.UnitQuaternion(pi / 2, [1.0, 0.0, 0.0]))\n",
    "# right_rear_wheel_node.graphics().showAxes(1.0)\n",
    "\n",
    "mat_info = ks.PhysicalMaterialInfo()\n",
    "mat_info.color = ks.Color.WHITESMOKE\n",
    "gray = ks.PhysicalMaterial(mat_info)\n",
    "\n",
    "ground = ks.ProxyScenePart(\n",
    "    \"ground\", scene=proxy_scene, geometry=ground_geom, material=gray, layers=ks.LAYER_ALL\n",
    ")\n",
    "ground.setTranslation([0, 0, -0.1])\n",
    "\n",
    "del ground, mat_info, wheel_geom, ground_geom, gray"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7116fecb",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Setup Integrator and models"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "27ce425f",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# set up state propagator\n",
    "sim.sp.setIntegrator(ki.IntegratorType.RK4)\n",
    "\n",
    "# Create a UniformGravity model, and set it's gravitational acceleration.\n",
    "ug = kmdl.Gravity(\"grav_model\", sim.sp, kmdl.UniformGravity(\"uniform_gravity\"), sim.mb)\n",
    "ug.getGravityInterface().setGravity(np.array([0, 0, -9.81]), 0.0, kmdl.OutputUpdateType.PRE_HOP)\n",
    "del ug\n",
    "\n",
    "# sync the simulation time with real-time.\n",
    "kmdl.SyncRealTime(\"sync_real_time\", sim.sp, 1.0)\n",
    "\n",
    "# add penalty contact model and register its bodies and parameters\n",
    "hc = kcoll.HuntCrossley(\"hunt_crossley_contact\")\n",
    "hc.params.kp = 100000\n",
    "hc.params.kc = 20000\n",
    "hc.params.mu = 0.3\n",
    "hc.params.n = 1.5\n",
    "hc.params.linear_region_tol = 1e-3\n",
    "kmdl.PenaltyContact(\n",
    "    \"penalty_contact\", sim.sp, sim.mb, [kcoll.FrameCollider(proxy_scene, col_scene)], hc\n",
    ")\n",
    "del col_scene, hc"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2791a2fa",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "Because forces are cleared out in between simulation steps, we create a callback function to apply forces depending on the intended direction. We attach it as a {py:attr}`Karana.Dynamics.SpFunctions.pre_deriv_fns`, since it is called before the multibody derivative which ensures the forces are used in the state update calculations."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "a63bd6fd",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "direction = \"coast\"\n",
    "\n",
    "\n",
    "def pre_deriv_fn(t, x):\n",
    "    \"\"\"Print out the time and state.\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    t : float\n",
    "        The current time.\n",
    "    x : NDArray[np.float64]\n",
    "       The current state.\n",
    "    \"\"\"\n",
    "    # manually set torques in pre_deriv function\n",
    "    global direction\n",
    "    match direction:\n",
    "        case \"coast\":\n",
    "            right_rear_wheel.parentHinge().subhinge(0).setT([0])\n",
    "            left_rear_wheel.parentHinge().subhinge(0).setT([0])\n",
    "            right_front_wheel.parentHinge().subhinge(0).setT([0])\n",
    "            left_front_wheel.parentHinge().subhinge(0).setT([0])\n",
    "        case \"forward\":\n",
    "            right_rear_wheel.parentHinge().subhinge(0).setT([4])\n",
    "            left_rear_wheel.parentHinge().subhinge(0).setT([4])\n",
    "            right_front_wheel.parentHinge().subhinge(0).setT([4])\n",
    "            left_front_wheel.parentHinge().subhinge(0).setT([4])\n",
    "        case \"backward\":\n",
    "            right_rear_wheel.parentHinge().subhinge(0).setT([-4])\n",
    "            left_rear_wheel.parentHinge().subhinge(0).setT([-4])\n",
    "            right_front_wheel.parentHinge().subhinge(0).setT([-4])\n",
    "            left_front_wheel.parentHinge().subhinge(0).setT([-4])\n",
    "        case \"rotate_right\":\n",
    "            right_rear_wheel.parentHinge().subhinge(0).setT([-5])\n",
    "            left_rear_wheel.parentHinge().subhinge(0).setT([5])\n",
    "            right_front_wheel.parentHinge().subhinge(0).setT([-5])\n",
    "            left_front_wheel.parentHinge().subhinge(0).setT([5])\n",
    "        case \"rotate_left\":\n",
    "            right_rear_wheel.parentHinge().subhinge(0).setT([5])\n",
    "            left_rear_wheel.parentHinge().subhinge(0).setT([-5])\n",
    "            right_front_wheel.parentHinge().subhinge(0).setT([5])\n",
    "            left_front_wheel.parentHinge().subhinge(0).setT([-5])\n",
    "\n",
    "\n",
    "sim.sp.fns.pre_deriv_fns[\"apply_torque\"] = pre_deriv_fn"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "084579aa",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Setup the Simulation\n",
    "Finish by setting the initial states and timed events"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "1817817a",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [],
   "source": [
    "# set StatePropagator state\n",
    "sim.sp.hardReset()\n",
    "sim.sp.ensureHealthy()\n",
    "t_init = np.timedelta64(0, \"ns\")\n",
    "x_init = sim.sp.assembleState()\n",
    "sim.sp.setTime(t_init)\n",
    "sim.sp.setState(x_init)\n",
    "\n",
    "# syncs up the graphics\n",
    "proxy_scene.update()\n",
    "\n",
    "# register timed event\n",
    "h = np.timedelta64(int(1e7), \"ns\")\n",
    "t = kd.TimedEvent(\"hop_size\", h, lambda _: None, False)\n",
    "t.period = h\n",
    "sim.sp.registerTimedEvent(t)\n",
    "del t"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7a95b814",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Run and Clean Up"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "e97e5f37",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<SpStatusEnum.REACHED_END_TIME: 1>"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "direction = \"coast\"\n",
    "sim.sp.advanceBy(5.0)\n",
    "\n",
    "direction = \"forward\"\n",
    "sim.sp.advanceBy(3.0)\n",
    "\n",
    "direction = \"backward\"\n",
    "sim.sp.advanceBy(6.0)\n",
    "\n",
    "direction = \"forward\"\n",
    "sim.sp.advanceBy(3.0)\n",
    "\n",
    "direction = \"rotate_right\"\n",
    "sim.sp.advanceBy(3.0)\n",
    "\n",
    "direction = \"rotate_left\"\n",
    "sim.sp.advanceBy(1.5)\n",
    "\n",
    "direction = \"forward\"\n",
    "sim.sp.advanceBy(4.0)\n",
    "\n",
    "direction = \"backward\"\n",
    "sim.sp.advanceBy(4.0)\n",
    "\n",
    "direction = \"rotate_right\"\n",
    "sim.sp.advanceBy(3.0)\n",
    "\n",
    "direction = \"rotate_left\"\n",
    "sim.sp.advanceBy(1.5)\n",
    "\n",
    "direction = \"forward\"\n",
    "sim.sp.advanceBy(4.0)\n",
    "\n",
    "direction = \"backward\"\n",
    "sim.sp.advanceBy(4.0)\n",
    "\n",
    "direction = \"rotate_right\"\n",
    "sim.sp.advanceBy(3.0)\n",
    "\n",
    "direction = \"rotate_left\"\n",
    "sim.sp.advanceBy(1.5)\n",
    "\n",
    "direction = \"forward\"\n",
    "sim.sp.advanceBy(4.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "7d1bfbcc",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<function __main__.cleanup()>"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Cleanup\n",
    "def cleanup():\n",
    "    \"\"\"Cleanup the simulation.\"\"\"\n",
    "    global \\\n",
    "        left_front_wheel, \\\n",
    "        left_front_wheel_node, \\\n",
    "        left_rear_wheel, \\\n",
    "        left_rear_wheel_node, \\\n",
    "        right_rear_wheel, \\\n",
    "        right_rear_wheel_node, \\\n",
    "        right_front_wheel, \\\n",
    "        right_front_wheel_node, \\\n",
    "        proxy_scene, \\\n",
    "        mb_info, \\\n",
    "        sim_info, \\\n",
    "        web_scene, \\\n",
    "        _, \\\n",
    "        sim\n",
    "    del (\n",
    "        left_front_wheel,\n",
    "        left_front_wheel_node,\n",
    "        left_rear_wheel,\n",
    "        left_rear_wheel_node,\n",
    "        right_rear_wheel,\n",
    "        right_rear_wheel_node,\n",
    "        right_front_wheel,\n",
    "        right_front_wheel_node,\n",
    "        proxy_scene,\n",
    "        web_scene,\n",
    "        mb_info,\n",
    "        sim_info,\n",
    "        _,\n",
    "        sim,\n",
    "    )\n",
    "\n",
    "\n",
    "atexit.register(cleanup)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bdfe83ae",
   "metadata": {
    "jupyter": {
     "outputs_hidden": false
    }
   },
   "source": [
    "## Summary\n",
    "You now understand how to drive a ATRVjr model by applying torque forces in the PreDeriv call."
   ]
  }
 ],
 "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.3"
  },
  "name": "notebook.ipynb"
 },
 "nbformat": 4,
 "nbformat_minor": 5
}