diff --git a/Go2Py/assets/mujoco/go2.xml b/Go2Py/assets/mujoco/go2.xml index d4f9687..f155e12 100644 --- a/Go2Py/assets/mujoco/go2.xml +++ b/Go2Py/assets/mujoco/go2.xml @@ -103,6 +103,7 @@ + @@ -131,6 +132,8 @@ + + @@ -159,6 +162,7 @@ + @@ -187,6 +191,8 @@ + + @@ -211,6 +217,17 @@ + + + + + + + + + + + diff --git a/Go2Py/sim/mujoco.py b/Go2Py/sim/mujoco.py index 5537b1b..8671601 100644 --- a/Go2Py/sim/mujoco.py +++ b/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)+ \ diff --git a/examples/mujoco_test.ipynb b/examples/mujoco_test.ipynb index a876168..d9befa3 100644 --- a/examples/mujoco_test.ipynb +++ b/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": {