From b6a24a93d116db3f0dce31131a821506654529cd Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Fri, 5 Apr 2024 15:59:21 -0400 Subject: [PATCH] HysteresisContactDetector added --- Go2Py/estimation/__init__.py | 0 Go2Py/estimation/contact.py | 14 ++ Go2Py/robot/model.py | 13 +- examples/contact_detector.ipynb | 192 +++++++++++++++++++++----- examples/fsm_sim.ipynb | 12 +- examples/lowlevel_dds_interface.ipynb | 32 ++++- examples/walk_these_ways_sim.ipynb | 115 ++++++++++++--- 7 files changed, 308 insertions(+), 70 deletions(-) create mode 100644 Go2Py/estimation/__init__.py create mode 100644 Go2Py/estimation/contact.py diff --git a/Go2Py/estimation/__init__.py b/Go2Py/estimation/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Go2Py/estimation/contact.py b/Go2Py/estimation/contact.py new file mode 100644 index 0000000..bad1480 --- /dev/null +++ b/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" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "import matplotlib.pyplot as plt\n", + "plt.plot(-dataset[:,-1])\n", + "plt.plot(dataset_measured[:,0])\n", + "plt.legend(['estimated', 'sensor'])\n", + "plt.grid(True)" + ] + }, + { + "cell_type": "code", + "execution_count": 327, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[]" + ] + }, + "execution_count": 327, + "metadata": {}, + "output_type": "execute_result" + }, + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "plt.plot(dataset_contact_states[:,0])" + ] } ], "metadata": { @@ -118,7 +246,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.8.10" + "version": "3.8.18" } }, "nbformat": 4, diff --git a/examples/fsm_sim.ipynb b/examples/fsm_sim.ipynb index c247e1b..de27767 100644 --- a/examples/fsm_sim.ipynb +++ b/examples/fsm_sim.ipynb @@ -26,17 +26,9 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 3, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "controller timeout\n" - ] - } - ], + "outputs": [], "source": [ "robot.sitDownReset()\n", "fsm = FSM(robot, remote, safety_hypervisor)" diff --git a/examples/lowlevel_dds_interface.ipynb b/examples/lowlevel_dds_interface.ipynb index 7b7df0c..0ffb68b 100644 --- a/examples/lowlevel_dds_interface.ipynb +++ b/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", - " q = np.zeros(12) \n", + "for i in range(1000000):\n", + " # q = np.zeros(12) \n", " dq = np.zeros(12)\n", - " kp = np.zeros(12)\n", - " kd = np.zeros(12)\n", + " kp = np.ones(12)*20\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", diff --git a/examples/walk_these_ways_sim.ipynb b/examples/walk_these_ways_sim.ipynb index c686ed6..471fbc6 100644 --- a/examples/walk_these_ways_sim.ipynb +++ b/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\")" ] }, {