gravity vector estimator in body frame is added

Go2Py/robot/interface/dds.py

@@ -16,6 +16,8 @@ from cyclonedds.sub import DataReader
 from cyclonedds.util import duration
 from Go2Py.unitree_go.msg.dds_ import LowState_
 from threading import Thread
+from scipy.spatial.transform import Rotation
+
 
 class GO2Real():
     def __init__(
@@ -190,4 +192,10 @@ class GO2Real():
         return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max)
 
     def overheat(self):
-        return False
+        return False
+
+    def getGravityInBody(self):
+        q = self.getIMU()['quat']
+        R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
+        g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1)
+        return g_in_body

Go2Py/robot/interface/ros2.py

@@ -14,6 +14,7 @@ from Go2Py.joy import xKeySwitch, xRockerBtn
 from geometry_msgs.msg import TwistStamped
 from Go2Py.msgs.unitree_go.msg import LowState, Go2pyLowCmd
 from nav_msgs.msg import Odometry   
+from scipy.spatial.transform import Rotation
 
 
 
@@ -253,4 +254,10 @@ class GO2Real(Node):
         return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max)
 
     def overheat(self):
-        return False
+        return False
+
+    def getGravityInBody(self):
+        q = self.getIMU()['quat']
+        R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
+        g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1)
+        return g_in_body

examples/lowlevel_dds_interface.ipynb

@@ -46,17 +46,21 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
-   "source": []
+   "source": [
+    "from scipy.spatial.transform import Rotation"
+   ]
   },
   {
    "cell_type": "code",
    "execution_count": null,
    "metadata": {},
    "outputs": [],
-   "source": []
+   "source": [
+    "Rotation.from_matrix(np.eye(3)).as_quat()"
+   ]
   }
  ],
  "metadata": {
@@ -75,7 +79,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.8.10"
+   "version": "3.8.18"
   }
  },
  "nbformat": 4,

examples/walk_these_ways_sim.ipynb

@@ -84,8 +84,8 @@
     "    if i % (control_dt // simulation_dt) == 0:\n",
     "        obs, ret, done, info = agent.step(action)\n",
     "    robot.step()\n",
-    "    command_profile.yaw_vel_cmd = 1.2\n",
+    "    command_profile.yaw_vel_cmd = 0.0\n",
-    "    command_profile.x_vel_cmd = 0.8\n",
+    "    command_profile.x_vel_cmd = 0.0\n",
     "    command_profile.y_vel_cmd = 0.0\n",
     "    command_profile.stance_width_cmd=0.2\n",
     "    command_profile.footswing_height_cmd=-0.05\n",
@@ -102,7 +102,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -116,7 +116,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -134,21 +134,22 @@
     "    def init(self):\n",
     "        self.obs = self.agent.reset()\n",
     "        self.policy_info = {}\n",
-    "\n",
+    "        self.command_profile.yaw_vel_cmd = 0.0\n",
-    "    def update(self, robot, remote):\n",
+    "        self.command_profile.x_vel_cmd = 0.0\n",
-    "        action = self.policy(self.obs, self.policy_info)\n",
-    "        self.obs, self.ret, self.done, self.info = self.agent.step(action)\n",
-    "        self.command_profile.yaw_vel_cmd = 1.2\n",
-    "        self.command_profile.x_vel_cmd = 0.8\n",
     "        self.command_profile.y_vel_cmd = 0.0\n",
     "        self.command_profile.stance_width_cmd=0.2\n",
     "        self.command_profile.footswing_height_cmd=-0.05\n",
-    "        self.command_profile.step_frequency_cmd = 2.5\n"
+    "        self.command_profile.step_frequency_cmd = 2.5\n",
+    "        self.command_profile.bodyHeight = 0.05\n",
+    "\n",
+    "    def update(self, robot, remote):\n",
+    "        action = self.policy(self.obs, self.policy_info)\n",
+    "        self.obs, self.ret, self.done, self.info = self.agent.step(action)\n"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -160,14 +161,48 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 4,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
+     ]
+    }
+   ],
    "source": [
     "checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
     "controller = walkTheseWaysController(robot, remote, checkpoint)\n",
     "fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
    ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [
+    {
+     "ename": "",
+     "evalue": "",
+     "output_type": "error",
+     "traceback": [
+      "\u001b[1;31mThe Kernel crashed while executing code in the the current cell or a previous cell. Please review the code in the cell(s) to identify a possible cause of the failure. Click <a href='https://aka.ms/vscodeJupyterKernelCrash'>here</a> for more info. View Jupyter <a href='command:jupyter.viewOutput'>log</a> for further details."
+     ]
+    }
+   ],
+   "source": [
+    "robot.standUpReset()\n",
+    "controller.command_profile.roll_cmd=0.3"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
   }
  ],
  "metadata": {