{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Lowlevel Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from Go2Py.sim.mujoco import Go2Sim\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = Go2Sim(mode='lowlevel')\n",
    "robot.standUpReset()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.getJointStates()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import mujoco\n",
    "import time\n",
    "robot.standUpReset()\n",
    "for i in range(100000):\n",
    "    state = robot.getJointStates()\n",
    "    tau = 20*np.eye(12)@(robot.q0 - state['q']).reshape(12,1)\n",
    "    robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), tau)\n",
    "    robot.step()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Highlevel Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/usr/lib/python3/dist-packages/scipy/__init__.py:146: UserWarning: A NumPy version >=1.17.3 and <1.25.0 is required for this version of SciPy (detected version 1.26.4\n",
      "  warnings.warn(f\"A NumPy version >={np_minversion} and <{np_maxversion}\"\n"
     ]
    }
   ],
   "source": [
    "from Go2Py.sim.mujoco import Go2Sim\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
     ]
    }
   ],
   "source": [
    "robot = Go2Sim(mode='highlevel')\n",
    "robot.standUpReset()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "frq: 0.06607245464787131 Hz\n",
      "frq: 33.44393324455997 Hz\n",
      "frq: 51.28137914170436 Hz\n",
      "frq: 57.72269242943451 Hz\n",
      "frq: 55.13307744886692 Hz\n",
      "frq: 54.144503969534625 Hz\n",
      "frq: 54.06006238238857 Hz\n",
      "frq: 54.02315846417393 Hz\n",
      "frq: 52.84362243612357 Hz\n",
      "frq: 56.53615139914811 Hz\n",
      "frq: 57.84608594913664 Hz\n",
      "frq: 54.998610054811046 Hz\n",
      "frq: 52.402598700649676 Hz\n",
      "frq: 55.65542315755951 Hz\n",
      "frq: 53.71872078279691 Hz\n",
      "frq: 57.66079652465597 Hz\n",
      "frq: 53.88919724534896 Hz\n",
      "frq: 56.06533798505567 Hz\n",
      "frq: 57.76641692375496 Hz\n",
      "frq: 55.159904785702075 Hz\n"
     ]
    }
   ],
   "source": [
    "import time\n",
    "robot.standUpReset\n",
    "for i in range(10000):\n",
    "    robot.step(0,0,0., step_height=0,kp=[2, 0.5, 0.5], ki=[0.02, 0.01, 0.01])\n",
    "    time.sleep(robot.dt)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'pcd': array([[-1.00000000e+00, -0.00000000e+00, -0.00000000e+00],\n",
       "        [-9.99981138e-01, -6.14188251e-03, -0.00000000e+00],\n",
       "        [-9.99924555e-01, -1.22835333e-02, -0.00000000e+00],\n",
       "        ...,\n",
       "        [-9.99924555e-01,  1.22835333e-02, -0.00000000e+00],\n",
       "        [-9.99981138e-01,  6.14188251e-03, -0.00000000e+00],\n",
       "        [-1.00000000e+00,  2.44929360e-16, -0.00000000e+00]]),\n",
       " 'geomid': array([-1, -1, -1, ..., -1, -1, -1], dtype=int32),\n",
       " 'dist': array([-1., -1., -1., ..., -1., -1., -1.])}"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.getLidarData()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "b1-env",
   "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
}