HysteresisContactDetector added

Go2Py/estimation/contact.py

@@ -0,0 +1,14 @@
+import numpy as np
+
+class HysteresisContactDetector:
+    def __init__(self, upper_limit, lower_limit):
+        self.upper_limit = upper_limit
+        self.lower_limit = lower_limit
+        self.contact_state = np.zeros(4)
+    
+    def update(self, contact_forces):
+        self.contact_state[np.where(contact_forces>self.upper_limit)[0]]=1
+        self.contact_state[np.where(contact_forces<self.lower_limit)[0]]=0
+
+    def getContactStates(self):
+        return self.contact_state

Go2Py/robot/model.py

@@ -156,8 +156,8 @@ class Go2Model:
         self.Minv_ = pin.computeMinverse(self.robot.model, self.robot.data, q_)[self.dq_reordering_idx,:]
         self.Minv_ = self.Minv_[:,self.dq_reordering_idx]
         for ef_frame in self.ef_frames:
-            # J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
+            J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
-            J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL)
+            # J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL)
             self.ef_J_[ef_frame]=J[:, self.dq_reordering_idx]
 
     def getInfo(self):
@@ -173,4 +173,11 @@ class Go2Model:
             'nle':self.nle_,
             'g':self.g_,
             'J':self.ef_J_,
-        }
+        }
+    
+    def getGroundReactionForce(self, tau_est, body_acceleration=None):
+        if body_acceleration is None:   
+            grf = {key:np.linalg.pinv(self.ef_J_[key][:3,6:].T)@(tau_est.squeeze() - self.nle_[6:]) for key in self.ef_J_.keys()}
+        else:
+            raise NotImplementedError("Ground reaction force with body dynamics is not implemented")
+        return grf

examples/fsm_sim.ipynb

@@ -26,17 +26,9 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 6,
+   "execution_count": 3,
    "metadata": {},
-   "outputs": [
+   "outputs": [],
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "controller timeout\n"
-     ]
-    }
-   ],
    "source": [
     "robot.sitDownReset()\n",
     "fsm = FSM(robot, remote, safety_hypervisor)"

examples/lowlevel_dds_interface.ipynb

@@ -2,9 +2,18 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 1,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n",
+      "Hello from the pygame community. https://www.pygame.org/contribute.html\n"
+     ]
+    }
+   ],
    "source": [
     "from Go2Py.robot.interface.dds import GO2Real\n",
     "import time\n",
@@ -13,17 +22,26 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "q = robot.getJointStates()['q']"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 4,
    "metadata": {},
    "outputs": [],
    "source": [
     "import numpy as np\n",
     "import time\n",
-    "for i in range(1000):\n",
+    "for i in range(1000000):\n",
-    "    q = np.zeros(12) \n",
+    "    # q = np.zeros(12) \n",
     "    dq = np.zeros(12)\n",
-    "    kp = np.zeros(12)\n",
+    "    kp = np.ones(12)*20\n",
-    "    kd = np.zeros(12)\n",
+    "    kd = np.ones(12)*0.3\n",
     "    tau = np.zeros(12)\n",
     "    tau[0] = -0.0\n",
     "    robot.setCommands(q, dq, kp, kd, tau)\n",

examples/walk_these_ways_sim.ipynb

@@ -9,7 +9,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -19,7 +19,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -29,7 +29,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -45,9 +45,17 @@
   },
   {
    "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": [
     "agent = WalkTheseWaysAgent(cfg, command_profile, robot)\n",
     "agent = HistoryWrapper(agent)"
@@ -55,7 +63,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 5,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -66,9 +74,20 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 6,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "frq: 15.967047985229458 Hz\n",
+      "frq: 42.47353444522081 Hz\n",
+      "frq: 43.69977078558033 Hz\n",
+      "frq: 40.20806211954177 Hz\n"
+     ]
+    }
+   ],
    "source": [
     "robot.reset()\n",
     "obs = agent.reset()\n",
@@ -93,7 +112,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -107,7 +126,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -116,7 +135,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -149,7 +168,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 4,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -160,9 +179,31 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 5,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
+     ]
+    },
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "frq: 0.10237921921805228 Hz\n",
+      "frq: 46.14348108298404 Hz\n",
+      "frq: 44.397794032030994 Hz\n",
+      "frq: 45.603148715941465 Hz\n",
+      "frq: 45.86595514341641 Hz\n",
+      "frq: 44.16591026356524 Hz\n",
+      "frq: 45.82636627843454 Hz\n",
+      "frq: 45.51950772169346 Hz\n"
+     ]
+    }
+   ],
    "source": [
     "checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
     "# checkpoint = \"/home/rstaion/projects/rooholla/walk-these-ways/runs/gait-conditioned-agility/pretrain-v0/train/025417.456545\"\n",
@@ -172,18 +213,56 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 7,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "frq: 45.741406386320016 Hz\n",
+      "frq: 44.6901430961184 Hz\n",
+      "frq: 44.451434445775085 Hz\n",
+      "frq: 45.19042385846963 Hz\n",
+      "frq: 45.1087737411542 Hz\n",
+      "frq: 46.690013692073066 Hz\n",
+      "frq: 45.09228519824546 Hz\n",
+      "frq: 45.70053825535531 Hz\n",
+      "frq: 46.01388873651992 Hz\n",
+      "frq: 44.94924554183813 Hz\n",
+      "frq: 45.49383372200228 Hz\n",
+      "frq: 44.522684330085134 Hz\n",
+      "frq: 44.42130458266699 Hz\n",
+      "frq: 43.71525649844704 Hz\n",
+      "frq: 44.37383889464886 Hz\n"
+     ]
+    }
+   ],
    "source": [
     "controller.command_profile.pitch_cmd=0.0\n",
-    "controller.command_profile.body_height_cmd=0.1\n",
+    "controller.command_profile.body_height_cmd=0.0\n",
     "controller.command_profile.footswing_height_cmd=0.08\n",
     "controller.command_profile.roll_cmd=0.0\n",
     "controller.command_profile.stance_width_cmd=0.2\n",
     "controller.command_profile.x_vel_cmd=-0.2\n",
     "controller.command_profile.y_vel_cmd=0.01\n",
-    "controller.command_profile.setGaitType(\"pronking\")"
+    "controller.command_profile.setGaitType(\"trotting\")"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "controller.command_profile.pitch_cmd=0.0\n",
+    "controller.command_profile.body_height_cmd=0.0\n",
+    "controller.command_profile.footswing_height_cmd=0.08\n",
+    "controller.command_profile.roll_cmd=0.0\n",
+    "controller.command_profile.stance_width_cmd=0.2\n",
+    "controller.command_profile.x_vel_cmd=-0.2\n",
+    "controller.command_profile.y_vel_cmd=0.01\n",
+    "controller.command_profile.setGaitType(\"trotting\")"
    ]
   },
   {