IMU and contact sensors added

Go2Py/assets/mujoco/go2.xml

@@ -103,6 +103,7 @@
             <geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
             <geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
             <geom name="FR" class="foot"/>
+            <site name="FR_foot_contact" pos="0 0 -0.213" size="0.02"/>
           </body>
         </body>
       </body>
@@ -131,6 +132,8 @@
             <geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
             <geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
             <geom name="FL" class="foot"/>
+            <site name="FL_foot_contact" pos="0 0 -0.213" size="0.02"/>
+
           </body>
         </body>
       </body>
@@ -159,6 +162,7 @@
             <geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
             <geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
             <geom name="RR" class="foot"/>
+            <site name="RR_foot_contact" pos="0 0 -0.213" size="0.02"/>
           </body>
         </body>
       </body>
@@ -187,6 +191,8 @@
             <geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/>
             <geom pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
             <geom name="RL" class="foot"/>
+            <site name="RL_foot_contact" pos="0 0 -0.213" size="0.02"/>
+
           </body>
         </body>
       </body>
@@ -211,6 +217,17 @@
     <motor class="knee" name="RL_calf" joint="RL_calf_joint"/>
   </actuator>
 
+  <sensor>
+    <!-- IMU -->
+    <accelerometer name="imu_accel" site="imu"/>
+    <gyro name="imu_gyro" site="imu"/>
+    <!-- Contact sensors for the feet -->
+    <touch name="FR_foot_contact" site="FR_foot_contact"/>
+    <touch name="FL_foot_contact" site="RL_foot_contact"/>
+    <touch name="RR_foot_contact" site="RR_foot_contact"/>
+    <touch name="RL_foot_contact" site="RL_foot_contact"/>
+  </sensor>
+
   <keyframe>
     <key name="home" qpos="0 0 0.27 1 0 0 0 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8"
       ctrl="0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8"/>

Go2Py/sim/mujoco.py

@@ -81,6 +81,15 @@ class Go2Sim:
     def getPose(self):
         return self.data.qpos[:3], self.data.qpos[3:7]
 
+    def getIMU(self):
+        return {
+                'accel':np.array(self.data.sensordata[0:3]),\
+                'gyro': np.array(self.data.sensordata[3:6])
+               }
+
+    def getFootContact(self):
+        return self.data.sensordata[6:10]
+
     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)+ \

examples/mujoco_test.ipynb

@@ -29,7 +29,7 @@
     "import mujoco\n",
     "import time\n",
     "q,dq = robot.getJointStates()\n",
-    "robot.sitDown()\n",
+    "robot.standUp()\n",
     "for i in range(100000):\n",
     "    q,dq = robot.getJointStates()\n",
     "    tau = 20*np.eye(12)@(robot.q0 - q).reshape(12,1)\n",
@@ -42,7 +42,9 @@
    "execution_count": null,
    "metadata": {},
    "outputs": [],
-   "source": []
+   "source": [
+    "robot.getFootContact(), robot.getIMU()"
+   ]
   }
  ],
  "metadata": {