examples/robot_interface_tutorial.ipynb

{
   "cells": [
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "# Robot Interface Example\n",
            "\n",
            "## C++ Driver\n",
            "\n",
            "This notebook shows how to connect to the robot and send joint velocity/torque commands to it. Before running the notebook, make sure you enable the FCI interface and run the C++ fr3_bridge as follows:\n",
            "\n",
            "**Joint Velocity Interface**\n",
            "```bash\n",
            "fr3_joint_interface 10.42.0.4 franka velocity\n",
            "```\n",
            "\n",
            "**Joint Torque Interface**\n",
            "```bash\n",
            "fr3_joint_interface 10.42.0.4 franka torque\n",
            "```\n",
            "\n",
            "**Note**: If the robot is connected to another computer, make sure that the two computers are on the same local network. Also use the `tools/multicast_config.py` tool to enable the multicast on both computers:\n",
            "\n",
            "```bash\n",
            "sudo tools/multicast_config.py <interface name>\n",
            "```\n",
            "where interface name is network interface that is on the same local network with the computer connected to the robot. (The computer running the real-time kernel)\n",
            "\n",
            "## Python Bridge\n",
            "\n",
            "Now that the interface is up, we can communicate with the robot through Python. Let's instantiate the communication manager:"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 13,
         "metadata": {},
         "outputs": [
            {
               "name": "stdout",
               "output_type": "stream",
               "text": [
                  "Interface Running...\n"
               ]
            }
         ],
         "source": [
            "import sys \n",
            "from FR3Py.robot.interface import FR3Real\n",
            "robot = FR3Real(robot_id='fr3')"
         ]
      },
      {
         "attachments": {},
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "If the interface the successfully exchanging data between the robot and the Python side, we should be able to read the state of the robot. Otherwise, we will get a None."
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 14,
         "metadata": {},
         "outputs": [
            {
               "data": {
                  "text/plain": [
                     "{'q': array([-6.51406912e-05, -7.85172720e-01,  2.84258223e-04, -2.35672392e+00,\n",
                     "         3.50003373e-04,  1.57029797e+00,  7.85698755e-01]),\n",
                     " 'dq': array([ 8.15426003e-05,  1.18722517e-04,  4.85325481e-04,  3.25609841e-04,\n",
                     "         3.49640579e-04, -4.79078854e-04, -4.39302759e-04]),\n",
                     " 'T': array([ 0.11434627, -4.27423239, -1.12868345, 23.50436783,  0.32335123,\n",
                     "         2.83811164, -0.31354305])}"
                  ]
               },
               "execution_count": 14,
               "metadata": {},
               "output_type": "execute_result"
            }
         ],
         "source": [
            "robot.getJointStates()"
         ]
      },
      {
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "Now that we successfully read the state, let's use a simple Jacobian pseudo inverse controller to command the robot's end-effector along the cartesian Z axis:"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": 15,
         "metadata": {},
         "outputs": [
            {
               "name": "stdout",
               "output_type": "stream",
               "text": [
                  "starting the controller\n"
               ]
            },
            {
               "ename": "AssertionError",
               "evalue": "q vector should be 9 or 7",
               "output_type": "error",
               "traceback": [
                  "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
                  "\u001b[0;31mAssertionError\u001b[0m                            Traceback (most recent call last)",
                  "Cell \u001b[0;32mIn[15], line 28\u001b[0m\n\u001b[1;32m     26\u001b[0m     q \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mhstack([q, np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m2\u001b[39m)])\n\u001b[1;32m     27\u001b[0m     dq \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mhstack([dq, np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m2\u001b[39m)])\n\u001b[0;32m---> 28\u001b[0m     cmd \u001b[38;5;241m=\u001b[39m \u001b[43mcontroller\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mcompute\u001b[49m\u001b[43m(\u001b[49m\u001b[43mq\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mdq\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mT_cmd\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mT\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m     29\u001b[0m     robot\u001b[38;5;241m.\u001b[39msetCommands(cmd[:\u001b[38;5;241m7\u001b[39m])\n\u001b[1;32m     31\u001b[0m robot\u001b[38;5;241m.\u001b[39msetCommands(np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m7\u001b[39m))\n",
                  "File \u001b[0;32m/home/FR3Py/FR3Py/controllers/jacobianPseudoInv.py:43\u001b[0m, in \u001b[0;36mWaypointController.compute\u001b[0;34m(self, q, dq, T_cmd)\u001b[0m\n\u001b[1;32m     40\u001b[0m q \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mhstack([q, np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m2\u001b[39m)])\n\u001b[1;32m     41\u001b[0m dq \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mhstack([dq, np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m2\u001b[39m)])\n\u001b[0;32m---> 43\u001b[0m info \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mrobot\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mgetInfo\u001b[49m\u001b[43m(\u001b[49m\u001b[43mq\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mdq\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m     44\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m T_cmd \u001b[38;5;129;01mis\u001b[39;00m \u001b[38;5;129;01mnot\u001b[39;00m \u001b[38;5;28;01mNone\u001b[39;00m:\n\u001b[1;32m     45\u001b[0m     \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mp_cmd \u001b[38;5;241m=\u001b[39m T_cmd[\u001b[38;5;241m0\u001b[39m:\u001b[38;5;241m3\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m]\u001b[38;5;241m.\u001b[39mreshape(\u001b[38;5;241m3\u001b[39m, \u001b[38;5;241m1\u001b[39m)\n",
                  "File \u001b[0;32m/home/FR3Py/FR3Py/robot/model.py:88\u001b[0m, in \u001b[0;36mPinocchioModel.getInfo\u001b[0;34m(self, q, dq)\u001b[0m\n\u001b[1;32m     85\u001b[0m _q \u001b[38;5;241m=\u001b[39m q\u001b[38;5;241m.\u001b[39msqueeze()\n\u001b[1;32m     86\u001b[0m _dq \u001b[38;5;241m=\u001b[39m dq\u001b[38;5;241m.\u001b[39msqueeze()\n\u001b[0;32m---> 88\u001b[0m \u001b[38;5;28;01massert\u001b[39;00m _q\u001b[38;5;241m.\u001b[39mshape \u001b[38;5;241m==\u001b[39m (\u001b[38;5;241m9\u001b[39m,) \u001b[38;5;129;01mor\u001b[39;00m _q\u001b[38;5;241m.\u001b[39mshape \u001b[38;5;241m==\u001b[39m (\u001b[38;5;241m7\u001b[39m,), \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mq vector should be 9 or 7\u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[1;32m     89\u001b[0m \u001b[38;5;28;01massert\u001b[39;00m _dq\u001b[38;5;241m.\u001b[39mshape \u001b[38;5;241m==\u001b[39m (\u001b[38;5;241m9\u001b[39m,) \u001b[38;5;129;01mor\u001b[39;00m _dq\u001b[38;5;241m.\u001b[39mshape \u001b[38;5;241m==\u001b[39m (\u001b[38;5;241m7\u001b[39m,), \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mdq vector should be 9 or 7,\u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[1;32m     91\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m q\u001b[38;5;241m.\u001b[39mshape[\u001b[38;5;241m0\u001b[39m] \u001b[38;5;241m==\u001b[39m \u001b[38;5;241m7\u001b[39m:\n",
                  "\u001b[0;31mAssertionError\u001b[0m: q vector should be 9 or 7"
               ]
            }
         ],
         "source": [
            "import time\n",
            "import numpy as np\n",
            "from FR3Py.controllers.jacobianPseudoInv import WaypointController\n",
            "\n",
            "controller = WaypointController(kp=1.5)\n",
            "# Read the initial state of the robot\n",
            "time.sleep(0.2)\n",
            "state = robot.getJointStates()\n",
            "if state is not None:\n",
            "    print('starting the controller')\n",
            "    q, dq = state['q'], state['dq']\n",
            "    q = np.hstack([q, np.zeros(2)])\n",
            "    dq = np.hstack([dq, np.zeros(2)])\n",
            "    p0 = controller.robot.getInfo(q,dq)['P_EE']\n",
            "    R0 = controller.robot.getInfo(q,dq)['R_EE']\n",
            "    T0 = np.vstack([np.hstack([R0, p0.reshape(3,1)]), np.array([0,0,0,1])])\n",
            "    \n",
            "    start_time = time.time()\n",
            "    while time.time()-start_time < 10:\n",
            "        time.sleep(0.01)\n",
            "        t = time.time() - start_time\n",
            "        x = 0.1 * np.sin(t)\n",
            "        T= T0 @ np.vstack([np.hstack([np.eye(3), np.array([0,0,x]).reshape(3,1)]), np.array([0,0,0,1])])\n",
            "        state = robot.getJointStates()\n",
            "        q, dq = state['q'], state['dq']\n",
            "        q = np.hstack([q, np.zeros(2)])\n",
            "        dq = np.hstack([dq, np.zeros(2)])\n",
            "        cmd = controller.compute(q, dq, T_cmd=T)\n",
            "        robot.setCommands(cmd[:7])\n",
            "\n",
            "    robot.setCommands(np.zeros(7))\n",
            "    print('Demo ended.')\n",
            "else:\n",
            "    print('State is None. check the connection')"
         ]
      },
      {
         "cell_type": "markdown",
         "metadata": {},
         "source": [
            "## Simple Joystick Control\n",
            "Now that we can control the pose of the end-effector in the task space, let's command the robot using a joystick. For this example, I use a Logitech Extreme 3D Pro joystick and read it using a helper class based on Pygame:"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": null,
         "metadata": {},
         "outputs": [],
         "source": [
            "from FR3Py.joysticks import PyGameJoyManager\n",
            "import time    \n",
            "joy1  = PyGameJoyManager()\n",
            "joy1.start_daq(joy_idx=0)\n",
            "joy1.offset_calibration()\n",
            "time.sleep(3)"
         ]
      },
      {
         "cell_type": "code",
         "execution_count": null,
         "metadata": {},
         "outputs": [],
         "source": [
            "import time\n",
            "import numpy as np\n",
            "import datetime\n",
            "from FR3Py.controllers.jacobianPseudoInv import WaypointController\n",
            "\n",
            "controller = WaypointController(kp=4)\n",
            "# Read the initial state of the robot\n",
            "time.sleep(0.2)\n",
            "state = robot.getStates()\n",
            "if state is not None:\n",
            "    print('starting the controller')\n",
            "    q, dq = state['q'], state['dq']\n",
            "    q = np.hstack([q, np.zeros(2)])\n",
            "    dq = np.hstack([dq, np.zeros(2)])\n",
            "    p0 = controller.robot.getInfo(q,dq)['P_EE']\n",
            "    R0 = controller.robot.getInfo(q,dq)['R_EE']\n",
            "    T0 = np.vstack([np.hstack([R0, p0.reshape(3,1)]), np.array([0,0,0,1])])\n",
            "\n",
            "    # Initialize the desired pose\n",
            "    x0, y0, z0 = 0.0, 0.0, 0.0 \n",
            "    R0 = np.eye(3)\n",
            "    start_time = time.time()\n",
            "    while time.time()-start_time < 10:\n",
            "        trans = controller.robot.getInfo(q,dq)['P_EE']\n",
            "        rot = controller.robot.getInfo(q,dq)['R_EE']\n",
            "        pose = np.vstack([np.hstack([rot, trans.reshape(3,1)]), np.array([0,0,0,1])])\n",
            "        \n",
            "        analog, digital = joy1.read_values()\n",
            "        cmd = np.array([analog[0],analog[1],analog[2]])\n",
            "        for i in range(3):\n",
            "            if np.abs(cmd[i])<0.1:\n",
            "                cmd[i] = 0 \n",
            "\n",
            "        if digital[0]==0:\n",
            "            y0 = y0 - cmd[0]*0.2/100\n",
            "            x0 = x0 - cmd[1]*0.2/100\n",
            "            z0 = z0 + cmd[2]*0.2/100\n",
            "        else:\n",
            "            omega_hat = np.array([[ 0,       -cmd[2],  cmd[1]],\n",
            "                                [ cmd[2],   0,      -cmd[0]],\n",
            "                                [-cmd[1],   cmd[0],      0]])\n",
            "            R0 = R0@(np.eye(3)+omega_hat/100)\n",
            "\n",
            "        time.sleep(0.01)\n",
            "        T= T0@np.vstack([np.hstack([R0, np.array([x0,y0,z0]).reshape(3,1)]), np.array([0,0,0,1])])\n",
            "        state = robot.getStates()\n",
            "        q, dq = state['q'], state['dq']\n",
            "        q = np.hstack([q, np.zeros(2)])\n",
            "        dq = np.hstack([dq, np.zeros(2)])\n",
            "        cmd = controller.compute(q,dq, T_cmd=T)\n",
            "        robot.setCommands(cmd[:7])\n",
            "    \n",
            "    robot.setCommands(np.zeros(7))\n",
            "    print('Demo ended.')\n",
            "else:\n",
            "    print('State is None. check the connection')"
         ]
      }
   ],
   "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.10.12"
      }
   },
   "nbformat": 4,
   "nbformat_minor": 2
}