Go2Py/examples/highlevel_ros2_interface.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Joystick Control\n",
    "As the first step after successfully communicating with the robot's onboard controller, we use a USB joystick (a Logitech Extreme 3D Pro) to command the robot with desired $x$, $y$, and $\\Psi$ velocities.\n",
    "\n",
    "First, use the Pygame-based joystick class provided with B1Py to connect to the joystick:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n",
      "Hello from the pygame community. https://www.pygame.org/contribute.html\n"
     ]
    }
   ],
   "source": [
    "from Go2Py.joy import Logitech3DPro\n",
    "# joy = Logitech3DPro(joy_id=0) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "***Note:*** Initially when you instantiate the joystick, the class performs an offset calibration to maker sure the neutral configuration of the device reads zero. It is important to leave the device untouched during the couple of seconds after calling the cell above. The prompts tells you when you can continue. \n",
    "\n",
    "Then as explained in [hardware interface documentation](unitree_locomotion_controller_interface.ipynb), instantiate and communication link to high-level controller on the robot. Note that the robot should be standing and ready to walk before we can control it. Furthermore, the the highlevel node in b1py_node ROS2 package (in `deploy/ros2_ws`) should be launched on the robot. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "pygame 2.5.2 (SDL 2.28.2, Python 3.8.10)\n",
      "Hello from the pygame community. https://www.pygame.org/contribute.html\n"
     ]
    }
   ],
   "source": [
    "from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
    "import time\n",
    "ros2_init()\n",
    "robot = GO2Real(mode='highlevel')\n",
    "ros2_exec_manager = ROS2ExecutorManager()\n",
    "ros2_exec_manager.add_node(robot)\n",
    "ros2_exec_manager.start()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, read the joystick values and send them to the robot in a loop:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Duration = 20\n",
    "N = Duration//0.01\n",
    "for i in range(500):\n",
    "    cmd = joy.readAnalog()\n",
    "    vx = cmd['x']\n",
    "    vy = cmd['y']\n",
    "    w = cmd['z'] / 2\n",
    "    body_height = cmd['aux'] / 10\n",
    "    robot.setCommands(vx,vy,w, bodyHeight=body_height)\n",
    "    time.sleep(0.01)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {},
   "outputs": [],
   "source": [
    "odometry = []\n",
    "imus = []\n",
    "joints = []\n",
    "foot_contacts = []\n",
    "stamps = []\n",
    "\n",
    "for i in range(60*200):\n",
    "    time.sleep(1/200)\n",
    "    odom = robot.getOdometry()\n",
    "    contact = robot.getFootContacts()\n",
    "    joint = robot.getJointStates()\n",
    "    imu = robot.getIMU()\n",
    "    imus.append(imu)\n",
    "    joints.append(joint)\n",
    "    foot_contacts.append(contact)\n",
    "    stamps.append(time.time())\n",
    "    odometry.append(odom)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pickle\n",
    "with open('data/april16-seq2.pkl', 'wb') as f:\n",
    "    pickle.dump(\n",
    "        {\n",
    "            'imu':imus,\n",
    "            'odometry':odometry,\n",
    "            'joints':joints,\n",
    "            'foot_contacts':foot_contacts,\n",
    "            'stamps':stamps\n",
    "        }, f\n",
    "    )"
   ]
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}