EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

IMU and contact sensors added

This commit is contained in:
Rooholla-KhorramBakht 2024-03-16 00:27:06 -04:00
parent 9cbf6edabe
commit 30574a1c0b
3 changed files with 30 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
Go2Py/assets/mujoco/go2.xml
View File

@ -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"/>

9
Go2Py/sim/mujoco.py
View File

@ -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)+ \

6
examples/mujoco_test.ipynb
View File

@ -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": {