velocity controller is added to the highlevel simulator

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,

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,