diff --git a/Go2Py/sim/mujoco.py b/Go2Py/sim/mujoco.py index ae2d202..1830f8a 100644 --- a/Go2Py/sim/mujoco.py +++ b/Go2Py/sim/mujoco.py @@ -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, diff --git a/examples/02-MuJoCo-sim.ipynb b/examples/02-MuJoCo-sim.ipynb index c0bea9b..d857a0f 100644 --- a/examples/02-MuJoCo-sim.ipynb +++ b/examples/02-MuJoCo-sim.ipynb @@ -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,