{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Floating-point error"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In computers, real numbers are commonly approximated using floating-point numbers, such as Julia's `Float64`. Unfortunately, not all real numbers can be exactly represented as a finite-size floating-point number, and the results of operations on floating-point numbers can only approximate the results of applying the operation to a true real number. This results in peculiarities like:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "2.220446049250313e-16"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "2.6 - 0.7 - 1.9"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "IntervalArithmetic.jl can be used to quantify floating point error, by computing _rigorous_ worst-case bounds on floating point error, within which the true result is _guaranteed_ to lie."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "using IntervalArithmetic"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "IntervalArithmetic.jl provides the `Interval` type, which stores an upper and a lower bound:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[1, 2]"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "i = Interval(1.0, 2.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "IntervalArithmetic.Interval{Float64}\n",
      "  lo: Float64 1.0\n",
      "  hi: Float64 2.0\n"
     ]
    }
   ],
   "source": [
    "dump(i)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "IntervalArithmetic.jl provides overloads for most common Julia functions that take these bounds into account. For example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[2, 4]"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "i + i"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[0.84147, 1]"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sin(i)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that the bounds computed by IntervalArithmetic.jl take floating point error into account. Also note that a given real number, once converted to (approximated by) a floating-point number may not be equal to the original real number. To rigorously construct an `Interval` that contains a given real number as an input, IntervalArithmetic.jl provides the `@interval` macro:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "i.lo === i.hi = false\n",
      "IntervalArithmetic.Interval{Float64}\n",
      "  lo: Float64 2.9\n",
      "  hi: Float64 2.9000000000000004\n"
     ]
    }
   ],
   "source": [
    "i = @interval(2.9)\n",
    "@show i.lo === i.hi\n",
    "dump(i)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Compare this to"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "i.lo === i.hi = true\n",
      "IntervalArithmetic.Interval{Float64}\n",
      "  lo: Float64 2.9\n",
      "  hi: Float64 2.9\n"
     ]
    }
   ],
   "source": [
    "i = Interval(2.9)\n",
    "@show i.lo === i.hi\n",
    "dump(i)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As an example, consider again the peculiar result from before, now using interval arithmetic:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-6.66134e-16, 2.22045e-16]"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "i = @interval(2.6) - @interval(0.7) - @interval(1.9)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "showing that the true result, `0`, is indeed in the guaranteed interval, and indeed:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "\u001b[1m\u001b[32mTest Passed\u001b[39m\u001b[22m"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "using Test\n",
    "@test (2.6 - 0.7 - 1.9) ∈ i"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Accuracy of RigidBodyDynamics.jl's `mass_matrix`"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's use IntervalArithmetic.jl to establish rigorous bounds on the accuracy of the accuracy of the `mass_matrix` algorithm for the Acrobot (double pendulum) in a certain configuration. Let's get started."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "using RigidBodyDynamics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We'll create a `Mechanism` by parsing the Acrobot URDF, passing in `Interval{Float64}` as the type used to store the parameters (inertias, link lengths, etc.) of the mechanism. Note that the parameters parsed from the URDF are treated as floating point numbers (i.e., like `Interval(2.9)` instead of `@interval(2.9)` above)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "MechanismState{IntervalArithmetic.Interval{Float64}, IntervalArithmetic.Interval{Float64}, IntervalArithmetic.Interval{Float64}, …}(…)"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "const T = Interval{Float64}\n",
    "const mechanism = parse_urdf(T, joinpath(dirname(pathof(RigidBodyDynamics)), \"..\", \"test\", \"urdf\", \"Acrobot.urdf\"))\n",
    "remove_fixed_tree_joints!(mechanism)\n",
    "state = MechanismState(mechanism)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's set the initial joint angle of the shoulder joint to the smallest `Interval{Float64}` containing the real number $1$, and similarly for the elbow joint:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "shoulder, elbow = joints(mechanism)\n",
    "set_configuration!(state, shoulder, @interval(1))\n",
    "set_configuration!(state, elbow, @interval(2));"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And now we can compute the mass matrix as normal:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "2×2 Symmetric{IntervalArithmetic.Interval{Float64},Array{IntervalArithmetic.Interval{Float64},2}}:\n",
       " [1.8307, 1.83071]     [0.913853, 0.913854]\n",
       " [0.913853, 0.913854]  [1.32999, 1.33001]  "
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "M = mass_matrix(state)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Woah, those bounds look pretty big. RigidBodyDynamics.jl must not be very accurate! Actually, things aren't so bad; the issue is just that IntervalArithmetic.jl isn't kidding when it comes to guaranteed bounds, and that includes printing the numbers in shortened form. Here are the lengths of the intervals:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "2×2 Array{Float64,2}:\n",
       " 4.21885e-15  5.10703e-15\n",
       " 5.10703e-15  6.66134e-15"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "err = map(x -> x.hi - x.lo, M)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "\u001b[1m\u001b[32mTest Passed\u001b[39m\u001b[22m"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "@test maximum(abs, err) ≈ 0 atol = 1e-14"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Rigorous (worst-case) uncertainty propagation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "IntervalArithmetic.jl can also be applied to propagate uncertainty in a rigorous way when the inputs themselves are uncertain. Consider for example the case that we only know the joint angles up to $\\pm 0.05$ radians:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "set_configuration!(state, shoulder, @interval(0.95, 1.05))\n",
    "set_configuration!(state, elbow, @interval(1.95, 2.05));"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and let's compute bounds on the center of mass position:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Point3D in \"world\": IntervalArithmetic.Interval{Float64}[[-0.770193, -0.630851], [0.199999, 0.200001], [0.0167304, 0.163822]]"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "center_of_mass(state)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that the bounds on the $y$-coordinate are very tight, since our mechanism only lives in the $x$-$z$ plane."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Julia 0.6.2",
   "language": "julia",
   "name": "julia-0.6"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "0.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}