mujoco simulator updated
This commit is contained in:
Rooholla-KhorramBakht
parent
82c06daac2
commit
bebf6b8e6b
|
|
@ -78,33 +78,7 @@
|
|||
<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"/>
|
||||
<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">
|
||||
<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"/>
|
||||
|
|
@ -132,33 +106,35 @@
|
|||
</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"/>
|
||||
<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">
|
||||
<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="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_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">
|
||||
<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="RL_calf_joint" class="knee"/>
|
||||
<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.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 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 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"
|
||||
diaginertia="0.00088403 0.000596003 0.000479967"/>
|
||||
|
|
@ -186,6 +162,37 @@
|
|||
</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>
|
||||
</worldbody>
|
||||
|
||||
|
|
|
|||
|
|
@ -142,7 +142,7 @@ class GO2Real():
|
|||
self.highcmd.twist.angular.z = _ω_z
|
||||
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"
|
||||
lowcmd = Go2pyLowCmd_(
|
||||
q,
|
||||
|
|
|
|||
|
|
@ -36,26 +36,63 @@ class Go2Sim:
|
|||
self.render = render
|
||||
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):
|
||||
self.q_nominal = np.array(
|
||||
12*[0.]
|
||||
self.q_nominal = np.hstack(
|
||||
[self.pos0.squeeze(), self.rot0.squeeze(), self.q0.squeeze()]
|
||||
)
|
||||
for i in range(12):
|
||||
self.data.qpos[i] = self.q_nominal[i]
|
||||
self.data.qpos = self.q_nominal
|
||||
self.data.qvel = np.zeros(18)
|
||||
|
||||
self.data.qpos[7] = 0.0
|
||||
self.data.qpos[8] = 0.0
|
||||
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()
|
||||
|
||||
def step(self, tau):
|
||||
def resetSitting(self):
|
||||
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 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.data.ctrl[:] = tau
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
# Render every render_ds_ratio steps (60Hz GUI update)
|
||||
if self.render and (self.step_counter%self.render_ds_ratio)==0:
|
||||
self.viewer.sync()
|
||||
|
||||
return self.data.qpos, self.data.qvel
|
||||
|
||||
|
||||
def close(self):
|
||||
self.viewer.close()
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
|
|
@ -12,69 +12,93 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"env = Go2Sim()"
|
||||
"robot = Go2Sim()\n",
|
||||
"robot.resetStanding()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/plain": [
|
||||
"False"
|
||||
"(11,)"
|
||||
]
|
||||
},
|
||||
"execution_count": 3,
|
||||
"execution_count": 9,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"env.step_counter+=1\n",
|
||||
"env.render and (env.step_counter%env.render_ds_ratio)==0\n"
|
||||
"dq.shape"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 10,
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"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": [
|
||||
"\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",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
|
|
|
|||
Loading…
Reference in New Issue