jupyter/Sawppy Ackermann Math.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Sawppy Ackermann Math\n",
    "This Jupyter notebook works through the process of calculating the angle and velocity for every Sawppy wheel in response to a command to move the entire rover at a particular angular and linear velocity.\n",
    "\n",
    "##  Setup\n",
    "Sawppy chassis is represented by a list of six wheels, each listing a human-readable name and the X,Y coordinates relative to rover's center of motion."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 225,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Sawppy chassis geometry (in meters)\n",
    "SAWPPY_WHEELBASE_FRONT = 0.285\n",
    "SAWPPY_WHEELBASE_MID = 0\n",
    "SAWPPY_WHEELBASE_REAR = -0.257\n",
    "SAWPPY_TRACK_FRONT = 0.23\n",
    "SAWPPY_TRACK_MID = 0.26\n",
    "SAWPPY_TRACK_REAR = 0.23"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 226,
   "metadata": {},
   "outputs": [],
   "source": [
    "class ChassisWheel(object):\n",
    "    \"\"\"Information needed to calculate angle and speed for a specific wheel.\n",
    "    Axis orientation conforms to REP103. (+X is forward, +Y is left, +Z is up)\n",
    "    https://www.ros.org/reps/rep-0103.html\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self, name, offset_front, offset_left):\n",
    "        \"\"\"Initialize a chassis wheel instance with X & Y relative to center.\n",
    "        Args:\n",
    "            name : name for this wheel\n",
    "            offset_front : front/back relative to center, positive forward.\n",
    "            offset_left : left/right relative to center, positive left.\n",
    "        \"\"\"\n",
    "        self.name = name\n",
    "        self.offset_front = offset_front\n",
    "        self.offset_left = offset_left"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 227,
   "metadata": {},
   "outputs": [],
   "source": [
    "class ChassisWheelAngleSpeed(object):\n",
    "    \"\"\"Results of chassis geometry calculation for the named wheel.\"\"\"\n",
    "\n",
    "    def __init__(self, name, angle, velocity):\n",
    "        \"\"\"Initialize a chassis wheel with desired angle and velocity.\n",
    "        Args:\n",
    "            name : name for this wheel\n",
    "            angle : steering angle for this wheel in radians.\n",
    "            velocity : rolling velocity for this wheel in meters/second\n",
    "        \"\"\"\n",
    "        self.name = name\n",
    "        self.angle = angle\n",
    "        self.velocity = velocity"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 228,
   "metadata": {},
   "outputs": [],
   "source": [
    "test_chassis = []\n",
    "test_chassis.append(\n",
    "    ChassisWheel(\n",
    "        'front_left',  SAWPPY_WHEELBASE_FRONT,  SAWPPY_TRACK_FRONT,\n",
    "        ),\n",
    "    )\n",
    "test_chassis.append(\n",
    "    ChassisWheel(\n",
    "        'front_right', SAWPPY_WHEELBASE_FRONT, -SAWPPY_TRACK_FRONT,\n",
    "        ),\n",
    "    )\n",
    "test_chassis.append(\n",
    "    ChassisWheel(\n",
    "        'mid_left',      SAWPPY_WHEELBASE_MID,  SAWPPY_TRACK_MID,\n",
    "        ),\n",
    "    )\n",
    "test_chassis.append(\n",
    "    ChassisWheel(\n",
    "        'mid_right',     SAWPPY_WHEELBASE_MID, -SAWPPY_TRACK_MID,\n",
    "        ),\n",
    "    )\n",
    "test_chassis.append(\n",
    "    ChassisWheel(\n",
    "        'rear_left',    SAWPPY_WHEELBASE_REAR,  SAWPPY_TRACK_REAR,\n",
    "        ),\n",
    "    )\n",
    "test_chassis.append(\n",
    "    ChassisWheel(\n",
    "        'rear_right',   SAWPPY_WHEELBASE_REAR, -SAWPPY_TRACK_REAR,\n",
    "        ),\n",
    "    )\n",
    "        "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Input"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 229,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "\n",
    "# Commanded inputs\n",
    "velocityAngular = -0.1*math.pi # radians/sec\n",
    "velocityLinear = 0.1 # meters/sec"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Calculation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Center of turn\n",
    "\n",
    "Consider the state of the robot after one second of traveling at commanded `velocityAngular` and `velocityLinear`. It would be pointing at `velocityAngular` radians off of +X axis (marking forward) and have covered `velocityLinear` distance. This describes an arc. Where is the center of the arc?\n",
    "\n",
    "One way to think about this problem is to consider the case where angular velocity is `2*pi`. After one second, the robot has traveled in a complete circle and `velocityLinear` is the circumference of that circle. Distance to the center of this circle would therefore be the radius, or `velocityLinear/2*pi`\n",
    "\n",
    "This formula holds for other values of `velocityAngular`. The linear distance traveled is some fraction of a circle, and dividing by the angular velocity returns the center of that circle.\n",
    "\n",
    "If `velocityAngular` is zero, we are traveling straight forward which is mathematically equivalent to a circle with infinite radius. This is set as our default value."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 230,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "-0.3183098861837907\n"
     ]
    }
   ],
   "source": [
    "turnCenter = math.inf\n",
    "if velocityAngular != 0:\n",
    "    turnCenter = velocityLinear / velocityAngular\n",
    "print(turnCenter)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Wheel Angle and Velocity\n",
    "Once the center of turn has been calculated, we can calculate the angle and distance from that point to each wheel.\n",
    "The angle becomes the [Ackermann steering angle](https://en.wikipedia.org/wiki/Ackermann_steering_geometry) for that wheel.\n",
    "The distance is compared with the center of turn, and the ratio determines velocity for that wheel."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 231,
   "metadata": {},
   "outputs": [],
   "source": [
    "calculated_results = []\n",
    "for wheel in test_chassis:\n",
    "    if turnCenter == math.inf:\n",
    "        # Heading directly forward or back\n",
    "        wheel_angle = 0\n",
    "        wheel_velocity = velocityLinear\n",
    "    else:\n",
    "        # Dimensions of a trialge representing the wheel relative to center of turn.\n",
    "        opposite = wheel.offset_front\n",
    "        adjacent = turnCenter - wheel.offset_left\n",
    "        hypotenuse = math.sqrt(pow(opposite,2)+pow(adjacent,2))\n",
    "        if wheel.offset_front == 0:\n",
    "            wheel_angle = 0\n",
    "        else:\n",
    "            wheel_angle = math.atan(opposite / adjacent)\n",
    "        wheel_velocity = math.copysign(velocityAngular * hypotenuse, velocityLinear)\n",
    "        # If center of turn is inside the wheel, we need to reverse direction.\n",
    "        if (turnCenter >= 0 and wheel.offset_left > 0 and wheel.offset_left > turnCenter) or \\\n",
    "           (turnCenter < 0 and wheel.offset_left < 0 and wheel.offset_left < turnCenter):\n",
    "            wheel_velocity = wheel_velocity * -1\n",
    "    calculated_results.append(ChassisWheelAngleSpeed(wheel.name, wheel_angle, wheel_velocity))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 232,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-0.4, 0.4, -0.4, 0.4]"
      ]
     },
     "execution_count": 232,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAYQAAAEICAYAAABfz4NwAAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADl0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uIDMuMC4zLCBodHRwOi8vbWF0cGxvdGxpYi5vcmcvnQurowAAIABJREFUeJzt3XmYVdWZ7/HvD7BABhUIEhAcQ0JIYtRU1HRQcOCKQgm5kla7tUGjtrGJfaNpwzW2idqda9RuY1q7DbGTOMRo0CiY2E7lkBiHWCqtQaUoFQVBQMQpOAHv/WMv6ENxavLsOucU9fs8Tz21115r7/XWPlX73XvtoRQRmJmZ9ah0AGZmVh2cEMzMDHBCMDOzxAnBzMwAJwQzM0ucEMzMDHBCsComabGkQzthvd+TdF3e663WfssVh6T7JZ1U4jqulPSPecVkHeOE0EVIGivpIUlvSnpd0h8kfbHScZnlKSJOjYgLACSNl7S00jF1J70qHYC1TdJ2wG+ArwO/AmqAA4D3KxmXmW1dfIbQNXwSICJ+GRHrI+LdiLgrIp4CkLSHpHslrZb0mqRfSNoh1Z0g6baNK5LUJOlXBeUlkvZK0yHpdEkvpPVcLKlHqpuRzkr+LZ2lPCfpkFT3VUmPFwYs6UxJtzb/QSQdJOnpgvI9kv5YUH5Q0tSCRfaS9FTq80ZJfQraTpY0X9Ib6expz4K64ZJulrRK0ouSTm/PhpY0UNJv0nJr0vSIgvr7JV2QtsXbku6S9LGC+r+R9FL6LP6xtWEvSfunuN+Q9N+SxrcS1yxJz6c+n5H0lYK6GWm7XZJiflHS4QX1u0l6IC17N/Cxop1kbZ+VNLmg3Cv9LuzTkZgl9ZB0TtoWKyVdI2n7gvqxBetZImlGmv9zSf8kqR/wX8BwSe+kr+GS1koaXLCeL6TPapuWfibrgIjwV5V/AdsBq4GrgcOBgc3qPwFMAHoDQ4DfAT9MdbsDb5Al/2HAS8ArBXVrgB6pHMB9wCBgZ6AROCnVzQDWAd8EtgGOBt5MbXsDrwOfLojpSeCoIj9LH+Bdsp1SL+BVYBkwANg21Q1ObRcDfwSGp36eBU5NdfsAK4H9gJ7A9NS+d/pZHwfOJTub2h14ATgsLfs94LoWtvVg4Cigb4ppDnBrQf39wPNkSXrbVL4w1Y0B3gHGpn4vAT4EDm3eL7BT+kyPSPFOSOUhLcT11bQdeqRt/2dgWMFn8yFwctoWX0/bVKn+YeBf07Y5EHi7lZ//XOAXBeVJwHPtiTlti42/LycCTWnb9wd+DVyb6nZOMRxL9rs0GNgr1f0c+Kc0PR5Y2iy+24GvF5QvBf6t0n+jW8tXxQPwVzs/KPh0+mNZSrZjngcMbaHtVODJgvISsh3oMcBssp3saOAEYF5BuwAmFpRPA+rT9IzCnUya90fg+DT9H8A/p+nPkCWa3i3E93vgfwP7A3eRDYNNBA4Cnipotxg4rqB8EXBlQX8XNFvvQmAcWZJ4uVnd/wV+lqa/19IOsUisewFrCsr3A+c020Z3pOlzgV8W1PUFPqB4Qvj2xh1kQfs7gentjGs+MKXgs2lq1m8AHyfb+a4D+hXUX9/Sz092cPE20DeVfwGc256Y2Twh1AOnFbT7FFnS6pU+i1ta6P/ntJ4Qjgb+kKZ7kh1Q7Fvpv8+t5cvXELqIiHiW7A8fSaOB64AfAsdK2hH4Edl1hQFkR29rChZ/gOyP6xNp+g2yHeeXUrnQkoLpl8iOSjd6JdJfYpH6q4FfSjoHOB74VUS0dI1jYzxL0/SaFM/7ReJ5tWB6bUF/uwDTJX2joL4m1a8nG2p4o6CuJ1kiapWkvmRHnROBgWn2AEk9I2J9CzH1T9PDKdh+EbFW0uoWutoF+KqkuoJ525CdoRWL62+AM4Bd06z+bD70symm1G9hmzUR8eeCti8BI4v1ExFNkp4F6tJQ45HA3h8h5uGpn8I+ewFDU9/PF+u/HeYCV0ranews7c2I+GMby1g7+RpCFxQRz5EdSX02zfp/ZEeEe0bEdsBxgAoW2bgDPiBNP0C2Ax7Hljvgwh3FzmRnBRvtpLSnaV4fEY+QHQ0fAPwVcG0rP8LGeA5sRzwtWUJ2RrJDwVffiPhlqnuxWd2AiDiiHes9k+xodr+0LQ9M89XyIpssBwqvN2xLNhzSUvzXNouxX0Rc2LyhpF2AnwAzyYbTdgD+1IGYBqYx+Y12bmOZX5IN50wBnomIpo7GTPZ7sUuzPtcBK9J69mhH7Fu8ijki3iM7o/xrsgOP1n7PrIOcELoASaPTRdoRqTyS7A/2kdRkANnY9RuSdgL+odkqHiAbjtk2IpaSHSlPJNtZPdms7T+kC6sjgb8Hbiyo2xE4XdI2kr5KNox1e0H9NcDlwLqIeLCVH+khsp3uvsAfI2IB2c5jP7LrH+3xE+BUSfsp00/SJEkDyIay3pL0bUnbSuop6bNq3226A8iuY7whaRDw3XbGA3AT2ZH1X0iqAc6j5Z32dantYSm+PspusxxRpG0/sp3jKshuFOB/DgZaFREvAQ3AeZJqJI0F6tpY7Abgf5Fdi7j+I8b8S+Cb6YJ2f+D7wI0RsY5sGOpQSX+ZLloPVrqxoZkVwODCi9HJNWRny0emmCwnTghdw9tkO8tHJf2ZLBH8iexoFrIdzz5kF3l/S3YBb5OIaCRLGL9P5bfILrL+oWAYZKO5ZBdk56d1/WdB3aPAKOA14J+BaRFROCRyLdmOqtWjtjR88QSwICI+SLMfBl6KiJWtLVuwjgayi6iXkw05NZGG1NLPVEc2/v9iivcqoPmOpZgfkl0sfo1sO9/RnnhSvwuAb5DtUJeTfW4rKXJ7cEQsITsCP5tsR7+ELJFv8TcZEc8A/0K2jVYAnwP+0N64yM7Y9iO78P9dsh1qaz/H8tTXX1BwQNCRmIGfkv0e/I7sM3iPbNsQES+TXZg+M8U0H/h8kTieI0ssL6S7kYan+X8ANgBPRMTidvz81k4b70IwQ1IAowqGCArrZpBdMBzbyvLbku0A94mIRZ0WaBeRjozfINumL1Y6nq2JpHuB6yPiqkrHsjXxGYLl6evAY905GUiqk9Q3jdlfAjxNdreU5SQN/e3D5sOZloNcEoKkiZIWKnvoaVYr7aYpe/ipNo9+rXpIWkx2zeHMNppu7aaQXVBdRja8dkz4NDw3kq4G7gH+T0S8Xel4tjYlDxlJ6kn2ANMEstsIHwOOTeOehe0GkI1J1wAz0xiwmZlViTzOEPYleyjmhXSB8Aayo6TmLiB7sOi9HPo0M7Oc5fFg2k5s/jDTUrI7GjaRtDcwMiJ+I+lbLa1I0inAKQD9+vX7wujRo3MIz8ys+3j88cdfi4ghH2XZPBJCsfusN41DKXs52qWkWwJbExGzyV6tQG1tbTQ0eFTJzKwjJL3Udqvi8hgyWsrmT7eOYPOnWweQ3Zt+f7rwuD8wzxeWzcyqSx4J4TFgVHoisYbsBWrzNlZGxJsR8bGI2DUidiV72OdIX1Q2M6suJSeE9Cj6TLK3Hj5L9lKzBZLOl3Rkqes3M7PyyOVtpxFxO5u/04aIOLeFtuPz6NPMzPLlJ5XNzAxwQjAzs8QJwczMACcEMzNLnBDMzAxwQjAzs8QJwczMACcEMzNLnBDMzAxwQjAzs8QJwczMACcEMzNLnBDMzAxwQjAzs8QJwczMACcEMzNLnBDMzAxwQjAzsySXhCBpoqSFkpokzSpSf6qkpyXNl/SgpDF59GtmZvkpOSFI6glcARwOjAGOLbLDvz4iPhcRewEXAf9aar9mZpavPM4Q9gWaIuKFiPgAuAGYUtggIt4qKPYDIod+zcwsR71yWMdOwJKC8lJgv+aNJP0dcAZQAxycQ79mZpajPM4QVGTeFmcAEXFFROwBfBs4p+iKpFMkNUhqWLVqVQ6hmZlZe+WREJYCIwvKI4BlrbS/AZharCIiZkdEbUTUDhkyJIfQzMysvfJICI8BoyTtJqkGOAaYV9hA0qiC4iRgUQ79mplZjkq+hhAR6yTNBO4EegI/jYgFks4HGiJiHjBT0qHAh8AaYHqp/ZqZWb7yuKhMRNwO3N5s3rkF03+fRz9mZtZ5/KSymZkBTgjdQkTw6quvsmbNmkqH0q19+OGHPPPMM0T4MRyrTrkMGVl1ePPNN2lsbGTRokU0NjZu9vWVr3yFq666qtIhdmvbbLMN3/zmN2lqaqKuro7Jkydz4IEHUlNTU+nQzABQtR6t1NbWRkNDQ6XDqFpLlizh+uuv32ynv3LlyqJtZ82axfe//32kYo+MWDk99NBDfPnLX95U3m677TjssMOoq6vjiCOOYPDgwRWMzrYGkh6PiNqPsqzPELqoESNGAPDzn/+cDRs2FG0jiR/96EfMnDmznKFt9V588UUOP/zwj7y8pE3DRm+99RZz5sxhzpw59OjRgy996UvU1dVRV1fHpz/9aSdxKyufIXRha9asYfr06dx2221b1NXU1PCLX/yCadOmVSCyrVtjYyOf+tSnOrWPfv36ceqpp/Ld736XAQMGdGpftnXxGUI3s2zZMi699FKuvPJK3nnnnS3qt99+e+bOncu4ceMqEN3Wr3///h850UYEv/71r4teWB45cuSms4Px48fTp0+fUkM16xAnhC6ksbGRiy++mGuuuYYPPvhg0/yhQ4eyYsUKAHbaaSfuuOMOPvvZz1YqzK3e8OHDmTNnzkda9tZbb+Xmm28GsqGjfffdl8mTJ1NXV8eee+7pISKrKCeELqChoYEf/OAH3HzzzZsdWY4dO5ZZs2bRr18/DjroIMaMGcMdd9zByJEjW1mbVUpEcMkllzB16lTq6uqYNGkSQ4cOrXRYZps4IVSpiKC+vp4LL7yQ+vr6zeomT57Mt7/9bcaOHQvAXXfdxQEHHMDcuXMZOHBgJcK1dli/fj333HOPh4KsajkhVJn169dzyy23cOGFF/L4449vmt+zZ0+OPfZYzjrrLD73uc9ttsyYMWO46667vKOpcr169aJXL//JWfXyb2eVeP/997nuuuu46KKLaGxs3DS/T58+nHTSSZx55pnsuuuuRZfdeAuqmVkpnBCqwOrVq9lzzz1Ztux//o3EDjvswMyZM/nGN77BjjvuWMHozLqXRx55hIEDB3b6rcXVyO8yqgKDBw9m9OjRQHYHyyWXXMLLL7/MBRdc4GRgVkbr16/nxBNPZMyYMZx44om89NJLlQ6prPxgWpV48MEHWbhwIccddxy9e/eudDhm3dIDDzzA+PHjN5W32WYb/vZv/5azzz6bYcOGVS6wDijlwTSfIVSJsWPH8rWvfc3JwKyCxo0bx5NPPsnkyZOB7A21l19+OXvssQdnnXUWq1evrnCEncsJwcyswF577cVtt93Gww8/zMEHHwzAu+++y8UXX8xuu+3Geeedx1tvvVXhKDuHE4KZWRH7778/9fX11NfXs//++wPw9ttv873vfY/dd9+diy++mLVr1262zDvvvMNrr71WiXBz4YRgZtaKgw8+mIceeojbbruNz3/+80B2Z+BZZ53FHnvswRVXXLHpVTKvv/46Bx988KZXyXQ1uSQESRMlLZTUJGlWkfozJD0j6SlJ9ZJ2yaNfM7NykMTkyZN54oknuPHGGzfdkvrqq68yc+ZMPvnJT/Kzn/2M9957j6effprx48dvdht5V1HyXUaSegKNwARgKfAYcGxEPFPQ5iDg0YhYK+nrwPiIOLq19Xa3u4zMrOtYt24d1113Heeddx6LFy/eNH/YsGEsX74cgE984hPce++9ZX+3WKXvMtoXaIqIFyLiA+AGYEphg4i4LyI2DrY9AvjRWjPrsnr16sWMGTNYuHAhV1xxxaZbUjcmA4CmpiYOPPBAXnzxxUqF2WF5JISdgCUF5aVpXku+BvxXsQpJp0hqkNSwatWqHEIzM+s8NTU1nHbaaTQ1NTFp0qQt6hcvXsy4ceNYtGhRBaLruDwSQrEXuBcdh5J0HFALXFysPiJmR0RtRNQOGTIkh9DMzDrXu+++y1lnncVvf/vbovVLlixh3LhxPPvss2WOrOPySAhLgcJBshHAFldTJB0KfAc4MiLez6FfM7OKW7lyJZ/5zGc4+eST+eIXv1j04dLly5czfvx4nn766QpE2H55XFTuRXZR+RDgFbKLyn8VEQsK2uwN3ARMjIh2nTv5orKZdUXr1q3jueeeY/78+Tz55JM8+eSTzJ8/nzVr1jBo0CDuvvtu9tlnn07rv5SLyrm8y0jSEcAPgZ7ATyPinyWdDzRExDxJ9wCfAzZecXk5Io5sbZ1OCGa2tYgIXn75ZebPn8/ixYs5+eST6du3b6f0VfGE0BmcEMzMOq7St52amdlWwAnBzMwAJwQzM0ucEMzMDHBCMDOzxAnBzMwAJwQzM0ucEMzMDHBCMDOzxAnBzMwAJwQzM0ucEMzMDHBCMDOzxAnBzMwAJwQzM0ucEMzMDHBCMDOzxAnBzMyAnBKCpImSFkpqkjSrSP2Bkp6QtE7StDz6NDOzfJWcECT1BK4ADgfGAMdKGtOs2cvADOD6UvszM7PO0SuHdewLNEXECwCSbgCmAM9sbBARi1Pdhhz6MzOzTpDHkNFOwJKC8tI0r8MknSKpQVLDqlWrcgjNzMzaK4+EoCLz4qOsKCJmR0RtRNQOGTKkxLDMzKwj8kgIS4GRBeURwLIc1mtmZmWUR0J4DBglaTdJNcAxwLwc1mtmZmVUckKIiHXATOBO4FngVxGxQNL5ko4EkPRFSUuBrwI/lrSg1H7NzCxfedxlRETcDtzebN65BdOPkQ0lmZlZlfKTymZmBjghmJlZ4oRgZmaAE4KZmSVOCGZmBjghmJlZ4oRgZmaAE4KZmSVOCGZmBjghmJlZ4oRgZmaAE4KZmSVOCGZmBjghmJlZ4oRgZmaAE4KZmSVOCGZmBjghmJlZkktCkDRR0kJJTZJmFanvLenGVP+opF3z6NfMzPJTckKQ1BO4AjgcGAMcK2lMs2ZfA9ZExCeAS4EflNqvmZnlK48zhH2Bpoh4ISI+AG4ApjRrMwW4Ok3fBBwiSTn0bWZmOckjIewELCkoL03ziraJiHXAm8Dg5iuSdIqkBkkNq1atyiE0MzNrrzwSQrEj/fgIbYiI2RFRGxG1Q4YMySE0MzNrrzwSwlJgZEF5BLCspTaSegHbA6/n0LeZmeUkj4TwGDBK0m6SaoBjgHnN2swDpqfpacC9EbHFGYKZmVVOr1JXEBHrJM0E7gR6Aj+NiAWSzgcaImIe8J/AtZKayM4Mjim1XzMzy1fJCQEgIm4Hbm8279yC6feAr+bRl5mZdQ4/qWxmZoATgpmZJU4IZmYGOCGYmVnihGBmZoATgpmZJU4IZmYGOCGYmVnihGBmZoATgpmZJU4IZmYGOCGYmVnihGBmZoATgpmZJU4IZmYGOCGYmVnihGBmZoATgpmZJSUlBEmDJN0taVH6PrCFdndIekPSb0rpz8zMOk+pZwizgPqIGAXUp3IxFwPHl9iXmZl1olITwhTg6jR9NTC1WKOIqAfeLrEvMzPrRKUmhKERsRwgfd+xlJVJOkVSg6SGVatWlRiamZl1RK+2Gki6B/h4karv5B1MRMwGZgPU1tZG3us3M7OWtZkQIuLQluokrZA0LCKWSxoGrMw1OjMzK5tSh4zmAdPT9HRgbonrMzOzCik1IVwITJC0CJiQykiqlXTVxkaSfg/MAQ6RtFTSYSX2a2ZmOWtzyKg1EbEaOKTI/AbgpILyAaX0Y2Zmnc9PKpuZGeCEYGZmiROCmZkBTghmZpY4IZiZGeCEYGZmiROCmZkBTghmZpY4IZiZGeCEYGZmiROCmZkBTghmZpY4IZiZGeCEYGZmiROCmZkBTghmZpY4IZiZGeCEULXWrVtX6RDMrJspKSFIGiTpbkmL0veBRdrsJelhSQskPSXp6FL67A7mz5/PnXfeWekwzKybKfUMYRZQHxGjgPpUbm4t8DcR8RlgIvBDSTuU2O9Wa+3atRx77LH07Nmz0qGYWTdTakKYAlydpq8GpjZvEBGNEbEoTS8DVgJDSux3q3XGGWfw3HPP0bdv30qHYmbdTKkJYWhELAdI33dsrbGkfYEa4PkW6k+R1CCpYdWqVSWG1vXccsst/PjHPwZwQjCzsuvVVgNJ9wAfL1L1nY50JGkYcC0wPSI2FGsTEbOB2QC1tbXRkfV3da+88gonnXTSprITgpmVW5sJISIObalO0gpJwyJiedrhr2yh3XbAb4FzIuKRjxztVmr9+vUcf/zxvP7665vmOSGYWbmVOmQ0D5iepqcDc5s3kFQD3AJcExFzSuxvq3TJJZdw3333bTbPCcHMyq3UhHAhMEHSImBCKiOpVtJVqc1fAgcCMyTNT197ldjvVuOxxx7jnHPO2WJ+v379KhCNmXVniqjOofra2tpoaGiodBid6p133mHvvfemqalpi7r169fTo4efGzSzjpH0eETUfpRlvcepoNNPP71oMujTp4+TgZmVnfc6FbJ27VqOPvpoGhsbOeqoowDo1Su7xu/rB2ZWCW3eZWSdo2/fvhx22GEAnHDCCfTs2ZMBAwYwevRoLrvssgpHZ9b9RASPPvooc+fO5Qtf+ALTpk1j9erV9O/fn969e1c6vLJwQqgCkyZNYtKkSZvKI0eOrGA0Zt3Thg0bmDp1KitWrGDixImMHTuWCRMm8Lvf/a7bJAQPGVWho4/2+//MymXdunW8+uqr9OjRg7q6OgDq6+sZN24cf/rTn9iwoehztFslnyGYWbfWs2dPZs6cyX333Uf//v0B+PDDD2lsbASyO/66C58hmFm3Jol///d/p0ePHrz88stb1HenMwQnBDPr9nbccUeuvPLKonVOCGZm3cxRRx3FMcccs8V8DxmZWYs2bNhAtT7hb6W5/PLLGTp06GbzfIZgZi1auXIlo0eP5lvf+hYrVxZ9wa91UYMHD970P0k2ckIwsxYNGjSI888/n7PPPpsdd2z1f0JZFzRlyhSOP/74TWUPGZlZi2pqajj66KMZNGhQpUOxTnLZZZcxbNgwwGcIZmbd2sCBA/nJT34C+AzBzKzbmzRpEieccEK3OkPwk8pmZi249NJLWbt2baXDKBsnBDOzFmy//fZsv/32lQ6jbDxkZGZmQIkJQdIgSXdLWpS+DyzSZhdJj6f/pbxA0qml9GlmZp2j1DOEWUB9RIwC6lO5ueXAX0TEXsB+wCxJw0vs18zMclZqQpgCXJ2mrwamNm8QER9ExPup2DuHPs3MrBOUunMeGhHLAdL3oo9tShop6SlgCfCDiFhWYr9mZpazNu8yknQP8PEiVd9pbycRsQTYMw0V3SrppohYUaSvU4BTAHbeeef2rt7MzHLQZkKIiENbqpO0QtKwiFguaRjQ6pu+ImKZpAXAAcBNRepnA7MBamtr/TpJM7MyKnXIaB4wPU1PB+Y2byBphKRt0/RA4MvAwhL7NTOznJWaEC4EJkhaBExIZSTVSroqtfk08Kik/wYeAC6JiKdL7NfMzHJW0pPKEbEaOKTI/AbgpDR9N7BnKf2YmVnn8y2gZmYGOCGYmVnihGBmZoATgpmZJU4IZmYGOCGYmVnihGBmZoATgpmZJU4IZmYGOCGYmVnihGBmZoATgpmZJU4IZmYGOCGYmVnihGBmZoATgpmZJU4IZmYGOCGYmVnihGBmZkCJCUHSIEl3S1qUvg9spe12kl6RdHkpfZqZWeco9QxhFlAfEaOA+lRuyQXAAyX2Z2ZmnaTUhDAFuDpNXw1MLdZI0heAocBdJfZnZmadpFeJyw+NiOUAEbFc0o7NG0jqAfwLcDxwSGsrk3QKcEoqvi/pTyXGVw4fA16rdBDt4Djz5Tjz1RXi7AoxAnzqoy7YZkKQdA/w8SJV32lnH6cBt0fEEkmtNoyI2cDs1G9DRNS2s4+KcZz5cpz5cpz56QoxQhbnR122zYQQEYe20vEKScPS2cEwYGWRZl8CDpB0GtAfqJH0TkS0dr3BzMzKrNQho3nAdODC9H1u8wYR8dcbpyXNAGqdDMzMqk+pF5UvBCZIWgRMSGUk1Uq6qsR1zy5x+XJxnPlynPlynPnpCjFCCXEqIvIMxMzMuig/qWxmZoATgpmZJVWTELrKazDaE6ekXSQ9Lmm+pAWSTq3SOPeS9HCK8SlJR1djnKndHZLekPSbMsc3UdJCSU2StrgZQlJvSTem+kcl7VrO+NoZ44GSnpC0TtK0csdXEEdbcZ4h6Zn0u1gvaZcqjfNUSU+nv+8HJY2pxjgL2k2TFJLavmU2IqriC7gImJWmZwE/aKXtZcD1wOXVGCdQA/RO0/2BxcDwKozzk8CoND0cWA7sUG1xprpDgDrgN2WMrSfwPLB7+kz/GxjTrM1pwJVp+hjgxjJvv/bEuCuwJ3ANMK2c8XUwzoOAvmn66+Xelh2Ic7uC6SOBO6oxztRuAPA74BGyOzxbXW/VnCHQdV6D0WacEfFBRLyfir2pzJlYe+JsjIhFaXoZ2XMkQ8oWYaZdn3tE1ANvlyuoZF+gKSJeiIgPgBvI4i1UGP9NwCFq6wnMMscYEYsj4ilgQxnjaq49cd4XEWtT8RFgRJljhPbF+VZBsR9QiTtz2vO7Cdk75C4C3mvPSqspIWz2Ggygtddg/EOZYyvUZpwAkkZKegpYQnbUu6yMMUI749xI0r5kRxrPlyG2Qh2Ks8x2Ivv8Nlqa5hVtExHrgDeBwWWJrln/SbEYq0FH4/wa8F+dGlFx7YpT0t9Jep5sZ3t6mWIr1GackvYGRkZEu4dZS30wrUPK+RqMUuQQJxGxBNhT0nDgVkk3RcSKvGKEfOJM6xkGXAtMj4jcjyLzirMCiv2SNT8abE+bzlTp/tur3XFKOg6oBcZ1akTFtSvOiLgCuELSXwHnkD2YW06txpkOni8FZnRkpWVNCNFFXoORQ5yF61omaQFwANmQQlXFKWk74LfAORHxSJ7x5RlnhSwFRhaURwDNz/Q2tlkqqRewPfB6ecLbrP+NisVYDdoVp6RDyQ4UxhUMu5ZTR7fnDcB/dGpExbUV5wDgs8D96eD548A8SUdGRIvvOqqmIaONr8GAVl6DERE7R8SuwLeAa/JOBu3QZpySRkjaNk0PBL4MLCxbhJn2xFkD3EK2HeeUMbZCbcYUx5pPAAABCUlEQVRZQY8BoyTtlrbVMWTxFiqMfxpwb6SreVUUYzVoM840xPFj4MiIqNSBQXviHFVQnAQsKmN8G7UaZ0S8GREfi4hd0/7yEbLt2vqL78p9dbyVq+aDyf7JzqL0fVCaXwtcVaT9DCpzl1GbcZK9xuMpsiv/TwGnVGmcxwEfAvMLvvaqtjhT+ffAKuBdsqOjw8oU3xFAI9m1le+keeenPy6APsAcoAn4I7B7BT7rtmL8YtpmfwZWAwvKHWM747wHWFHwuzivSuO8DFiQYrwP+Ew1xtms7f204y4jv7rCzMyA6hoyMjOzCnJCMDMzwAnBzMwSJwQzMwOcEMzMLHFCMDMzwAnBzMyS/w8NHKtz1zTYdgAAAABJRU5ErkJggg==\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "# if using a Jupyter notebook, include:\n",
    "%matplotlib inline\n",
    "\n",
    "plot_x = []\n",
    "plot_y = []\n",
    "plot_u = []\n",
    "plot_v = []\n",
    "wheel_state = dict()\n",
    "\n",
    "for wheel in test_chassis:\n",
    "    wheel_state[wheel.name] = dict()\n",
    "    wheel_state[wheel.name]['x'] = wheel.offset_front\n",
    "    wheel_state[wheel.name]['y'] = wheel.offset_left\n",
    "    wheel_state[wheel.name]['hypotenuse'] = math.sqrt(pow(wheel.offset_front,2)+pow(wheel.offset_left,2))\n",
    "for wheel in calculated_results:\n",
    "    plot_x.append(wheel_state[wheel.name]['x'])\n",
    "    plot_y.append(wheel_state[wheel.name]['y'])\n",
    "    wheel_state[wheel.name]['angle'] = wheel.angle\n",
    "    wheel_state[wheel.name]['velocity'] = wheel.velocity\n",
    "    plot_u.append(math.cos(wheel.angle)*wheel.velocity)\n",
    "    plot_v.append(math.sin(wheel.angle)*wheel.velocity)\n",
    "\n",
    "fig, ax = plt.subplots()\n",
    "ax.set_title('Sawppy wheel angle and velocity')\n",
    "ax.quiver(plot_x, plot_y, plot_u, plot_v)\n",
    "ax.axis([-0.4, 0.4, -0.4, 0.4])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Print the input and output of this particular scenario in a format suitable for the unit test data file `chassis_wheel_calculator_tests.csv`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 233,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "-0.314159, 0.100000,-0.479345, 0.194136,-1.270319, 0.093735, 0.000000, 0.181681, 0.000000, 0.018319, 0.438306, 0.190240, 1.239818, 0.085373\n"
     ]
    }
   ],
   "source": [
    "print('{: .6f},{: .6f},{: .6f},{: .6f},{: .6f},{: .6f},{: .6f},{: .6f},{: .6f},{: .6f},{: .6f},{: .6f},{: .6f},{: .6f}'.format(\n",
    "velocityAngular, velocityLinear,\n",
    "wheel_state['front_left']['angle'],\n",
    "wheel_state['front_left']['velocity'],\n",
    "wheel_state['front_right']['angle'],\n",
    "wheel_state['front_right']['velocity'],\n",
    "wheel_state['mid_left']['angle'],\n",
    "wheel_state['mid_left']['velocity'],\n",
    "wheel_state['mid_right']['angle'],\n",
    "wheel_state['mid_right']['velocity'],\n",
    "wheel_state['rear_left']['angle'],\n",
    "wheel_state['rear_left']['velocity'],\n",
    "wheel_state['rear_right']['angle'],\n",
    "wheel_state['rear_right']['velocity']))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}