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

simulator bug fixed

This commit is contained in:
Rooholla-KhorramBakht 2024-03-15 22:41:55 -04:00
parent bebf6b8e6b
commit 9ea07ec6a8
2 changed files with 26 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Go2Py/sim/mujoco.py
View File

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

78
examples/mujoco_test.ipynb
View File

@ -2,7 +2,7 @@
"cells": [ "cells": [
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": 1,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -12,9 +12,21 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 4, "execution_count": 2,
"metadata": {}, "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": [ "source": [
"robot = Go2Sim()\n", "robot = Go2Sim()\n",
"robot.resetStanding()" "robot.resetStanding()"
@ -22,20 +34,18 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 5, "execution_count": 3,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
"ename": "KeyboardInterrupt", "ename": "NameError",
"evalue": "", "evalue": "name 'env' is not defined",
"output_type": "error", "output_type": "error",
"traceback": [ "traceback": [
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)", "\u001b[0;31mNameError\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", "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",
"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", "\u001b[0;31mNameError\u001b[0m: name 'env' is not defined"
"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: "
] ]
} }
], ],
@ -43,56 +53,14 @@
"import mujoco\n", "import mujoco\n",
"import time\n", "import time\n",
"q,dq = robot.getJointStates()\n", "q,dq = robot.getJointStates()\n",
"robot.resetSitting()\n", "robot.sitDown()\n",
"for i in range(100000):\n", "for i in range(100000):\n",
" q,dq = robot.getJointStates()\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", " robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), tau)\n",
" env.step()" " 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", "cell_type": "code",
"execution_count": null, "execution_count": null,