examples/notebooks/pinocchio_reachable_space.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "cd8639c0-f0fe-47f8-9754-0e5321efe327",
   "metadata": {},
   "source": [
    "# Pinocchio interactive example - Robot's reachable space approximation\n",
    "![](https://github.com/stack-of-tasks/pinocchio/raw/master/doc/images/pinocchio-logo-large.png)\n",
    "\n",
    "This is an example tutorial of how to setup `pinocchio` with `pycapacity` to calculate and visualise the robot capacities\n",
    "\n",
    "## Installing the necessary packages\n",
    "\n",
    "Install `jupyter` to run this notebook\n",
    "```\n",
    "pip install jupyter\n",
    "```\n",
    "Install the `pinocchio` library\n",
    "```\n",
    "pip install pin\n",
    "```\n",
    "Install an additional library with robot data `example_robot_data` provided by pinocchio community as well [more info](https://github.com/Gepetto/example-robot-data)\n",
    "```\n",
    "pip install example-robot-data\n",
    "```\n",
    "Finally install the visualisation library `meshcat` that is compatible with pinocchio simple and powerful visualisaiton library [more info](https://pypi.org/project/meshcat/)\n",
    "```\n",
    "pip install meshcat\n",
    "```\n",
    "Finally install `pycapacity` for the workspace analysis\n",
    "```bash\n",
    "pip install pycapacity\n",
    "```\n",
    "\n",
    "For interactive visualisation\n",
    "```\n",
    "pip install ipywidgets\n",
    "pip install ipympl\n",
    "```\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "1b71557f-22bc-4077-bbba-da11f57c1b31",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pinocchio as pin\n",
    "import numpy as np\n",
    "import time\n",
    "\n",
    "from example_robot_data import load\n",
    "\n",
    "## visualise the robot\n",
    "from pinocchio.visualize import MeshcatVisualizer\n",
    "\n",
    "## visualise the polytope and the ellipsoid\n",
    "import meshcat.geometry as g \n",
    "\n",
    "# import pycapacity \n",
    "import pycapacity as pycap"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "dbc54f4b-20d9-465e-9cc6-dcd79593e17c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# function visualising the polutope in meshcat\n",
    "def visualise_polytope(q, horizon_time =None, frame_name = None):\n",
    "\n",
    "    if horizon_time is None:\n",
    "        horizon_time = 0.2 # 200 ms by default\n",
    "        \n",
    "    if frame_name is None:\n",
    "        frame_id = robot.model.getFrameId(robot.model.frames[-1].name)\n",
    "    else:\n",
    "        frame_id = robot.model.getFrameId(frame_name)\n",
    "\n",
    "    # calculate the jacobian\n",
    "    pin.framesForwardKinematics(robot.model,data,q)\n",
    "    pin.computeJointJacobians(robot.model,data, q)\n",
    "    J = pin.getFrameJacobian(robot.model, data, frame_id, pin.LOCAL_WORLD_ALIGNED)\n",
    "    # use only position jacobian\n",
    "    J = J[:3,:]\n",
    "    # mass amtrix calculation\n",
    "    M = pin.crba(robot.model, data, q)\n",
    "    \n",
    "    # end-effector pose\n",
    "    Xee = data.oMf[frame_id]\n",
    "\n",
    "    # calculate the polytope\n",
    "    opt = {'calculate_faces':True}\n",
    "    # calculate the polytope\n",
    "    dx_poly = pycap.robot.reachable_space_approximation(\n",
    "        J=J,\n",
    "        M=M,\n",
    "        t_min=t_min,\n",
    "        t_max=t_max,\n",
    "        dq_min = dq_min,\n",
    "        dq_max=dq_max,\n",
    "        q_min=q_min,\n",
    "        q_max=q_max,\n",
    "        horizon=horizon_time,\n",
    "        q0 = q\n",
    "    )\n",
    "    # meshcat triangulated mesh\n",
    "    poly = g.TriangularMeshGeometry(vertices=dx_poly.vertices.T + Xee.translation, faces=dx_poly.face_indices)\n",
    "    viz.viewer['poly'].set_object(poly, g.MeshBasicMaterial(color=0x0022ff, wireframe=True, linewidth=3, opacity=0.2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "fba564f4-4e74-4c4b-ad8a-753f9de06cce",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7000/static/\n"
     ]
    },
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7000/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "\n",
    "# get panda robot usinf example_robot_data\n",
    "# try out other robots using: load('ur5') # load('ur10')\n",
    "robot = load('panda') \n",
    "data = robot.model.createData()\n",
    "\n",
    "# get joint position ranges\n",
    "q_max = robot.model.upperPositionLimit.T\n",
    "q_min = robot.model.lowerPositionLimit.T\n",
    "# get max velocity\n",
    "dq_max = robot.model.velocityLimit\n",
    "dq_min = -dq_max\n",
    "# get max torque\n",
    "t_max = robot.model.effortLimit.T\n",
    "t_min = -t_max\n",
    "\n",
    "# initial horizon time\n",
    "horizon_time = 0.2 #seconds\n",
    "\n",
    "# Use robot configuration.\n",
    "# q0 = np.random.uniform(q_min,q_max)\n",
    "q = (q_min+q_max)/2\n",
    "\n",
    "viz = MeshcatVisualizer(robot.model, robot.collision_model, robot.visual_model)\n",
    "# Start a new MeshCat server and client.\n",
    "viz.initViewer(open=True)\n",
    "# Load the robot in the viewer.\n",
    "viz.loadViewerModel()\n",
    "viz.display(q)\n",
    "# small time window for loading the model \n",
    "# if meshcat does not visualise the robot properly, augment the time\n",
    "# it can be removed in most cases\n",
    "time.sleep(0.2) \n",
    "\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "658810ec-24d0-4ee6-8218-9088c9707471",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "ec2228579768485fabc777cb30875b8c",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=0.0, description='q[0]', max=2.8973, min=-2.8973, step=0.01), FloatSli…"
      ]
     },
     "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",
    "          for i,q_1 in enumerate(q)}\n",
    "kwargs = kwargs | {'horizon time' :  FloatSlider( min = 0.01, max = 1, step = 0.01, value = horizon_time)}\n",
    "@interact(**kwargs)\n",
    "def update(**kwargs):\n",
    "    q = np.array([v  for v in kwargs.values()])\n",
    "    viz.display(q[:-1])\n",
    "    horizon_time = q[-1]\n",
    "    visualise_polytope(q[:-1], horizon_time)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9fbb8611-23c6-4e1a-8901-90ffa7f155cb",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.11.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}