{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "c59c0954d76d414d9f02f569711e6d40",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "KlamptWidgetAdaptor(scene={'geometries': [{'data': {'attributes': {'position': {'itemSize': 3, 'array': [-0.10…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "2e811df0a26247d0819753cbeebd90a9",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Playback(children=(HBox(children=(Button(description='Play', icon='play', style=ButtonStyle(), tooltip='Start …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "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",
    "\n",
    "#Python 2/3 compatibility\n",
    "from __future__ import print_function,division\n",
    "\n",
    "import time\n",
    "from klampt import *\n",
    "from klampt import vis\n",
    "from IPython.display import clear_output\n",
    "from klampt.vis.ipython import Playback\n",
    "\n",
    "world = WorldModel()\n",
    "if world.loadFile(\"../data/tx90scenario0.xml\"):\n",
    "    sim = Simulator(world)\n",
    "    vis.add(\"world\",world)\n",
    "    playback_widget = Playback(vis.scene())\n",
    "    \n",
    "    #If you'd like to show the print output from loading the file, comment out this line\n",
    "    clear_output()\n",
    "    vis.show()\n",
    "    display(playback_widget)\n",
    "    \n",
    "    #NOTE: if you are going to add/modify items to the world in the same cell that it is created, you will\n",
    "    #need to place all of those calls in a begin_rpc/end_rpc block\n",
    "    vis.addText(\"HUD1\",\"0\",position=(1,1))\n",
    "    vis.add(\"ghost\",world.robot(0).getConfig(),color=(1,0,0,0.5))\n",
    "else:\n",
    "    print(\"There was a problem loading the world file\")\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": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import random\n",
    "from klampt.plan.cspace import *\n",
    "from klampt.plan.robotcspace import RobotCSpace\n",
    "from klampt.model.collide import WorldCollider\n",
    "from klampt.model.trajectory import *\n",
    "\n",
    "#first time the plan-to is called\n",
    "next_move_time = 1.0\n",
    "#if this is too large, Jupyter will complain...\n",
    "planning_time_limit = 2.0\n",
    "\n",
    "robot = world.robot(0)\n",
    "\n",
    "def plan_to(qa,qb):\n",
    "    collider = WorldCollider(world)\n",
    "    cspace = RobotCSpace(robot,collider)\n",
    "    cspace.eps = 1e-2\n",
    "    optimizing = True\n",
    "    MotionPlan.setOptions(type=\"sbl\",connectionThreshold=2,perturbationRadius=2,shortcut=True)\n",
    "    planner = MotionPlan(cspace)\n",
    "    planner.setEndpoints(qa,qb)\n",
    "    t0 = time.time()\n",
    "    foundTime = None\n",
    "    path = None\n",
    "    vis.addText(\"HUD2\",\"Planning...\",position=(1,5))\n",
    "    while time.time() - t0 < planning_time_limit:\n",
    "        planner.planMore(1)\n",
    "        if foundTime == None:\n",
    "            path = planner.getPath()\n",
    "            if path != None and len(path) > 0:\n",
    "                foundTime = time.time()\n",
    "                vis.addText(\"HUD2\",\"Found first path in %gs\"%(foundTime-t0,),pos=(1,5))\n",
    "                if not optimizing:\n",
    "                    break\n",
    "    if foundTime != None:\n",
    "        if optimizing:\n",
    "            vis.addText(\"HUD2\",\"Optimized for another %gs\"%(time.time()-foundTime,),pos=(1,5))\n",
    "        path = planner.getPath()\n",
    "    else:\n",
    "        path = None\n",
    "    return path\n",
    "\n",
    "\n",
    "def control_loop(t,controller):\n",
    "    global next_move_time\n",
    "    vis.addText(\"HUD1\",\"%.2f\"%(t,),pos=(1,1))\n",
    "    if t+0.02 >= next_move_time:\n",
    "        print(\"Planning on next time step for %fs...\"%(planning_time_limit,))\n",
    "    if t >= next_move_time:\n",
    "        qmin,qmax = robot.getJointLimits()\n",
    "        q = [random.uniform(a,b) for (a,b) in zip(qmin,qmax)]\n",
    "        print(\"Calling plan to...\",q)\n",
    "        vis.setColor(\"ghost\",1,1,0,0.5)\n",
    "        vis.add(\"ghost\",q)\n",
    "        try:\n",
    "            path = plan_to(controller.getCommandedConfig(),q)\n",
    "        except Exception as e:\n",
    "            print(e)\n",
    "            path = None\n",
    "        if path != None:\n",
    "            vis.setColor(\"ghost\",0,1,0,0.5)\n",
    "            for i,q in enumerate(path):\n",
    "                if i == 1:\n",
    "                    controller.addMilestoneLinear(q)\n",
    "                elif i >= 2:\n",
    "                    controller.addMilestoneLinear(q)\n",
    "        else:\n",
    "            vis.setColor(\"ghost\",1,0,0,0.5)\n",
    "        #controller.setMilestone(q)\n",
    "        next_move_time += 5.0\n",
    "    pass\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "dt = 0.02\n",
    "\n",
    "def do_reset():\n",
    "    global next_move_time\n",
    "    sim.reset()\n",
    "    sim.updateWorld()\n",
    "    next_move_time = 2.0\n",
    "\n",
    "def do_advance():\n",
    "    global world,sim\n",
    "    if world.numRobots() > 0:\n",
    "        control_loop(sim.getTime(),sim.controller(0))  #call code in the above cell\n",
    "    #print(\"Simulating...\",dt)\n",
    "    sim.simulate(dt)\n",
    "    sim.updateWorld()\n",
    "    #print(\"Done.\")\n",
    "\n",
    "#this binds the playback widget buttons\n",
    "playback_widget.advance = do_advance\n",
    "playback_widget.reset = do_reset\n",
    "playback_widget.quiet = False"
   ]
  },
  {
   "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.5.2"
  },
  "latex_envs": {
   "LaTeX_envs_menu_present": true,
   "autoclose": false,
   "autocomplete": true,
   "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": 2
}