simulator bug fixed

Go2Py/sim/mujoco.py

@@ -53,7 +53,7 @@ class Go2Sim:
         self.data.qpos = self.q_nominal
         self.data.qvel = np.zeros(18)
     
-    def resetStanding(self):
+    def standUp(self):
         self.q0 = np.array([ 0.00901526,  0.77832842, -1.56065452,
                             -0.00795561,  0.76754963, -1.56634164,
                             -0.05375515,  0.76681757, -1.53601146,  
@@ -64,7 +64,7 @@ class Go2Sim:
         mujoco.mj_step(self.model, self.data)
         self.viewer.sync()
 
-    def resetSitting(self):
+    def sitDown(self):
         self.q0 = np.array([-0.03479636,  1.26186061, -2.81310153,
                              0.03325212,  1.25883281, -2.78329301,
                             -0.34708387,  1.27193761, -2.8052032 ,  
@@ -81,7 +81,7 @@ class Go2Sim:
     def getPose(self):
         return self.data.qpos[:3], self.data.qpos[3:7]
 
-    def setCommands(self, q_des, dq_des, kp, kd, tau_ff):
+    def setCommands(self, q_des, dq_des, kp, kv, tau_ff):
         q, dq = self.getJointStates()
         tau = np.diag(kp)@(q_des-q).reshape(12,1)+ \
               np.diag(kv)@(dq_des-dq).reshape(12,1)+tau_ff.reshape(12,1)

examples/mujoco_test.ipynb

@@ -2,7 +2,7 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -12,9 +12,21 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": 2,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "ename": "AttributeError",
+     "evalue": "'Go2Sim' object has no attribute 'resetStanding'",
+     "output_type": "error",
+     "traceback": [
+      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
+      "\u001b[0;31mAttributeError\u001b[0m                            Traceback (most recent call last)",
+      "Cell \u001b[0;32mIn[2], line 2\u001b[0m\n\u001b[1;32m      1\u001b[0m robot \u001b[38;5;241m=\u001b[39m Go2Sim()\n\u001b[0;32m----> 2\u001b[0m \u001b[43mrobot\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mresetStanding\u001b[49m()\n",
+      "\u001b[0;31mAttributeError\u001b[0m: 'Go2Sim' object has no attribute 'resetStanding'"
+     ]
+    }
+   ],
    "source": [
     "robot = Go2Sim()\n",
     "robot.resetStanding()"
@@ -22,20 +34,18 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 5,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [
     {
-     "ename": "KeyboardInterrupt",
-     "evalue": "",
+     "ename": "NameError",
+     "evalue": "name 'env' is not defined",
      "output_type": "error",
      "traceback": [
       "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
-      "\u001b[0;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
-      "Cell \u001b[0;32mIn[5], line 11\u001b[0m\n\u001b[1;32m      8\u001b[0m env\u001b[38;5;241m.\u001b[39msetCommands(np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), tau)\n\u001b[1;32m      9\u001b[0m \u001b[38;5;66;03m# tau[1:]=0\u001b[39;00m\n\u001b[1;32m     10\u001b[0m \u001b[38;5;66;03m# tau[0]=100*(-0.3-q[7])\u001b[39;00m\n\u001b[0;32m---> 11\u001b[0m \u001b[43menv\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mstep\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\n",
-      "File \u001b[0;32m~/projects/rooholla/locomotion/Go2Py/Go2Py/sim/mujoco.py:96\u001b[0m, in \u001b[0;36mstep\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m     94\u001b[0m     # Render every render_ds_ratio steps (60Hz GUI update)\n\u001b[1;32m     95\u001b[0m     if self.render and (self.step_counter%self.render_ds_ratio)==0:\n\u001b[0;32m---> 96\u001b[0m         self.viewer.sync()\n\u001b[1;32m     97\u001b[0m \n\u001b[1;32m     98\u001b[0m def close(self):\n",
-      "File \u001b[0;32m~/miniconda3/envs/b1-env/lib/python3.9/site-packages/mujoco/viewer.py:125\u001b[0m, in \u001b[0;36mHandle.sync\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m    123\u001b[0m sim \u001b[38;5;241m=\u001b[39m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_get_sim()\n\u001b[1;32m    124\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m sim \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[0;32m--> 125\u001b[0m   \u001b[43msim\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msync\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\n",
-      "\u001b[0;31mKeyboardInterrupt\u001b[0m: "
+      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
+      "Cell \u001b[0;32mIn[3], line 7\u001b[0m\n\u001b[1;32m      5\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m i \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mrange\u001b[39m(\u001b[38;5;241m100000\u001b[39m):\n\u001b[1;32m      6\u001b[0m     q,dq \u001b[38;5;241m=\u001b[39m robot\u001b[38;5;241m.\u001b[39mgetJointStates()\n\u001b[0;32m----> 7\u001b[0m     tau \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m20\u001b[39m\u001b[38;5;241m*\u001b[39mnp\u001b[38;5;241m.\u001b[39meye(\u001b[38;5;241m12\u001b[39m)\u001b[38;5;241m@\u001b[39m(\u001b[43menv\u001b[49m\u001b[38;5;241m.\u001b[39mq0 \u001b[38;5;241m-\u001b[39m q)\u001b[38;5;241m.\u001b[39mreshape(\u001b[38;5;241m12\u001b[39m,\u001b[38;5;241m1\u001b[39m)\n\u001b[1;32m      8\u001b[0m     robot\u001b[38;5;241m.\u001b[39msetCommands(np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m12\u001b[39m), tau)\n\u001b[1;32m      9\u001b[0m     env\u001b[38;5;241m.\u001b[39mstep()\n",
+      "\u001b[0;31mNameError\u001b[0m: name 'env' is not defined"
      ]
     }
    ],
@@ -43,56 +53,14 @@
     "import mujoco\n",
     "import time\n",
     "q,dq = robot.getJointStates()\n",
-    "robot.resetSitting()\n",
+    "robot.sitDown()\n",
     "for i in range(100000):\n",
     "    q,dq = robot.getJointStates()\n",
-    "    tau = 20*np.eye(12)@(env.q0 - q).reshape(12,1)\n",
+    "    tau = 20*np.eye(12)@(robot.q0 - q).reshape(12,1)\n",
     "    robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), tau)\n",
     "    env.step()"
    ]
   },
-  {
-   "cell_type": "code",
-   "execution_count": 9,
-   "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "(11,)"
-      ]
-     },
-     "execution_count": 9,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "dq.shape"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "\n",
-    "import matplotlib.pyplot as plt\n",
-    "%matplotlib\n",
-    "_ = plt.hist(freq_list, bins=100)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import mujoco\n",
-    "dir(mujoco.mjtObj)"
-   ]
-  },
   {
    "cell_type": "code",
    "execution_count": null,