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

mujoco simulator updated

This commit is contained in:
Rooholla-KhorramBakht 2024-03-15 22:39:10 -04:00
parent 82c06daac2
commit bebf6b8e6b
4 changed files with 152 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -78,33 +78,7 @@
<geom size="0.05 0.045" pos="0.285 0 0.01" type="cylinder" class="collision"/> <geom size="0.05 0.045" pos="0.285 0 0.01" type="cylinder" class="collision"/>
<geom size="0.047" pos="0.293 0 -0.06" class="collision"/> <geom size="0.047" pos="0.293 0 -0.06" class="collision"/>
<site name="imu" pos="-0.02557 0 0.04232"/> <site name="imu" pos="-0.02557 0 0.04232"/>
<body name="FL_hip" pos="0.1934 0.0465 0">
<inertial pos="-0.0054 0.00194 -0.000105" quat="0.497014 0.499245 0.505462 0.498237" mass="0.678"
diaginertia="0.00088403 0.000596003 0.000479967"/>
<joint name="FL_hip_joint" class="abduction"/>
<geom mesh="hip_0" material="metal" class="visual"/>
<geom mesh="hip_1" material="gray" class="visual"/>
<geom size="0.046 0.02" pos="0 0.08 0" quat="1 1 0 0" type="cylinder" class="collision"/>
<body name="FL_thigh" pos="0 0.0955 0">
<inertial pos="-0.00374 -0.0223 -0.0327" quat="0.829533 0.0847635 -0.0200632 0.551623" mass="1.152"
diaginertia="0.00594973 0.00584149 0.000878787"/>
<joint name="FL_thigh_joint" class="front_hip"/>
<geom mesh="thigh_0" material="metal" class="visual"/>
<geom mesh="thigh_1" material="gray" class="visual"/>
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
<body name="FL_calf" pos="0 0 -0.213">
<inertial pos="0.00629595 -0.000622121 -0.141417" quat="0.710672 0.00154099 -0.00450087 0.703508"
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
<joint name="FL_calf_joint" class="knee"/>
<geom mesh="calf_0" material="gray" class="visual"/>
<geom mesh="calf_1" material="black" class="visual"/>
<geom size="0.012 0.06" pos="0.008 0 -0.06" quat="0.994493 0 -0.104807 0" type="cylinder" class="collision"/>
<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"/>
</body>
</body>
</body>
<body name="FR_hip" pos="0.1934 -0.0465 0"> <body name="FR_hip" pos="0.1934 -0.0465 0">
<inertial pos="-0.0054 -0.00194 -0.000105" quat="0.498237 0.505462 0.499245 0.497014" mass="0.678" <inertial pos="-0.0054 -0.00194 -0.000105" quat="0.498237 0.505462 0.499245 0.497014" mass="0.678"
diaginertia="0.00088403 0.000596003 0.000479967"/> diaginertia="0.00088403 0.000596003 0.000479967"/>
@ -132,33 +106,35 @@
</body> </body>
</body> </body>
</body> </body>
<body name="RL_hip" pos="-0.1934 0.0465 0">
<inertial pos="0.0054 0.00194 -0.000105" quat="0.505462 0.498237 0.497014 0.499245" mass="0.678" <body name="FL_hip" pos="0.1934 0.0465 0">
<inertial pos="-0.0054 0.00194 -0.000105" quat="0.497014 0.499245 0.505462 0.498237" mass="0.678"
diaginertia="0.00088403 0.000596003 0.000479967"/> diaginertia="0.00088403 0.000596003 0.000479967"/>
<joint name="RL_hip_joint" class="abduction"/> <joint name="FL_hip_joint" class="abduction"/>
<geom mesh="hip_0" material="metal" class="visual" quat="4.63268e-05 0 1 0"/> <geom mesh="hip_0" material="metal" class="visual"/>
<geom mesh="hip_1" material="gray" class="visual" quat="4.63268e-05 0 1 0"/> <geom mesh="hip_1" material="gray" class="visual"/>
<geom size="0.046 0.02" pos="0 0.08 0" quat="0.707107 0.707107 0 0" type="cylinder" class="collision"/> <geom size="0.046 0.02" pos="0 0.08 0" quat="1 1 0 0" type="cylinder" class="collision"/>
<body name="RL_thigh" pos="0 0.0955 0"> <body name="FL_thigh" pos="0 0.0955 0">
<inertial pos="-0.00374 -0.0223 -0.0327" quat="0.829533 0.0847635 -0.0200632 0.551623" mass="1.152" <inertial pos="-0.00374 -0.0223 -0.0327" quat="0.829533 0.0847635 -0.0200632 0.551623" mass="1.152"
diaginertia="0.00594973 0.00584149 0.000878787"/> diaginertia="0.00594973 0.00584149 0.000878787"/>
<joint name="RL_thigh_joint" class="back_hip"/> <joint name="FL_thigh_joint" class="front_hip"/>
<geom mesh="thigh_0" material="metal" class="visual"/> <geom mesh="thigh_0" material="metal" class="visual"/>
<geom mesh="thigh_1" material="gray" class="visual"/> <geom mesh="thigh_1" material="gray" class="visual"/>
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/> <geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
<body name="RL_calf" pos="0 0 -0.213"> <body name="FL_calf" pos="0 0 -0.213">
<inertial pos="0.00629595 -0.000622121 -0.141417" quat="0.710672 0.00154099 -0.00450087 0.703508" <inertial pos="0.00629595 -0.000622121 -0.141417" quat="0.710672 0.00154099 -0.00450087 0.703508"
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/> mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
<joint name="RL_calf_joint" class="knee"/> <joint name="FL_calf_joint" class="knee"/>
<geom mesh="calf_0" material="gray" class="visual"/> <geom mesh="calf_0" material="gray" class="visual"/>
<geom mesh="calf_1" material="black" class="visual"/> <geom mesh="calf_1" material="black" class="visual"/>
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0" type="cylinder" class="collision"/> <geom size="0.012 0.06" pos="0.008 0 -0.06" quat="0.994493 0 -0.104807 0" type="cylinder" class="collision"/>
<geom size="0.011 0.0325" pos="0.02 0 -0.148" quat="0.999688 0 0.0249974 0" type="cylinder" class="collision"/> <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 pos="0 0 -0.213" mesh="foot" class="visual" material="black"/>
<geom name="RL" class="foot"/> <geom name="FL" class="foot"/>
</body> </body>
</body> </body>
</body> </body>
<body name="RR_hip" pos="-0.1934 -0.0465 0"> <body name="RR_hip" pos="-0.1934 -0.0465 0">
<inertial pos="0.0054 -0.00194 -0.000105" quat="0.499245 0.497014 0.498237 0.505462" mass="0.678" <inertial pos="0.0054 -0.00194 -0.000105" quat="0.499245 0.497014 0.498237 0.505462" mass="0.678"
diaginertia="0.00088403 0.000596003 0.000479967"/> diaginertia="0.00088403 0.000596003 0.000479967"/>
@ -186,6 +162,37 @@
</body> </body>
</body> </body>
</body> </body>
<body name="RL_hip" pos="-0.1934 0.0465 0">
<inertial pos="0.0054 0.00194 -0.000105" quat="0.505462 0.498237 0.497014 0.499245" mass="0.678"
diaginertia="0.00088403 0.000596003 0.000479967"/>
<joint name="RL_hip_joint" class="abduction"/>
<geom mesh="hip_0" material="metal" class="visual" quat="4.63268e-05 0 1 0"/>
<geom mesh="hip_1" material="gray" class="visual" quat="4.63268e-05 0 1 0"/>
<geom size="0.046 0.02" pos="0 0.08 0" quat="0.707107 0.707107 0 0" type="cylinder" class="collision"/>
<body name="RL_thigh" pos="0 0.0955 0">
<inertial pos="-0.00374 -0.0223 -0.0327" quat="0.829533 0.0847635 -0.0200632 0.551623" mass="1.152"
diaginertia="0.00594973 0.00584149 0.000878787"/>
<joint name="RL_thigh_joint" class="back_hip"/>
<geom mesh="thigh_0" material="metal" class="visual"/>
<geom mesh="thigh_1" material="gray" class="visual"/>
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" class="collision"/>
<body name="RL_calf" pos="0 0 -0.213">
<inertial pos="0.00629595 -0.000622121 -0.141417" quat="0.710672 0.00154099 -0.00450087 0.703508"
mass="0.241352" diaginertia="0.0014901 0.00146356 5.31397e-05"/>
<joint name="RL_calf_joint" class="knee"/>
<geom mesh="calf_0" material="gray" class="visual"/>
<geom mesh="calf_1" material="black" class="visual"/>
<geom size="0.013 0.06" pos="0.01 0 -0.06" quat="0.995004 0 -0.0998334 0" type="cylinder" class="collision"/>
<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"/>
</body>
</body>
</body>
</body> </body>
</worldbody> </worldbody>

