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

velocity controller is added to the highlevel simulator

This commit is contained in:
Rooholla-KhorramBakht 2024-05-27 10:49:55 -04:00
parent 735cd3ca46
commit 62df03d4ae
2 changed files with 145 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
Go2Py/sim/mujoco.py
View File

@ -95,6 +95,9 @@ class Go2Sim:
self.standUpReset()
self.step_counter = 0
self.step = self.stepHighlevel
self.ex_sum=0
self.ey_sum=0
self.e_omega_sum=0
else:
self.step = self.stepLowlevel
@ -104,6 +107,9 @@ class Go2Sim:
)
self.data.qpos = self.q_nominal
self.data.qvel = np.zeros(18)
self.ex_sum=0
self.ey_sum=0
self.e_omega_sum=0
def standUpReset(self):
self.q0 = self.standing_q
@ -160,18 +166,29 @@ class Go2Sim:
if self.render and (self.step_counter % self.render_ds_ratio) == 0:
self.viewer.sync()
def stepHighlevel(self, vx, vy, omega_z, body_z_offset=0):
def stepHighlevel(self, vx, vy, omega_z, body_z_offset=0, step_height = 0.08, kp=[2, 0.5, 0.5], ki=[0.02, 0.01, 0.01]):
policy_info = {}
if self.step_counter % (self.control_dt // self.dt) == 0:
action = self.policy(self.obs, policy_info)
self.obs, ret, done, info = self.agent.step(action)
#Body velocity tracker PI controller
_, q = self.getPose()
world_R_body = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
body_v = world_R_body.T@self.data.qvel[0:3].reshape(3,1)
ex = (vx-body_v[0])
ey = (vy-body_v[1])
e_omega = (omega_z-self.data.qvel[5])
self.ex_sum+=ex
self.ey_sum+=ey
self.e_omega_sum+=e_omega
self.command_profile.yaw_vel_cmd = np.clip(kp[2]*e_omega+ki[2]*self.e_omega_sum + omega_z, -2*np.pi, 2*np.pi)
self.command_profile.x_vel_cmd = np.clip(kp[0]*ex+ki[0]*self.ex_sum + vx, -2.5, 2.5)
self.command_profile.y_vel_cmd = np.clip(kp[1]*ey+ki[1]*self.ey_sum + vy,-1.5, 1.5)
self.command_profile.body_height_cmd = body_z_offset
self.command_profile.footswing_height_cmd = step_height
self.step_counter+=1
self.stepLowlevel()
self.command_profile.yaw_vel_cmd = omega_z
self.command_profile.x_vel_cmd = vx
self.command_profile.y_vel_cmd = vy
self.command_profile.body_height_cmd = body_z_offset
def getSiteJacobian(self, site_name):
id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, site_name)
@ -203,7 +220,7 @@ class Go2Sim:
m=self.model,
d=self.data,
pnt=pnt,
vec=vec.flatten(),
vec=vec_in_w.flatten(),
geomgroup=None,
flg_static=1,
bodyexclude=-1,

123
examples/02-MuJoCo-sim.ipynb
View File

@ -1,5 +1,12 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Lowlevel Simulation"
]
},
{
"cell_type": "code",
"execution_count": null,
@ -16,7 +23,7 @@
"metadata": {},
"outputs": [],
"source": [
"robot = Go2Sim()\n",
"robot = Go2Sim(mode='lowlevel')\n",
"robot.standUpReset()"
]
},
@ -44,6 +51,118 @@
" robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), tau)\n",
" robot.step()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Highlevel Simulation"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/usr/lib/python3/dist-packages/scipy/__init__.py:146: UserWarning: A NumPy version >=1.17.3 and <1.25.0 is required for this version of SciPy (detected version 1.26.4\n",
" warnings.warn(f\"A NumPy version >={np_minversion} and <{np_maxversion}\"\n"
]
}
],
"source": [
"from Go2Py.sim.mujoco import Go2Sim\n",
"import numpy as np"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
]
}
],
"source": [
"robot = Go2Sim(mode='highlevel')\n",
"robot.standUpReset()"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"frq: 0.06607245464787131 Hz\n",
"frq: 33.44393324455997 Hz\n",
"frq: 51.28137914170436 Hz\n",
"frq: 57.72269242943451 Hz\n",
"frq: 55.13307744886692 Hz\n",
"frq: 54.144503969534625 Hz\n",
"frq: 54.06006238238857 Hz\n",
"frq: 54.02315846417393 Hz\n",
"frq: 52.84362243612357 Hz\n",
"frq: 56.53615139914811 Hz\n",
"frq: 57.84608594913664 Hz\n",
"frq: 54.998610054811046 Hz\n",
"frq: 52.402598700649676 Hz\n",
"frq: 55.65542315755951 Hz\n",
"frq: 53.71872078279691 Hz\n",
"frq: 57.66079652465597 Hz\n",
"frq: 53.88919724534896 Hz\n",
"frq: 56.06533798505567 Hz\n",
"frq: 57.76641692375496 Hz\n",
"frq: 55.159904785702075 Hz\n"
]
}
],
"source": [
"import time\n",
"robot.standUpReset\n",
"for i in range(10000):\n",
" robot.step(0,0,0., step_height=0,kp=[2, 0.5, 0.5], ki=[0.02, 0.01, 0.01])\n",
" time.sleep(robot.dt)"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'pcd': array([[-1.00000000e+00, -0.00000000e+00, -0.00000000e+00],\n",
" [-9.99981138e-01, -6.14188251e-03, -0.00000000e+00],\n",
" [-9.99924555e-01, -1.22835333e-02, -0.00000000e+00],\n",
" ...,\n",
" [-9.99924555e-01, 1.22835333e-02, -0.00000000e+00],\n",
" [-9.99981138e-01, 6.14188251e-03, -0.00000000e+00],\n",
" [-1.00000000e+00, 2.44929360e-16, -0.00000000e+00]]),\n",
" 'geomid': array([-1, -1, -1, ..., -1, -1, -1], dtype=int32),\n",
" 'dist': array([-1., -1., -1., ..., -1., -1., -1.])}"
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"robot.getLidarData()"
]
}
],
"metadata": {
@ -62,7 +181,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.8.18"
"version": "3.10.12"
}
},
"nbformat": 4,