S31_vacuum_state.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "view-in-github"
   },
   "source": [
    "<a href=\"https://colab.research.google.com/github/gtbook/robotics/blob/main/S31_vacuum_state.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "id": "JoW4C_OkOMhe",
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Note: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip install -U -q gtbook"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "id": "10-snNDwOSuC",
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "import gtsam \n",
    "import gtbook\n",
    "from gtbook.discrete import Variables\n",
    "from gtbook import vacuum # various definitions for this chapter\n",
    "\n",
    "VARIABLES = Variables()\n",
    "def pretty(obj): \n",
    "    return gtbook.display.pretty(obj, VARIABLES)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "nAvx4-UCNzt2"
   },
   "source": [
    "# Modeling the State of the Vacuum Cleaning Robot\n",
    "\n",
    "> We introduce discrete states as an abstraction of navigationg through continuous spaces."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "tags": [
     "remove-input"
    ]
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <div align='center'>\n",
       "        <img src='https://github.com/gtbook/robotics/blob/main/Art/steampunk/S31-iRobot%20vacuuming%20robot-02.jpg?raw=1' style='height:256 width:100%'/>\n",
       "        </div>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from gtbook.display import randomImages\n",
    "from IPython.display import display\n",
    "display(randomImages(3, 1, \"steampunk\", 1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this chapter, we describe the state space for our vacuum cleaning robot.\n",
    "Like the trash sorting robot, the vacuuming robot has a discrete state space.\n",
    "However, unlike the trash sorting robot, transitions between arbitrary states is not possible for the vacuuming robot.\n",
    "We will use a graph to encode this aspect of the the state space of the vacuuming robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "nAvx4-UCNzt2"
   },
   "source": [
    "## Defining the Robot's State\n",
    "\n",
    "As described in Chapter 1, the representation of a robot's state should include all information that is necessary for the robot to act effectively in its environment to achieve its goals.  For a vacuum cleaning robot, this might include the exact location of the robot (e.g., x-y coordinates in a map of the house),\n",
    "the heading direction of the robot,\n",
    "whether the floor is carpeted,\n",
    "or the location of any pets or children that might be moving throughout the house.\n",
    "\n",
    "In this chapter, we will abstract away many of these details by making the following\n",
    "assumptions:\n",
    "- The robot can move in any direction, which means that its heading angle is not important.\n",
    "- The robot is equipped with low-level navigation software that will allow it to move from one room to another, through doorways, etc. (though not with 100% reliability).\n",
    "- The robot is equipped with path planning software to clean the floor in a particular room (e.g., execute random\n",
    "motions, or follow a boustrophedon path).\n",
    "- The robot is equipped with collision-avoidance software, so that it need not worry about the presence of obstacles (e.g., furniture, small dogs, or children).\n",
    "\n",
    "Taken together, these assumptions allow us to abstract away all of the geometry of the vacuum cleaning problem,\n",
    "and to **define the state of the robot as the room** of the house in which it is located."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As a running example, we will consider a simple house that\n",
    "includes five rooms: living room, kitchen, office, hallway, and dining room.\n",
    "The floor plan is illustrated \n",
    "below, in Figure <a href=\"#fig:Vacuum\" data-reference-type=\"ref\" data-reference=\"fig:House\">1</a>.\n",
    "\n",
    "<figure>\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures3/S31-House.png?raw=1\" id=\"fig:House\" alt=\"\" />\n",
    "<figcaption id=\"fig:House\" label=\"fig:House\">The floor plan of a a house in which our hypothetical vacuum robot will operate.</figcaption>\n",
    "</figure>\n",
    "\n",
    "As for the trash sorting robot, we define the state to be a random variable with a prior probability distribution.\n",
    "We denote the random state at time $t$ by $X_t$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this chapter, we will consider the case of **discrete time systems**.\n",
    "In particular, rather than consider $t$ as a continuous variable, we will consider the system evolution\n",
    "at a set of discrete moments in time, $t \\in \\{ t_1, t_2 \\dots \\}$.\n",
    "This approach is appropriate for systems whose state changes qualitatively at distinct\n",
    "moments of time (e.g., as when the robot moves from the living room to the kitchen),\n",
    "rather than evolving continuously as time passes (e.g., the position of a robot that rolls along the floor).\n",
    "While the physical vacuum cleaning robot rolls along the floor to reach its destination,\n",
    "our representation of state includes only the room in which the robot is currently located.\n",
    "Therefore, the only interesting moments in time are when the robot traverses from one room to the next, or -as we will see later in this chapter- when the robot executes an action or takes a sensor measurement.\n",
    "To keep notation simple, we will let  $t \\in \\{ 1, 2 \\dots \\}$, rather than using the more general $t_i$ notation.\n",
    "<!-- The concept of state is an abstraction, allowing robots to reason about what is happening to them and their environment as they act. This does not necessarily mean that we, as robot programmers, have no deeper knowledge of what is going on: we do know that a vacuuming robot operates in a two dimensional space, typically a single floor in a domestic residence. However, we might have programmed in \"actions\" that abstract to the notion of moving from room to room, and hence may only be interested in knowing which room the robot is in for the purposes of triggering those actions. Other actions that are useful but will not discuss further in this section might involve vacuuming the current room, terminating when it is done or when the robot is stuck. Rather, we will concentrate below on navigation actions, moving from room to room, knowing that it might be only a small part of the repertoire that a true vacuuming robot needs to be useful. -->"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Since the robot will execute a sequence of actions over time, our prior distribution $P(X_1)$ will only encode knowledge about the initial state $X_1$ of the robot, and the probability distribution for future states must be determined using\n",
    "probabilistic inference.\n",
    "In this chapter, we will assume that the robot *always* starts out in the office, where its charging station is located,\n",
    "i.e., $P(X_1 = Office) = 1.$ The following code does this:\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<p>  <i>P(X1):</i></p>\n",
       "<div>\n",
       "<table class='DecisionTreeFactor'>\n",
       "  <thead>\n",
       "    <tr><th>X1</th><th>value</th></tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr><th>Living Room</th><td>0</td></tr>\n",
       "    <tr><th>Kitchen</th><td>0</td></tr>\n",
       "    <tr><th>Office</th><td>1</td></tr>\n",
       "    <tr><th>Hallway</th><td>0</td></tr>\n",
       "    <tr><th>Dining Room</th><td>0</td></tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "<gtbook.display.pretty at 0x1294ddca0>"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# vacuum.rooms = [\"Living Room\", \"Kitchen\", \"Office\", \"Hallway\", \"Dining Room\"]\n",
    "X = VARIABLES.discrete_series(\"X\", [1], vacuum.rooms)\n",
    "prior = gtsam.DiscreteDistribution(X[1], \"0/0/1/0/0\")\n",
    "pretty(prior)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We use this discrete distribution to convey our knowledge that the robot *always* starts out in the office. We do this with $100\\%$ certainty in this case. This use of probability values to indicate how certain we are about facts in the world is a \"Bayesian\" view, as we explain below."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The State Space for our Vacuum Cleaning Robot\n",
    "\n",
    "For the trash sorting robot, the state was defined as the category of the current item in the work space.\n",
    "This category had no dependence on the category of the previous piece of trash, nor did it have any effect on the category\n",
    "of the subsequent piece of trash. There were no constraints on the transition from the state at time $t$\n",
    "to the state at time $t+1$.\n",
    "Therefore, it was sufficient to merely enumerate the set of possible states; there\n",
    "were no important relationships between states that required representation.\n",
    "\n",
    "This is not the case for our vacuming robot.\n",
    "For example, as can be seen from\n",
    "Figure <a href=\"#fig:Vacuum\" data-reference-type=\"ref\" data-reference=\"fig:House\">1</a>,\n",
    "if the robot is currently in the office, it cannot transition directly to the living room;\n",
    "it must first transition to the hallway before it can transition to the living room.\n",
    "For the vacuum cleaning robot, room adjacency is an important relationship, and therefore\n",
    "it should be encoded into our representation.\n",
    "This can be accomplished using a connectivity graph.\n",
    "Each vertex of this graph represents a state (i.e., a specific room in the house), and two\n",
    "vertices, $x_i, x_j$ are connected by an edge if and only if it the transition between\n",
    "these two states is possible.  Since our robot can move in any direction, if it\n",
    "can transition from $x_i$ to $x_j$, then it can also transition from $x_j$ to $x_i$\n",
    "(as is the case, e.g., for $x_i = Hallway$ and $x_j = Dining Room$).\n",
    "Therefore, we represent the state space by an *undirected* graph.\n",
    "Taken together, the collection of states along with connectivity information is refered to as the **state space**,\n",
    "which in this can be represented by the connectivity graph shown in \n",
    "Figure <a href=\"#fig:Vacuum\" data-reference-type=\"ref\" data-reference=\"fig:House-ss\">2</a>.\n",
    "\n",
    "<figure>\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures3/S31-house-state-space.png?raw=1\" id=\"fig:House-ss\" alt=\"\" />\n",
    "<figcaption id=\"fig:House-ss\" label=\"fig:House-ss\">The state space corresponding\n",
    "to the house above.</figcaption>\n",
    "</figure>\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Bayesian vs. Frequentist\n",
    "\n",
    "<figure>\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures3/N3-Bayes.png?raw=1\" id=\"fig: Bayesian\" style=\"width:14cm\" alt=\"\" />\n",
    "<figcaption id=\"fig: Bayesian\" label=\"fig: Bayesian\">The Reverend Thomas Bayes gave his name to associating probabilities with the strength of beliefs rather than a frequency of events, even though this seems to have been first introduced by Laplace.</figcaption>\n",
    "</figure>\n",
    "\n",
    "We take a Bayesian view of probability, rather than a frequentist one.\n",
    "This means that we see probabilities as describing our knowledge about\n",
    "events, rather than tallying up frequencies by which they occur. Think\n",
    "of the weather-person talking about the probability of rain tomorrow.\n",
    "Probabilities viewed this way can be used to describe knowledge about\n",
    "the state of the world, and how actions affect the state of an agent and\n",
    "the world.\n",
    "\n",
    "<figure>\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures3/N3-Frequentist.png?raw=1\" id=\"fig: Frequentist\" style=\"width:9cm\" alt=\"\" /><figcaption id=\"fig: Frequentist\" label=\"fig: Frequentist\">The caricature of the frequentist view involves counting many heads and tails.</figcaption>\n",
    "</figure>\n",
    "\n",
    "This is to be contrasted with a frequentist view, where probabilities\n",
    "are used to describe the frequencies of events in a series of repeated\n",
    "trials. A Bayesian, instead, might qualify their knowledge about an\n",
    "event that has not even happened yet, let alone multiple times. Of\n",
    "course, in most cases this belief is based on experience, i.e., lots of\n",
    "repeated events in the past, and so it can be seen that perhaps these\n",
    "views are not so different after all.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## GTSAM 101\n",
    "\n",
    "Most everything above should by now be familiar, but we used a new `Variables` method, `discrete_series`, to define a time series of state variables. The signature of this method is\n",
    "```python\n",
    "def discrete_series(self, character: str, indices: Iterable[int],\n",
    "                    domain: List[str]) -> Dict[int, DiscreteKey]:\n",
    "    \"\"\"Create several discrete variables with Symbol names.\n",
    "\n",
    "    Args:\n",
    "        character (str): a single character.\n",
    "        indices: (Iterable[int]): a set of integer indices.\n",
    "        domain (List[str]): names for the different values.\n",
    "\n",
    "    Returns:\n",
    "        Dict[int, DiscreteKey], i.e., [(gtsam.Key, cardinality)]\n",
    "    \"\"\"\n",
    "```\n",
    "\n",
    "For example, the following creates a series of 5 state variables:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "states = VARIABLES.discrete_series('X', [1, 2, 3], vacuum.rooms)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "{1: (6341068275337658369, 5), 2: (6341068275337658370, 5), 3: (6341068275337658371, 5)}\n"
     ]
    }
   ],
   "source": [
    "print(states)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When we print the results, we see that we now get a dictionary of DiscreteKeys, i.e., integer tuples of the form *(Key, cardinality)*. However, the \"Keys\" now seem to be very large integers. This is because for series of variables we use the `gtsam.Symbol` type, composed of a single character and an integer index:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "X1\n"
     ]
    }
   ],
   "source": [
    "symbol = gtsam.Symbol('X', 1)\n",
    "print(symbol.string())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "GTSAM internally stores symbols as a 64-bit integer key, with the 8-bit character in the most significant bits, which explains the large integer value:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "6341068275337658369\n"
     ]
    }
   ],
   "source": [
    "print(symbol.key())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can see that this corresponds to the first state above. However, as before, pretty printing translates these into a nicer looking strings wherever it matters."
   ]
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "include_colab_link": true,
   "name": "S31_vacuum_state.ipynb",
   "provenance": []
  },
  "kernelspec": {
   "display_name": "Python 3.8.12 ('gtbook')",
   "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.8.12"
  },
  "latex_metadata": {
   "affiliation": "Georgia Institute of Technology",
   "author": "Frank Dellaert and Seth Hutchinson",
   "title": "Introduction to Robotics"
  },
  "vscode": {
   "interpreter": {
    "hash": "9f7376ced4243bb13dfcffa8a3ba834e0602aa8334cd3a1d8ba8d285f4628083"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}