{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Lowlevel Control"
   ]
  },
  {
   "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.18)\n",
      "Hello from the pygame community. https://www.pygame.org/contribute.html\n"
     ]
    }
   ],
   "source": [
    "from Go2Py.robot.interface.dds import GO2Real\n",
    "import time\n",
    "robot = GO2Real(mode='lowlevel')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'q': array([-0.04851204,  1.25695562, -2.80253339,  0.05441612,  1.24787235,\n",
       "        -2.78187132, -0.34375334,  1.27221012, -2.80291271,  0.32734287,\n",
       "         1.27893162, -2.81329131]),\n",
       " 'dq': array([ 0.03875524,  0.0155021 ,  0.0323522 ,  0.07363495,  0.02712867,\n",
       "        -0.0161761 ,  0.01937762,  0.03487971,  0.00808805, -0.03487971,\n",
       "         0.        ,  0.02628616]),\n",
       " 'ddq': array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]),\n",
       " 'tau_est': array([-0.04947656,  0.04947656, -0.04741504, -0.04947656, -0.04947656,\n",
       "         0.        ,  0.        ,  0.04947656, -0.04741504,  0.04947656,\n",
       "        -0.07421485,  0.04741504]),\n",
       " 'temperature': array([34., 30., 29., 32., 30., 30., 36., 31., 29., 37., 31., 30.])}"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.getJointStates()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import time\n",
    "for i in range(1000000):\n",
    "    # q = np.zeros(12) \n",
    "    dq = np.zeros(12)\n",
    "    kp = np.ones(12)*0.0\n",
    "    kd = np.ones(12)*0.0\n",
    "    tau = np.zeros(12)\n",
    "    tau[0] = 0.0\n",
    "    robot.setCommands(q, dq, kp, kd, tau)\n",
    "    time.sleep(0.01)    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Highlevel Control"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "ename": "NotImplementedError",
     "evalue": "DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNotImplementedError\u001b[0m                       Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[4], line 3\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mGo2Py\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mrobot\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01minterface\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mdds\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m GO2Real\n\u001b[1;32m      2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mtime\u001b[39;00m\n\u001b[0;32m----> 3\u001b[0m robot \u001b[38;5;241m=\u001b[39m \u001b[43mGO2Real\u001b[49m\u001b[43m(\u001b[49m\u001b[43mmode\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mhighlevel\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m)\u001b[49m\n",
      "File \u001b[0;32m~/projects/rooholla/locomotion/Go2Py/Go2Py/robot/interface/dds.py:35\u001b[0m, in \u001b[0;36mGO2Real.__init__\u001b[0;34m(self, mode, vx_max, vy_max, ωz_max)\u001b[0m\n\u001b[1;32m     33\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mmode \u001b[38;5;241m=\u001b[39m mode\n\u001b[1;32m     34\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mmode \u001b[38;5;241m==\u001b[39m \u001b[38;5;124m'\u001b[39m\u001b[38;5;124mhighlevel\u001b[39m\u001b[38;5;124m'\u001b[39m:\n\u001b[0;32m---> 35\u001b[0m     \u001b[38;5;28;01mraise\u001b[39;00m \u001b[38;5;167;01mNotImplementedError\u001b[39;00m(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mDDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.\u001b[39m\u001b[38;5;124m'\u001b[39m)\n\u001b[1;32m     36\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msimulated \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mFalse\u001b[39;00m\n\u001b[1;32m     37\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mprestanding_q \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39marray([ \u001b[38;5;241m0.0\u001b[39m,  \u001b[38;5;241m1.26186061\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.5\u001b[39m,\n\u001b[1;32m     38\u001b[0m                             \u001b[38;5;241m0.0\u001b[39m,  \u001b[38;5;241m1.25883281\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.5\u001b[39m,\n\u001b[1;32m     39\u001b[0m                             \u001b[38;5;241m0.0\u001b[39m,  \u001b[38;5;241m1.27193761\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.6\u001b[39m,  \n\u001b[1;32m     40\u001b[0m                             \u001b[38;5;241m0.0\u001b[39m,  \u001b[38;5;241m1.27148342\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.6\u001b[39m])\n",
      "\u001b[0;31mNotImplementedError\u001b[0m: DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface."
     ]
    }
   ],
   "source": [
    "from Go2Py.robot.interface.dds import GO2Real\n",
    "import time\n",
    "robot = GO2Real(mode='highlevel')"
   ]
  }
 ],
 "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.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}