S31_vacuum_state.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "view-in-github",
    "tags": [
     "no-tex"
    ]
   },
   "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": 3,
   "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\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "id": "10-snNDwOSuC",
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "import gtsam\n",
    "\n",
    "from gtbook import display\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 display.pretty(obj, VARIABLES)\n"
   ]
  },
  {
   "attachments": {},
   "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 navigating through continuous spaces.\n",
    "\n",
    "<img src=\"Figures3/S31-iRobot_vacuuming_robot-04.jpg\" alt=\"Splash image with 6 panels of a robot in different states\" width=\"40%\" align=center style=\"vertical-align:middle;margin:10px 0px\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this section, we describe the state space for our vacuum cleaning robot.\n",
    "Like the trash sorting robot, the vacuum cleaning robot has a discrete state space. However, unlike the trash sorting robot, the state at the current time depends on the state at the previous time. To model this, we introduce the notion of a **discrete-time system**, which we will use extensively throughout this book. In the next section we will then see how we can use actions to (more or less) control the state of the robot over time.\n",
    "\n",
    "To make our vacuum-cleaning robot example a bit more realistic, we will also make it clear that transitions between arbitrary states are not possible: teleportation for mobile robots has unfortunately not been invented yet! We will use a *graph* to encode this aspect of the the state space for the vacuum cleaning robot, which establishes a deep connection with the notion of graph search in classical AI. In Section 3.5 we will not use graph search but a probabilistic version of it to plan in such state spaces."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "nAvx4-UCNzt2"
   },
   "source": [
    "## Defining the Robot's State\n",
    "\n",
    "> Just rooms, states are...\n",
    "\n",
    "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 floor plan that\n",
    "includes five rooms: the living room, a kitchen, an office, a hallway, and the dining room.\n",
    "The floor plan is illustrated \n",
    "below, in Figure <a href=\"#fig:House\" data-reference-type=\"ref\" data-reference=\"fig:House\">3.1</a>.\n",
    "\n",
    "<figure id=\"fig:House\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures3/S31-House.png?raw=1\" alt=\"\" />\n",
    "<figcaption>The floor plan of a house in which our hypothetical vacuum cleaning robot will operate.</figcaption>\n",
    "</figure>\n",
    "\n",
    "As we did for the trash sorting robot, we define the state to be a discrete random variable with a prior probability distribution."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this chapter, we will consider the case of **discrete time systems**.\n",
    "In particular, rather than considering the time 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",
    "Hence, we can refer to time using the index $k \\in \\{ 1, 2 \\dots \\}$, knowing that each $k$ corresponds to a known time stamp $t_k$, and we denote the random state at time $t_k$ by $X_k$.\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",
    "<!-- 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 vacuum cleaning 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 vacuum cleaning 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 vacuum cleaning 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 assume that the robot *always* starts out in the office, where its charging station is located,\n",
    "i.e., $P(X_1 = \\text{Office}) = 1.$ \n",
    "\n",
    "The following code defines a variable and encodes this prior probability distribution, which we can then pretty-print:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "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 0x1251d1300>"
      ]
     },
     "execution_count": 8,
     "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)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We use this discrete distribution to convey our knowledge that the robot *always* starts out in the office. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The State Space for our Vacuum Cleaning Robot\n",
    "\n",
    "> Connected rooms, un-directionally.\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_k$\n",
    "to the state at time $t_{k+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",
    "<figure id=\"fig:House-ss\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures3/S31-house-state-space.png?raw=1\" alt=\"\" />\n",
    "<figcaption>The state space corresponding\n",
    "to the house above.</figcaption>\n",
    "</figure>\n",
    "\n",
    "This is not the case for our vacuum cleaning robot.\n",
    "For example, as can be seen from\n",
    "Figure <a href=\"#fig:House\" data-reference-type=\"ref\" data-reference=\"fig:House\">3.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 = \\text{Hallway}$ and $x_j = \\text{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 referred to as the **state space**,\n",
    "which in this can be represented by the connectivity graph shown in \n",
    "Figure <a href=\"#fig:House-ss\" data-reference-type=\"ref\" data-reference=\"fig:House-ss\">3.2</a>."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Bayesian vs. Frequentist Interpretations or Probability\n",
    "\n",
    "> Believing with probabilities.\n",
    "\n",
    "<figure id=\"fig:Bayesian\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures3/N3-Bayes.png?raw=1\" style=\"width:14cm\" alt=\"\" />\n",
    "<figcaption>The Reverend Thomas Bayes gave his name to associating probabilities with the strength of beliefs rather than a frequency of events.</figcaption>\n",
    "</figure>\n",
    "\n",
    "In this book, we take a **Bayesian view** of probability, rather than a frequentist one.\n",
    "This means that we view probabilities as describing our *knowledge* about\n",
    "events, rather than tallying up the frequencies by which they occur.\n",
    "For example, think of the weather forecaster talking about the probability of rain tomorrow: this represents a belief about a future state, rather than statistics about previous rainy days.\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 id=\"fig:Frequentist\">\n",
    "<img src=\"https://github.com/gtbook/robotics/blob/main/Figures3/N3-Frequentist.png?raw=1\"  style=\"width:9cm\" alt=\"\" />\n",
    "<figcaption>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 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",
    "\n",
    "In this chapter we will get comfortable with the Bayesian view, and learn how to calculate with them. This will allow use to fuse knowledge from different sources, e.g., how uncertain actions and noisy measurements can nevertheless yield enough information about our current state to make reasonable decisions.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## GTSAM 101\n",
    "\n",
    "The code above should feel familiar, but we used a new `Variables` method called `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": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "states = VARIABLES.discrete_series('X', [1, 2, 3], vacuum.rooms)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "{1: (6341068275337658369, 5), 2: (6341068275337658370, 5), 3: (6341068275337658371, 5)}\n"
     ]
    }
   ],
   "source": [
    "print(states)\n"
   ]
  },
  {
   "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": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "X1\n"
     ]
    }
   ],
   "source": [
    "symbol = gtsam.Symbol('X', 1)\n",
    "print(symbol.string())\n"
   ]
  },
  {
   "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": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "6341068275337658369\n"
     ]
    }
   ],
   "source": [
    "print(symbol.key())\n"
   ]
  },
  {
   "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.10.13"
  },
  "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
}