Go2Py_SIM/examples/02-MuJoCo-sim.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Lowlevel Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from Go2Py.sim.mujoco import Go2Sim\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = Go2Sim(mode='lowlevel')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<matplotlib.image.AxesImage at 0x7d4b807ef0d0>"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "camera_state = robot.getCameraState()\n",
    "plt.imshow(camera_state['rgb'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 800x800 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "from mpl_toolkits.mplot3d import proj3d\n",
    "\n",
    "state = robot.getCameraState(get_pointcloud=True)\n",
    "pcd = state['pointcloud']\n",
    "\n",
    "fig = plt.figure(figsize=(8, 8))\n",
    "ax = fig.add_subplot(111, projection='3d')\n",
    "\n",
    "ax.scatter(pcd[:,0], pcd[:,1], pcd[:,2])\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'q': array([-0.02473053,  1.26249091, -2.82722067,  0.04536265,  1.25053037,\n",
       "        -2.79265437, -0.30646674,  1.2828985 , -2.82216135,  0.26426387,\n",
       "         1.29362285, -2.84158745]),\n",
       " 'dq': array([ 0.11278784, -0.00277931,  0.5229731 , -0.1364931 , -0.00321283,\n",
       "         0.35066361, -0.11362303,  0.03049349,  0.49368981,  0.13078971,\n",
       "         0.03355434,  0.59049033]),\n",
       " 'tau_est': array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.])}"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.getJointStates()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "import mujoco\n",
    "import time\n",
    "import cv2\n",
    "camera_dt = 1./60.\n",
    "robot.standUpReset()\n",
    "start_time = time.time()\n",
    "step_counter = 0\n",
    "while time.time()-start_time < 10:\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()\n",
    "    if step_counter%(camera_dt//robot.dt)==0:\n",
    "        camera_state = robot.getCameraState()\n",
    "        cv2.imshow('rgb', cv2.cvtColor(camera_state['rgb'], cv2.COLOR_BGR2RGB))\n",
    "        cv2.waitKey(1)\n",
    "    step_counter+=1\n",
    "cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Highlevel Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "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": [],
   "source": [
    "import mujoco\n",
    "import time\n",
    "import cv2\n",
    "camera_dt = 1./60.\n",
    "robot.standUpReset()\n",
    "start_time = time.time()\n",
    "step_counter = 0\n",
    "while time.time()-start_time < 10:\n",
    "    robot.step(0,0,0., step_height=0,kp=[2, 0.5, 0.5], ki=[0.02, 0.01, 0.01])\n",
    "    if step_counter%(camera_dt//robot.dt)==0:\n",
    "        camera_state = robot.getCameraState()\n",
    "        cv2.imshow('rgb', cv2.cvtColor(camera_state['rgb'], cv2.COLOR_BGR2RGB))\n",
    "        cv2.waitKey(1)\n",
    "    step_counter+=1\n",
    "cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.getLaserScan()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "lidar = robot.getLaserScan(max_range=3.)\n",
    "idx = np.where(lidar['dist']!=-1)[0]\n",
    "plt.plot(lidar['dist'][idx])"
   ]
  }
 ],
 "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
}