From 57e401478be9c694660368bfd95d55316869a997 Mon Sep 17 00:00:00 2001
From: Rooholla-KhorramBakht <learn.kho@gmail.com>
Date: Wed, 20 Mar 2024 17:40:33 -0400
Subject: [PATCH] walk these ways real deployment without actuatornet

---
 Go2Py/robot/interface/dds.py        |   2 +
 examples/walk_these_ways_real.ipynb | 157 ++++++++++++++++++++++++++++
 2 files changed, 159 insertions(+)
 create mode 100644 examples/walk_these_ways_real.ipynb

diff --git a/Go2Py/robot/interface/dds.py b/Go2Py/robot/interface/dds.py
index 72ab0d1..7306c48 100644
--- a/Go2Py/robot/interface/dds.py
+++ b/Go2Py/robot/interface/dds.py
@@ -17,6 +17,8 @@ from cyclonedds.util import duration
 from Go2Py.unitree_go.msg.dds_ import LowState_
 from threading import Thread
 from scipy.spatial.transform import Rotation
+from Go2Py.joy import xKeySwitch, xRockerBtn
+
 
 
 class GO2Real():
diff --git a/examples/walk_these_ways_real.ipynb b/examples/walk_these_ways_real.ipynb
new file mode 100644
index 0000000..8e5c18e
--- /dev/null
+++ b/examples/walk_these_ways_real.ipynb
@@ -0,0 +1,157 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from Go2Py.robot.interface.dds import GO2Real\n",
+    "import numpy as np\n",
+    "robot = GO2Real(mode='lowlevel')"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from Go2Py.robot.fsm import FSM\n",
+    "from Go2Py.robot.remote import KeyboardRemote\n",
+    "from Go2Py.robot.safety import SafetyHypervisor\n",
+    "from Go2Py.control.walk_these_ways import *\n",
+    "import numpy as np"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "class walkTheseWaysController:\n",
+    "    def __init__(self, robot, remote, checkpoint):\n",
+    "        self.remote = remote\n",
+    "        self.robot = robot\n",
+    "        self.cfg = loadParameters(checkpoint)\n",
+    "        self.policy = Policy(checkpoint)\n",
+    "        self.command_profile = CommandInterface()\n",
+    "        self.agent = WalkTheseWaysAgent(self.cfg, self.command_profile, self.robot)\n",
+    "        self.agent = HistoryWrapper(self.agent)\n",
+    "        self.init()\n",
+    "\n",
+    "    def init(self):\n",
+    "        self.obs = self.agent.reset()\n",
+    "        self.policy_info = {}\n",
+    "        self.command_profile.yaw_vel_cmd = 0.0\n",
+    "        self.command_profile.x_vel_cmd = 0.0\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.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",
+    "        vy = robot.getRemoteState().rx\n",
+    "        vx = robot.getRemoteState().ry\n",
+    "        omega = robot.getRemoteState().lx*2.2\n",
+    "        self.command_profile.x_vel_cmd = vx\n",
+    "        self.command_profile.y_vel_cmd = vy\n",
+    "        self.command_profile.yaw_vel_cmd = omega\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "class BaseRemote:\n",
+    "    def __init__(self):\n",
+    "        pass\n",
+    "    def startSeq(self):\n",
+    "        return False\n",
+    "    def standUpDownSeq(self):\n",
+    "        return False\n",
+    "\n",
+    "    def flushStates(self):\n",
+    "        pass\n",
+    "\n",
+    "    def getEstop(self):\n",
+    "        return False\n",
+    "\n",
+    "class UnitreeRemote(BaseRemote):\n",
+    "    def __init__(self, robot):\n",
+    "        self.robot = robot\n",
+    "\n",
+    "    def startSeq(self):\n",
+    "        remote = self.robot.getRemoteState()\n",
+    "        if remote.btn.start:\n",
+    "            return True\n",
+    "        else:\n",
+    "            return False\n",
+    "\n",
+    "    def standUpDownSeq(self):\n",
+    "        remote = self.robot.getRemoteState()\n",
+    "        if remote.btn.L2 and remote.btn.A:\n",
+    "            return True\n",
+    "        else:\n",
+    "            return False\n",
+    "\n",
+    "    def getEstop(self):\n",
+    "        remote = self.robot.getRemoteState()\n",
+    "        if remote.btn.L2 and remote.btn.R2:\n",
+    "            return True\n",
+    "        else:\n",
+    "            return False\n",
+    "        "
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "remote = UnitreeRemote(robot)\n",
+    "safety_hypervisor = SafetyHypervisor(robot)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "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)\n",
+    "safety_hypervisor = SafetyHypervisor(robot)"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "b1-env",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.8.18"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}