{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "59a99259",
   "metadata": {},
   "source": [
    "# BotOp-2: Real robot operation checklist & first steps\n",
    "\n",
    "The BotOp interface should work equally for a simulated and real robot. Note that currently only a locally compiled robotic package supports connecting to a Franka robot and Realsense camera. Here some first cautious steps to get started with a real robot."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "28f075b1",
   "metadata": {},
   "source": [
    "### Basic Checklist for operating a Franka\n",
    "\n",
    "For a full franka setup description see https://frankaemika.github.io/docs/getting_started.html. Here just a minimal checklist for operating a Franka:\n",
    "\n",
    "* Is the a real time kernel running? Check `uname -r`. The kernel name should end with -rt*\n",
    "* Is the user in `dialout` and `realtime` group? Check with `groups`. If not, add with `sudo usermod -a -G realtime <username>` and `sudo usermod -a -G dialout <username>`, and log out and in (or reboot) and check again.\n",
    "* Is the robot ready? E.g. at https://172.16.0.2/desk/. Are joints unlocked and status Ready?\n",
    "* Test the console tool `ry-bot -real -up -home`. (See `ry-bot -h` for other test commands, e.g. homing.) Potential issues:\n",
    "   * `ry-bot: command not found`. Is it located in `~/.local/bin`? Do you have `export PATH=\"$HOME/.local/bin:$PATH\"` in your `.bashrc`?\n",
    "   * 'FCI mode is not activated'. Activate it in the Desk: Older versions: side panel, newer versions: hidden in a top menu.\n",
    "   * Stalls when connecting to Franka - are the Franka IP addresses ({\"172.16.0.2\", \"172.17.0.2\"} in franka.cpp) correct?\n",
    "   * Franka not Ready or can't move - check as above?\n",
    "   * Error message \"== ERROR:franka.cpp:init:356(-2) not implemented with this compiler options\". Do you use the pypi package instead of a locally compiled package? Did you enable USE_LIBFRANKA with cmake? Did you `make install`?\n",
    "   * Error message roughly like 'Franka uses wrong library version'. Do you have an old franka and need to install version 0.7.1 of the franka lib? (See `./install.sh -v 0.7.1 libfranka` in the README of `robotic`.)\n",
    "   * When executing the cell below, check whether `print(ry.compiled())` prints the correct compile time.\n",
    "   \n",
    "Finally, the `ry-bot` (usually installed in `~/.local/bin`) provides a minimalistic example for how to run the robot - have a look!"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fee0684d",
   "metadata": {},
   "source": [
    "## First robot motion, camera & pcl, and gripper motion"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eca938ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotic as ry\n",
    "import numpy as np\n",
    "import time\n",
    "print('robotic version:', ry.__version__, ry.compiled())"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e3280d7b",
   "metadata": {},
   "source": [
    "Also for a real robot we first load the configuration and maintain our own workspace configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "33f75e77",
   "metadata": {},
   "outputs": [],
   "source": [
    "C = ry.Config()\n",
    "C.addFile(ry.raiPath('../rai-robotModels/scenarios/pandaSingle.g'))\n",
    "pcl = C.addFrame('pcl', 'cameraWrist')\n",
    "C.view(False, 'this is your workspace data structure C -- NOT THE SIMULTATION')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f0a194da",
   "metadata": {},
   "source": [
    "Now we start botop with real=True. By passing the model configuration C, the system knows which and how many robots there should be an tries to connect with them."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f1119ecc",
   "metadata": {},
   "outputs": [],
   "source": [
    "# True = real robot!!\n",
    "bot = ry.BotOp(C, useRealRobot=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "570afef1",
   "metadata": {},
   "source": [
    "If that failed, you could `ry.params_print()` to see which global parameters were used and whether you should change them. See also the checklist above. Or continue with sim from here one:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bafdead7",
   "metadata": {},
   "outputs": [],
   "source": [
    "if bot.get_t()==0: #if the above failed, use a sim...\n",
    "    del bot\n",
    "    bot = ry.BotOp(C, useRealRobot=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b7d17dd0",
   "metadata": {},
   "source": [
    "A first **motion**:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "df4ea390",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = bot.get_qHome()\n",
    "q[1] = q[1] + .2\n",
    "\n",
    "bot.moveTo(q)\n",
    "bot.wait(C)\n",
    "print('first motion done')\n",
    "\n",
    "bot.moveTo(bot.get_qHome())\n",
    "bot.wait(C)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a845b0fb",
   "metadata": {},
   "source": [
    "Grabbing a **camera image & pcl**, adding the pcl to the workspace C:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1b2144a8",
   "metadata": {},
   "outputs": [],
   "source": [
    "pcl = C.getFrame(\"pcl\")\n",
    "pcl.setShape(ry.ST.pointCloud, [2]) # the size here is pixel size for display\n",
    "bot.sync(C)\n",
    "\n",
    "count = 0\n",
    "\n",
    "while bot.getKeyPressed()!=ord('q'):\n",
    "    image, depth, points = bot.getImageDepthPcl(\"cameraWrist\")\n",
    "    pcl.setPointCloud(points, image)\n",
    "    pcl.setColor([1,0,0])\n",
    "    bot.sync(C, .1)\n",
    "    \n",
    "    if bot.getTimeToEnd()<=0.:\n",
    "        bot.moveTo(q)\n",
    "        bot.moveTo(bot.get_qHome())\n",
    "        count = count + 1\n",
    "    if count>=3:\n",
    "        break"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e0563891",
   "metadata": {},
   "source": [
    "Closing & opening the **gripper**:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "79a8549c",
   "metadata": {},
   "outputs": [],
   "source": [
    "#slow close\n",
    "bot.gripperMove(ry._left, width=.0, speed=.1)\n",
    "\n",
    "while not bot.gripperDone(ry._left):\n",
    "    bot.sync(C)\n",
    "\n",
    "#fast open\n",
    "bot.gripperMove(ry._left, width=.08, speed=1.)\n",
    "\n",
    "while not bot.gripperDone(ry._left):\n",
    "    bot.sync(C)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0f41998c",
   "metadata": {},
   "source": [
    "Always shut down the robot properly by destroying the handle:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "10443e09",
   "metadata": {},
   "outputs": [],
   "source": [
    "del bot\n",
    "del C"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c1278498",
   "metadata": {},
   "source": [
    "## Advanced: Compliance & Force/Torque feedback"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6e82d8f8",
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotic as ry\n",
    "import numpy as np\n",
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aa1a93be",
   "metadata": {},
   "outputs": [],
   "source": [
    "C = ry.Config()\n",
    "C.addFile(ry.raiPath('scenarios/pandaSingle.g'))\n",
    "C.view(False, 'this is your workspace data structure C -- NOT THE SIMULTATION')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5844a69c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# True = real robot!!\n",
    "bot = ry.BotOp(C, True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fba6e622",
   "metadata": {},
   "source": [
    "After opening the robot, it is holding its position. Try moving it you can feel the gains."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "18468cf5",
   "metadata": {},
   "outputs": [],
   "source": [
    "C.view(True, 'floating=False, damping=True -- Try moving the robot by hand!\\n-- press any key to continue --')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "63ee0298",
   "metadata": {},
   "source": [
    "We can let it float (=setting the reference always to q_current) and turn off the damping, which makes the robot move more freely:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "953b9f3f",
   "metadata": {},
   "outputs": [],
   "source": [
    "bot.hold(floating=True, damping=False)\n",
    "C.view(True, 'floating=True, damping=False -- Try moving the robot by hand!\\n-- press any key to continue --')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f79a4d54",
   "metadata": {},
   "source": [
    "We can also float with daming:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5d51abbd",
   "metadata": {},
   "outputs": [],
   "source": [
    "bot.hold(floating=True, damping=True)\n",
    "C.view(True, 'floating=True, damping=True -- Try moving the robot by hand!\\n-- press any key to continue --')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3e838508",
   "metadata": {},
   "source": [
    "The `hold` methods above might be useful for kinestetic teaching or so (you can always keep C sync'ed and compute any features while moving the robot).\n",
    "\n",
    "But for autonomous compliant manipulation we want to follow a reference and impose compliance in following this reference *along some task space dimensions only*. I.e., a task space compliance."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "86cad3be",
   "metadata": {},
   "outputs": [],
   "source": [
    "bot.moveTo(bot.get_qHome(), 1.)\n",
    "bot.wait(C)\n",
    "\n",
    "while bot.getKeyPressed()!=ord('q'):\n",
    "    bot.sync(C, .1)\n",
    "    y, J = C.eval(ry.FS.position, [\"l_gripper\"], [[1,0,0]])\n",
    "    bot.setCompliance(J, 1.)\n",
    "    print(' direct:', J @ bot.get_tauExternal(),\n",
    "          ' pseudoInv:', np.linalg.pinv(J.T, rcond=1e-3) @ bot.get_tauExternal())\n",
    "\n",
    "bot.setCompliance([], 0.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "549b1e19",
   "metadata": {},
   "outputs": [],
   "source": [
    "bot.home(C)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "248e8726",
   "metadata": {},
   "outputs": [],
   "source": [
    "del bot\n",
    "del C"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5f3f852e",
   "metadata": {},
   "source": [
    "## Testing the depth camera\n",
    "\n",
    "The following grabs an attached realsense camera, which in our setup is attached by default to the wrist. The robot is put in floating mode, so that you can move the camera. At the same time, the point cloud is attached in the model configuration to the camera frame -- and forward kinematics displays it in world coordinates. That way you can visually see how well you configuration model is calibrated to your real world -- and how noisy the depth camera really is."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "09599476",
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotic as ry\n",
    "import numpy as np\n",
    "import time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b52de13c",
   "metadata": {},
   "outputs": [],
   "source": [
    "C = ry.Config()\n",
    "C.addFile(ry.raiPath('scenarios/pandaSingle.g'))\n",
    "C.view(False, 'this is your workspace data structure C -- NOT THE SIMULTATION')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "69b11c6a",
   "metadata": {},
   "outputs": [],
   "source": [
    "# True = real robot!!\n",
    "bot = ry.BotOp(C, True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9a4ad737",
   "metadata": {},
   "outputs": [],
   "source": [
    "bot.hold(floating=True, damping=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "41a1f137",
   "metadata": {},
   "outputs": [],
   "source": [
    "pclFrame = C.addFrame('pcl', 'cameraWrist')\n",
    "\n",
    "while bot.getKeyPressed()!=ord('q'):\n",
    "    rgb, depth, points = bot.getImageDepthPcl('cameraWrist')\n",
    "    pclFrame.setPointCloud(points, rgb)\n",
    "    \n",
    "    bot.sync(C, .1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "11284fdc",
   "metadata": {},
   "outputs": [],
   "source": [
    "del bot\n",
    "del C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2eb82692",
   "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}