2
Go2Py/robot/interface/dds.py
View File

@ -142,7 +142,7 @@ class GO2Real():
self.highcmd.twist.angular.z = _ω_z self.highcmd.twist.angular.z = _ω_z
self.highcmd_publisher.publish(self.highcmd) self.highcmd_publisher.publish(self.highcmd)
def setCommandsLow(self, q, dq, kp, kd, tau_ff): def setCommandsLow(self, q_des, dq_des, kp, kd, tau_ff):
assert q.size == dq.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12" assert q.size == dq.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12"
lowcmd = Go2pyLowCmd_( lowcmd = Go2pyLowCmd_(
q, q,

59
Go2Py/sim/mujoco.py
View File

@ -35,27 +35,64 @@ class Go2Sim:
self.renderer = None self.renderer = None
self.render = render self.render = render
self.step_counter = 0 self.step_counter = 0
self.q0 = np.array([-0.03479636, 1.26186061, -2.81310153,
0.03325212, 1.25883281, -2.78329301,
-0.34708387, 1.27193761, -2.8052032 ,
0.32040933, 1.27148342, -2.81436563])
self.pos0 = np.array([0., 0., 0.1])
self.rot0 = np.array([1., 0., 0., 0.])
self.reset()
mujoco.mj_step(self.model, self.data)
self.viewer.sync()
def reset(self): def reset(self):
self.q_nominal = np.array( self.q_nominal = np.hstack(
12*[0.] [self.pos0.squeeze(), self.rot0.squeeze(), self.q0.squeeze()]
) )
for i in range(12): self.data.qpos = self.q_nominal
self.data.qpos[i] = self.q_nominal[i] self.data.qvel = np.zeros(18)
def resetStanding(self):
self.q0 = np.array([ 0.00901526, 0.77832842, -1.56065452,
-0.00795561, 0.76754963, -1.56634164,
-0.05375515, 0.76681757, -1.53601146,
0.06183922, 0.75422204, -1.53229916])
self.pos0 = np.array([0., 0., 0.33])
self.rot0 = np.array([1., 0., 0., 0.])
self.reset()
mujoco.mj_step(self.model, self.data)
self.viewer.sync()
self.data.qpos[7] = 0.0 def resetSitting(self):
self.data.qpos[8] = 0.0 self.q0 = np.array([-0.03479636, 1.26186061, -2.81310153,
0.03325212, 1.25883281, -2.78329301,
-0.34708387, 1.27193761, -2.8052032 ,
0.32040933, 1.27148342, -2.81436563])
self.pos0 = np.array([0., 0., 0.1])
self.rot0 = np.array([1., 0., 0., 0.])
self.reset()
mujoco.mj_step(self.model, self.data)
self.viewer.sync()
def step(self, tau): def getJointStates(self):
return self.data.qpos[7:], self.data.qvel[6:]
def getPose(self):
return self.data.qpos[:3], self.data.qpos[3:7]
def setCommands(self, q_des, dq_des, kp, kd, 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)
self.data.ctrl[:] = tau.squeeze()
def step(self):
self.step_counter += 1 self.step_counter += 1
self.data.ctrl[:] = tau
mujoco.mj_step(self.model, self.data) mujoco.mj_step(self.model, self.data)
# Render every render_ds_ratio steps (60Hz GUI update) # Render every render_ds_ratio steps (60Hz GUI update)
if self.render and (self.step_counter%self.render_ds_ratio)==0: if self.render and (self.step_counter%self.render_ds_ratio)==0:
self.viewer.sync() self.viewer.sync()
return self.data.qpos, self.data.qvel
def close(self): def close(self):
self.viewer.close() self.viewer.close()

90
examples/mujoco_test.ipynb
View File

@ -2,7 +2,7 @@
"cells": [ "cells": [
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 3,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -12,69 +12,93 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 4,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
"env = Go2Sim()" "robot = Go2Sim()\n",
"robot.resetStanding()"
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": 5,
"metadata": {},
"outputs": [
{
"ename": "KeyboardInterrupt",
"evalue": "",
"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: "
]
}
],
"source": [
"import mujoco\n",
"import time\n",
"q,dq = robot.getJointStates()\n",
"robot.resetSitting()\n",
"for i in range(100000):\n",
" q,dq = robot.getJointStates()\n",
" tau = 20*np.eye(12)@(env.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": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
"data": { "data": {
"text/plain": [ "text/plain": [
"False" "(11,)"
] ]
}, },
"execution_count": 3, "execution_count": 9,
"metadata": {}, "metadata": {},
"output_type": "execute_result" "output_type": "execute_result"
} }
], ],
"source": [ "source": [
"env.step_counter+=1\n", "dq.shape"
"env.render and (env.step_counter%env.render_ds_ratio)==0\n"
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 10, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [
"import mujoco\n",
"import time\n",
"freq_list = []\n",
"for i in range(100000):\n",
" tick = time.time()\n",
" q,dq = env.step(np.zeros(12))\n",
" tock = time.time()\n",
" freq_list.append(1/(tock-tick))"
]
},
{
"cell_type": "code",
"execution_count": 14,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Using matplotlib backend: TkAgg\n"
]
}
],
"source": [ "source": [
"\n", "\n",
"import matplotlib.pyplot as plt\n", "import matplotlib.pyplot as plt\n",
"%matplotlib\n", "%matplotlib\n",
"_ = plt.hist(freq_list, bins=100)" "_ = 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,
"metadata": {},
"outputs": [],
"source": []
} }
], ],
"metadata": { "metadata": {