{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tolerances on a path\n",
    "\n",
    "The software includes algorithms to solve path following problems. The end-effector pose is given as discrete points along a path in Cartesian space. In addition we can add a tolerance to the position and orientation of these poses. The tools to do this are explained here.\n",
    "\n",
    "## Toleranced numbers\n",
    "\n",
    "One of the main techniques to add tolerences is to use a `TolerancedNumber` where you would normally use a Python `float`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "from acrobotics.path import TolerancedNumber, TrajectoryPt\n",
    "\n",
    "# position and orientation in euler angles\n",
    "x, y, z = 1.0, 2.0, 3.0\n",
    "rx, ry, rz = 0.0, 0.0, 1.57\n",
    "\n",
    "# a fixed end-effector pose along the path\n",
    "# given a position [x, y, z, and orientation as euler angles rx, ry, rz]\n",
    "tp_fixed = TrajectoryPt([x, y, z, rx, ry, rz])\n",
    "\n",
    "# the same point except the x-position may vary between 0.5 and 1.5 (nominal value = 1.0)\n",
    "xt = TolerancedNumber(1.0, 0.5, 1.5)\n",
    "tp_tol = TrajectoryPt([xt, y, z, rx, ry, rz])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Build in special cases\n",
    "\n",
    "There are different types of toleranced for wich we have default functions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pyquaternion import Quaternion\n",
    "from acrobotics.path import FreeOrientationPt, TolOrientationPt\n",
    "\n",
    "# Only position specified, orientation is free\n",
    "tp_pos = FreeOrientationPt([x, y, z])\n",
    "\n",
    "q1 = Quaternion()\n",
    "# Orientation given as a quaternion\n",
    "# TrajectoryPt([x, y, z], quat = q1)\n",
    "\n",
    "# Tolerance on orientation given as a quaternion and max distance\n",
    "tp_quat_tol = TolOrientationPt([x, y, z], q1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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
}