examples/notebooks/robotics_toolbox_swift.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "9f3a69a1-201b-44e9-b67d-3008b87298e5",
   "metadata": {},
   "source": [
    "# Robotics toolbox Interactive example using Swift backend\n",
    "![](https://github.com/petercorke/robotics-toolbox-python/raw/master/docs/figs/RobToolBox_RoundLogoB.png)\n",
    "\n",
    "\n",
    "## Installing the necessary packages\n",
    "Install `jupyter` to run this notebook\n",
    "```\n",
    "pip install jupyter\n",
    "```\n",
    "Install the robotics toolbox\n",
    "```\n",
    "pip install roboticstoolbox-python\n",
    "```\n",
    "If you still have not installed `pycapacity`, you can install it using\n",
    "```\n",
    "pip install pycapacity\n",
    "```\n",
    "\n",
    "For interactive visualisation\n",
    "```\n",
    "pip install ipywidgets\n",
    "pip install ipympl\n",
    "```\n",
    "\n",
    "### Swift does not remove old polytopes\n",
    "\n",
    "This is a problem of Swift, a bugfix was sent in this pull request: https://github.com/jhavl/swift/pull/49<br>\n",
    "Untill Swift has integrated the bugfix, please use this fork which has updated code: https://github.com/askuric/swift/tree/master<br>\n",
    "You can simply install it with\n",
    "```\n",
    "pip install git+https://github.com/askuric/swift.git\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "02d38ada-610b-455b-ac89-20e065825fc2",
   "metadata": {},
   "outputs": [],
   "source": [
    "import roboticstoolbox as rp\n",
    "import numpy as np\n",
    "import os\n",
    "\n",
    "# variable containing the reference of the polytope\n",
    "poly_mesh = None"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "31a6d704-55aa-492e-8f3c-b67771f2bbeb",
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "# polytope python module\n",
    "import pycapacity.robot as pyc\n",
    "# polytope visualisation\n",
    "import trimesh\n",
    "from spatialgeometry import Mesh\n",
    "\n",
    "# funciton, calculating and visualising the polytope\n",
    "def visualise_polytope(q, rob, env):\n",
    "    Jac = rob.jacob0(q)[:3,:]\n",
    "    # gravity torque\n",
    "    gravity = rob.gravload(q).reshape((-1,1))\n",
    "    \n",
    "    # calculate for the polytope\n",
    "    f_poly =  pyc.force_polytope(Jac, t_max, t_min, gravity)\n",
    "    f_poly.find_faces()\n",
    "    # save polytope as mesh file\n",
    "    scaling = 500\n",
    "    # create the mesh\n",
    "    mesh = trimesh.Trimesh(vertices=(f_poly.vertices.T/scaling + rob.fkine(q).t),\n",
    "                           faces=f_poly.face_indices, use_embree=True, validate=True)\n",
    "\n",
    "    # remove the old polytope files generated\n",
    "    file_dir = os.path.join(os.getcwd())\n",
    "    for i in os.listdir(file_dir):\n",
    "        if \"tmp_polytope\" in i:\n",
    "            os.remove(os.path.join(file_dir,i))\n",
    "    \n",
    "    # save the new polytope file\n",
    "    # in the stl format\n",
    "    file = os.path.join(file_dir,'tmp_polytope_file{}.stl'.format(int(100*time.time())))\n",
    "    f = open(file, \"wb\")\n",
    "    f.write(trimesh.exchange.stl.export_stl(mesh))\n",
    "    f.close()\n",
    "    # create the Mesh class from the file\n",
    "    # that is the only way to do it for the moment\n",
    "    # that's a pitty\n",
    "    poly_mesh = Mesh(file)\n",
    "    poly_mesh.color = (0.9,0.6,0.0,0.5)\n",
    "    # visualise the polytope\n",
    "    env.add(poly_mesh)\n",
    "    return poly_mesh"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "e906ac08-51ab-453c-8b0d-fd5b25b3e5a2",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "        <iframe\n",
       "            width=\"600\"\n",
       "            height=\"400\"\n",
       "            src=\"http://localhost:52000/?53002\"\n",
       "            frameborder=\"0\"\n",
       "            allowfullscreen\n",
       "            \n",
       "        ></iframe>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.lib.display.IFrame at 0x7f82e7f968f0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# robot for calculating the kinematics and jacobians\n",
    "robot = rp.models.DH.Panda()\n",
    "# initial pose\n",
    "q= np.array([0.00138894 ,5.98736e-05,-0.30259058,   -1.6, -6.64181e-05,    1.56995,-5.1812e-05])\n",
    "robot.q = q\n",
    "q_min,q_max = robot.qlim\n",
    "# joint torque limits\n",
    "t_max = np.array([87, 87, 87, 87, 20, 20, 20])\n",
    "t_min = -t_max\n",
    "\n",
    "\n",
    "# visualisation robot\n",
    "robot_vis = rp.models.Panda()\n",
    "import swift.Swift as Swift\n",
    "robot_vis.q = q\n",
    "env = Swift()\n",
    "env.launch(browser=\"notebook\") # if having an old version of swift replace with env.launch()\n",
    "env.add(robot_vis)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "1eb336b7-a0a7-490b-9fbd-04b824807d63",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "0e82735f0a9a4f69b7cb75c864f09f82",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=0.00138894, continuous_update=False, description='q[0]', max=2.8973, m…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from ipywidgets import interact, FloatSlider\n",
    "kwargs = {'q[{}]'.format(i) : \n",
    "          FloatSlider(\n",
    "              min = q_min[i], \n",
    "              max = q_max[i], \n",
    "              step = 0.01, \n",
    "              value = q[i],\n",
    "              continuous_update=False) \n",
    "          for i,q_1 in enumerate(q)}\n",
    "@interact(**kwargs)\n",
    "def update(**kwargs):\n",
    "    global poly_mesh\n",
    "    q = np.array([v  for v in kwargs.values()])\n",
    "    robot_vis.q = q\n",
    "    # remove the old polytope\n",
    "    if poly_mesh: # if having an old version of remove this condition\n",
    "        env.remove(poly_mesh)\n",
    "    # plot the new polytope\n",
    "    poly_mesh = visualise_polytope(q, robot, env)\n",
    "    # robot visualisation\n",
    "    env.step()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}