From 592dfb3eb8663746495ea6853f91b8e8368906cf Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Sun, 17 Mar 2024 23:36:53 -0400 Subject: [PATCH] FSM added --- Go2Py/robot/fsm.py | 0 Go2Py/sim/mujoco.py | 54 +++-- examples/fsm.ipynb | 420 +++++++++++++++++++++++++++++++++++++ examples/mujoco_test.ipynb | 48 +---- 4 files changed, 460 insertions(+), 62 deletions(-) create mode 100644 Go2Py/robot/fsm.py create mode 100644 examples/fsm.ipynb diff --git a/Go2Py/robot/fsm.py b/Go2Py/robot/fsm.py new file mode 100644 index 0000000..e69de29 diff --git a/Go2Py/sim/mujoco.py b/Go2Py/sim/mujoco.py index fa3296a..11bacd6 100644 --- a/Go2Py/sim/mujoco.py +++ b/Go2Py/sim/mujoco.py @@ -36,10 +36,17 @@ class Go2Sim: self.render = render self.step_counter = 0 - self.q0 = np.array([-0.03479636, 1.26186061, -2.81310153, - 0.03325212, 1.25883281, -2.78329301, - -0.34708387, 1.27193761, -2.8052032 , - 0.32040933, 1.27148342, -2.81436563]) + self.sitting_q = np.array([ 0.0, 1.26186061, -2.81310153, + 0.0, 1.25883281, -2.78329301, + 0.0, 1.27193761, -2.8052032 , + 0.0, 1.27148342, -2.81436563]) + + self.standing_q = np.array([ 0.0, 0.77832842, -1.56065452, + 0.0, 0.76754963, -1.56634164, + 0.0, 0.76681757, -1.53601146, + 0.0, 0.75422204, -1.53229916]) + + self.q0 = self.sitting_q self.pos0 = np.array([0., 0., 0.1]) self.rot0 = np.array([1., 0., 0., 0.]) self.reset() @@ -50,6 +57,13 @@ class Go2Sim: self.jacr = np.zeros((3, self.nv)) self.M = np.zeros((self.nv, self.nv)) + self.q_des = np.zeros(12) + self.dq_des = np.zeros(12) + self.tau_ff = np.zeros(12) + self.kp = np.zeros(12) + self.kv = np.zeros(12) + self.latest_command_stamp = time.time() + def reset(self): self.q_nominal = np.hstack( [self.pos0.squeeze(), self.rot0.squeeze(), self.q0.squeeze()] @@ -57,22 +71,16 @@ class Go2Sim: self.data.qpos = self.q_nominal self.data.qvel = np.zeros(18) - def standUp(self): - self.q0 = np.array([ 0.00901526, 0.77832842, -1.56065452, - -0.00795561, 0.76754963, -1.56634164, - -0.05375515, 0.76681757, -1.53601146, - 0.06183922, 0.75422204, -1.53229916]) + def standUpReset(self): + self.q0 = self.standing_q self.pos0 = np.array([0., 0., 0.33]) self.rot0 = np.array([1., 0., 0., 0.]) self.reset() mujoco.mj_step(self.model, self.data) self.viewer.sync() - def sitDown(self): - self.q0 = np.array([-0.03479636, 1.26186061, -2.81310153, - 0.03325212, 1.25883281, -2.78329301, - -0.34708387, 1.27193761, -2.8052032 , - 0.32040933, 1.27148342, -2.81436563]) + def sitDownReset(self): + self.q0 = self.sitting_q self.pos0 = np.array([0., 0., 0.1]) self.rot0 = np.array([1., 0., 0., 0.]) self.reset() @@ -95,12 +103,19 @@ class Go2Sim: return self.data.sensordata[6:10] def setCommands(self, q_des, dq_des, kp, kv, tau_ff): - q, dq = self.getJointStates() - tau = np.diag(kp)@(q_des-q).reshape(12,1)+ \ - np.diag(kv)@(dq_des-dq).reshape(12,1)+tau_ff.reshape(12,1) - self.data.ctrl[:] = tau.squeeze() + self.q_des = q_des + self.dq_des = dq_des + self.kp = kp + self.kv = kv + self.tau_ff = tau_ff + self.latest_command_stamp = time.time() def step(self): + q, dq = self.getJointStates() + tau = np.diag(self.kp)@(self.q_des-q).reshape(12,1)+ \ + np.diag(self.kv)@(self.dq_des-dq).reshape(12,1)+self.tau_ff.reshape(12,1) + self.data.ctrl[:] = tau.squeeze() + self.step_counter += 1 mujoco.mj_step(self.model, self.data) # Render every render_ds_ratio steps (60Hz GUI update) @@ -121,5 +136,8 @@ class Go2Sim: 'nle':nle } + def overheat(self): + return False + def close(self): self.viewer.close() diff --git a/examples/fsm.ipynb b/examples/fsm.ipynb new file mode 100644 index 0000000..208bae3 --- /dev/null +++ b/examples/fsm.ipynb @@ -0,0 +1,420 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.sim.mujoco import Go2Sim\n", + "import numpy as np" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "robot = Go2Sim()\n", + "robot.sitDownReset()" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "import threading\n", + "from pynput import keyboard\n", + "class BaseRemote:\n", + " def __init__(self):\n", + " pass\n", + " def startSeq(self):\n", + " return False\n", + " def standUpDownSeq(self):\n", + " return False\n", + "\n", + "remote = BaseRemote()\n", + "\n", + "class KeyboardRemote(BaseRemote):\n", + " def __init__(self):\n", + " super().__init__()\n", + " self.start_seq_flag = False\n", + " self.stand_up_down_seq_flag = False\n", + " self.listener_thread = threading.Thread(target=self._listen_to_keyboard)\n", + " self.listener_thread.daemon = True\n", + " self.listener_thread.start()\n", + "\n", + " def _on_press(self, key):\n", + " try:\n", + " if key.char == 's': # Start sequence\n", + " self.start_seq_flag = True\n", + " elif key.char == 'u': # Stand up/down sequence\n", + " self.stand_up_down_seq_flag = True\n", + " except AttributeError:\n", + " pass # Special keys (like space) will be handled here\n", + "\n", + " def _listen_to_keyboard(self):\n", + " with keyboard.Listener(on_press=self._on_press) as listener:\n", + " listener.join()\n", + "\n", + " def startSeq(self):\n", + " if self.start_seq_flag:\n", + " self.start_seq_flag = False\n", + " return True\n", + " return False\n", + "\n", + " def standUpDownSeq(self):\n", + " if self.stand_up_down_seq_flag:\n", + " self.stand_up_down_seq_flag = False\n", + " return True\n", + " return False" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "remote = KeyboardRemote()" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n", + "False\n" + ] + }, + { + "ename": "KeyboardInterrupt", + "evalue": "", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[5], line 4\u001b[0m\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28;01mwhile\u001b[39;00m \u001b[38;5;28;01mTrue\u001b[39;00m:\n\u001b[1;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(remote\u001b[38;5;241m.\u001b[39mstandUpDownSeq())\n\u001b[0;32m----> 4\u001b[0m \u001b[43mtime\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msleep\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m0.1\u001b[39;49m\u001b[43m)\u001b[49m\n", + "\u001b[0;31mKeyboardInterrupt\u001b[0m: " + ] + } + ], + "source": [ + "import time\n", + "while True:\n", + " print(remote.standUpDownSeq())\n", + " time.sleep(0.1)" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [], + "source": [ + "class SafetyHypervisor():\n", + " def __init__(self, robot):\n", + " self.robot = robot\n", + "\n", + " def unsafe(self):\n", + " return True\n", + " \n", + " def controlTimeout(self):\n", + " if time.time() - robot.latest_command_stamp > 0.1:\n", + " print('controller timeout')\n", + " return True\n", + " else:\n", + " return False \n", + "\n", + "safety = SafetyHypervisor(robot)" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [], + "source": [ + "import threading\n", + "import numpy as np\n", + "import time\n", + "from enum import Enum\n", + "\n", + "\n", + "class FSM:\n", + " def __init__(self, robot, remote, safety_hypervisor):\n", + " self.robot = robot\n", + " self.remote = remote\n", + " self.safety = safety_hypervisor\n", + "\n", + " self.states = [ \"pre_standing\", \\\n", + " \"standing\", \\\n", + " \"locked_stance\", \\\n", + " \"user_loop\" ,\\\n", + " \"sitting\", \\\n", + " \"damping\"]\n", + "\n", + " self.state = \"damping\"\n", + "\n", + " self.tracking_kp = np.array(4*[200, 200, 350.]).reshape(12)\n", + " self.tracking_kv = np.array(12*[10.])\n", + " self.tracking_complete = True\n", + " self.damping_kv = np.array(12*[3.])\n", + " self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), self.damping_kv, np.zeros(12))\n", + " self.dT = 1./50.\n", + "\n", + " self.modes = {\"tracking\":self.trackingControlUpdate,\n", + " \"damping\" :self.dampingControlUpdate,\n", + " \"user\": self.userControlUpdate,\n", + " }\n", + " self.setMode(\"damping\")\n", + "\n", + " self.running = True\n", + " self.fsm_thread = threading.Thread(target = self.update)\n", + " self.fsm_thread.start()\n", + "\n", + " self.sim_thread = threading.Thread(target=self.simUpdate)\n", + " self.sim_thread.start()\n", + "\n", + " def setMode(self, mode):\n", + " assert mode in self.modes.keys(), 'the requested control update mode is not implemented'\n", + " self.updateCommands = self.modes[mode]\n", + " \n", + " def moveTo(self, target, duration=0.5):\n", + " # assert self.tracking_complete, 'The previous moveTo command is not completed yet!'\n", + " self.q_start = self.robot.getJointStates()[0]\n", + " self.q_target = target\n", + " self.time = 0.0\n", + " self.move_duration = duration\n", + " self.q_des = lambda t: [self.q_start + np.clip((t)/self.move_duration,0, 1)*(self.q_target - self.q_start), \\\n", + " True if np.clip((t)/self.move_duration,0, 1)==1 else False] # q_des(t), Movement finished\n", + " self.tracking_complete = False\n", + " self.setMode(\"tracking\")\n", + "\n", + " def trackingControlUpdate(self):\n", + " self.time +=self.dT\n", + " q_des, done = self.q_des(self.time)\n", + " self.robot.setCommands(q_des, np.zeros(12), self.tracking_kp, self.tracking_kv, np.zeros(12))\n", + " self.tracking_complete = done\n", + "\n", + " def dampingControlUpdate(self):\n", + " self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), self.damping_kv, np.zeros(12))\n", + "\n", + " def userControlUpdate(self):\n", + " pass\n", + "\n", + " def simUpdate(self):\n", + " while self.running:\n", + " self.robot.step()\n", + " time.sleep(robot.dt)\n", + " \n", + " def update(self):\n", + " while self.running:\n", + " getattr(self, self.state)()\n", + " time.sleep(self.dT)\n", + " self.updateCommands()\n", + " \n", + " def damping(self):\n", + " # print('damping')\n", + " if self.remote.standUpDownSeq():\n", + " self.moveTo(robot.sitting_q, 1)\n", + " self.state = \"pre_standing\"\n", + " else:\n", + " self.setMode(\"damping\")\n", + " self.state = \"damping\"\n", + "\n", + " def pre_standing(self):\n", + " # print(\"pre_stance\")\n", + " if self.tracking_complete:\n", + " self.moveTo(robot.standing_q, duration=2)\n", + " self.state = 'standing'\n", + " else:\n", + " self.state = \"pre_standing\"\n", + " \n", + " def standing(self):\n", + " # print(\"standing\")\n", + " if self.tracking_complete:\n", + " self.moveTo(robot.standing_q, duration=2)\n", + " self.state = 'locked_stance'\n", + " else:\n", + " self.state = \"standing\"\n", + "\n", + " def locked_stance(self):\n", + " # print(\"locked_stance\")\n", + " if self.remote.startSeq():\n", + " self.setMode(\"user\")\n", + " self.state = \"user_loop\"\n", + " self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12))\n", + " elif self.remote.standUpDownSeq() or robot.overheat():\n", + " self.moveTo(robot.sitting_q, duration = 3)\n", + " self.state = \"sitting\"\n", + "\n", + " def user_loop(self):\n", + " print(\"user_loop\")\n", + " if self.safety.unsafe():\n", + " self.setMode(\"damping\")\n", + " elif self.remote.standUpDownSeq() or self.safety.controlTimeout():\n", + " self.moveTo(robot.standing_q, duration = 1)\n", + " self.state = \"locked_stance\"\n", + " else:\n", + " self.state = \"user_loop\"\n", + "\n", + " def sitting(self):\n", + " # print('sitting')\n", + " if self.tracking_complete:\n", + " self.setMode(\"damping\")\n", + " self.state = 'damping'\n", + " else:\n", + " self.state = \"sitting\"\n", + " \n", + " def close(self):\n", + " self.running = False" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n", + "user_loop\n" + ] + } + ], + "source": [ + "fsm = FSM(robot, remote, safety)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "fsm.moveTo(robot.sitting_q, 1)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "fsm.moveTo(robot.standing_q, 2)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "fsm.setMode(\"damping\")" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [], + "source": [ + "fsm.close()\n", + "robot.sitDownReset()\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "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 +} diff --git a/examples/mujoco_test.ipynb b/examples/mujoco_test.ipynb index 4e8857b..b78685d 100644 --- a/examples/mujoco_test.ipynb +++ b/examples/mujoco_test.ipynb @@ -2,7 +2,7 @@ "cells": [ { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -12,7 +12,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -76,49 +76,9 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "(array([[ 1.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, -3.03599143e-01, 1.39266782e-01,\n", - " 0.00000000e+00, -3.02750500e-01, -1.51075728e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 1.00000000e+00, 0.00000000e+00,\n", - " 3.03599143e-01, 0.00000000e+00, 1.94005151e-01,\n", - " 3.03599143e-01, 5.45551624e-06, 1.35362601e-03,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", - " [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00,\n", - " -1.39266782e-01, -1.94005151e-01, 0.00000000e+00,\n", - " -9.27667816e-02, -6.05126024e-04, -1.50144238e-01,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", - " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n", - " array([[0. , 0. , 0. , 1. , 0. ,\n", - " 0. , 1. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. ],\n", - " [0. , 0. , 0. , 0. , 1. ,\n", - " 0. , 0. , 0.99995936, 0.99995936, 0. ,\n", - " 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. ],\n", - " [0. , 0. , 0. , 0. , 0. ,\n", - " 1. , 0. , 0.00901514, 0.00901514, 0. ,\n", - " 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. ]]))" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "robot.getSiteJacobian('FR_foot')" ]