{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext wurlitzer\n",
    "#^^^ the wurlitzer extension is used to capture C/C++ output to be displayed in the notebook\n",
    "#^^^ this is very useful for debugging, but it doesn't work on windows\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "from klampt import *\n",
    "from klampt.math import vectorops,so3,se3\n",
    "from klampt import vis\n",
    "from klampt.vis.ipython import EditConfig,EditPoint,EditTransform\n",
    "\n",
    "world = WorldModel()\n",
    "world.loadFile(\"../data/simulation_test_worlds/sensortest.xml\")\n",
    "vis.add(\"world\",world)\n",
    "\n",
    "robot = world.robot(0)\n",
    "cam = robot.sensor('rgbd_camera')\n",
    "\n",
    "vis.show()\n",
    "\n",
    "#Controls:\n",
    "#left mouse click to rotate the view\n",
    "#right click or ctrl+click to pan the view\n",
    "#mouse wheel or shift+click to zoom the view"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "vis.edit((\"world\",robot.getName()))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#this call renders the scene... since we are in Jupyter notebook this falls back to slow software rendering\n",
    "cam.kinematicSimulate(world,0.01)\n",
    "\n",
    "#this converts the sensed measurements to Numpy arrays\n",
    "from klampt.model import sensing\n",
    "rgb,depth = sensing.camera_to_images(cam)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "from matplotlib import pyplot as plt\n",
    "fig,axs = plt.subplots(1,2,figsize=(14,4))\n",
    "axs[0].imshow(rgb)\n",
    "axs[1].imshow(depth)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Hmm... nothing was showing up... let's try to debug where the sensor is located!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xform = cam.getTransformWorld()\n",
    "vis.add(\"cam_xform\",xform)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Scroll back up to the visualization window... Aha, the camera is pointing upright!  Let's edit the robot configuration so that the end effector is pointing toward the ball.  On the sliders above, choose Link 3 and set the joint position to -1.5."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, re-run the cell with cam.kinematicSimulate(), and re-run the cells generating matplotlib windows. You should now see the ball! Try moving the link around some more, and re-generate the images."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Software emulation is fine for small images, but let's see what happens when the sensor is larger:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "settings = cam.settings()\n",
    "for k in settings:\n",
    "    try:\n",
    "        print(k,\":\",cam.getSetting(k))\n",
    "    except Exception:\n",
    "        pass"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cam.setSetting('xres',str(640))\n",
    "cam.setSetting('yres',str(480))\n",
    "import time\n",
    "t0 = time.time()\n",
    "cam.kinematicSimulate(world,0.01)\n",
    "t1 = time.time()\n",
    "print(\"Rendered 640 x 480 image in time\",t1-t0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Hmm... this is much too slow.  Let's try something else."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Hardware accelerated rendering"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's set up hardware rendering using OpenGL.  Before you call any sensor simulation calls, you will need to set up an OpenGL window.  Klampt comes with PyQt and GLUT visualization backends that will do this for you.  Note that you will need to do this on a local machine, it won't work in Colab or Jupyterhub.  \n",
    "\n",
    "Restart the kernel, then **run the below cells**.  The first time the sensor is simulated, it might take a little more time as the data structures on your GPU are set up for the first time.  But afterwards, rendering should be quite fast (tens of milliseconds)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "from klampt import *\n",
    "from klampt.math import vectorops,so3,se3\n",
    "from klampt import vis\n",
    "from klampt.vis.ipython import EditConfig,EditPoint,EditTransform\n",
    "\n",
    "world = WorldModel()\n",
    "world.loadFile(\"../data/simulation_test_worlds/sensortest.xml\")\n",
    "vis.add(\"world\",world)\n",
    "\n",
    "robot = world.robot(0)\n",
    "q = robot.config\n",
    "q[3] = -1.6\n",
    "robot.config = q\n",
    "cam = robot.sensor('rgbd_camera')\n",
    "cam.setSetting('xres',str(640))\n",
    "cam.setSetting('yres',str(480))\n",
    "\n",
    "#key call: using GLUT or PyQt to open an OpenGL window\n",
    "#vis.init(\"GLUT\")\n",
    "vis.init(\"PyQt\")\n",
    "vis.add(\"world\",world)  #not really necessary -- the visualization window is not what the simulated camera \"sees\"; it only sees what's in the world\n",
    "vis.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#since GPU packages don't work nicely with multithreading, we will need to call the sensor\n",
    "#simulation inside the visualization thread using vis.threadCall(func)\n",
    "\n",
    "def simCam():\n",
    "    t0 = time.time()\n",
    "    cam.kinematicSimulate(world,0.01)\n",
    "    t1 = time.time()\n",
    "    print(\"Rendered 640 x 480 image in time\",t1-t0)\n",
    "\n",
    "vis.threadCall(simCam)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "from klampt.model import sensing\n",
    "rgb,depth = sensing.camera_to_images(cam)\n",
    "\n",
    "from matplotlib import pyplot as plt\n",
    "fig,axs = plt.subplots(1,2,figsize=(14,4))\n",
    "axs[0].imshow(rgb)\n",
    "axs[1].imshow(depth)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "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.10.12"
  },
  "latex_envs": {
   "LaTeX_envs_menu_present": true,
   "autoclose": false,
   "autocomplete": false,
   "bibliofile": "biblio.bib",
   "cite_by": "apalike",
   "current_citInitial": 1,
   "eqLabelWithNumbers": true,
   "eqNumInitial": 1,
   "hotkeys": {
    "equation": "Ctrl-E",
    "itemize": "Ctrl-I"
   },
   "labels_anchors": false,
   "latex_user_defs": false,
   "report_style_numbering": false,
   "user_envs_cfg": false
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}