diff --git a/examples/fsm.ipynb b/examples/fsm.ipynb index 208bae3..62cb9a7 100644 --- a/examples/fsm.ipynb +++ b/examples/fsm.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": [ @@ -22,7 +22,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -36,6 +36,9 @@ " def standUpDownSeq(self):\n", " return False\n", "\n", + " def flushStates(self):\n", + " pass\n", + "\n", "remote = BaseRemote()\n", "\n", "class KeyboardRemote(BaseRemote):\n", @@ -56,8 +59,18 @@ " except AttributeError:\n", " pass # Special keys (like space) will be handled here\n", "\n", + " \n", + " def _on_release(self, key):\n", + " try:\n", + " if key.char == 's': # Start sequence\n", + " self.start_seq_flag = False\n", + " elif key.char == 'u': # Stand up/down sequence\n", + " self.stand_up_down_seq_flag = False\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", + " with keyboard.Listener(on_press=self._on_press, on_release=self._on_release) as listener:\n", " listener.join()\n", "\n", " def startSeq(self):\n", @@ -70,12 +83,16 @@ " if self.stand_up_down_seq_flag:\n", " self.stand_up_down_seq_flag = False\n", " return True\n", - " return False" + " return False\n", + "\n", + " def flushStates(self):\n", + " self.stand_up_down_seq_flag = False\n", + " self.start_seq_flag = False" ] }, { "cell_type": "code", - "execution_count": 4, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -84,65 +101,9 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": null, "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: " - ] - } - ], + "outputs": [], "source": [ "import time\n", "while True:\n", @@ -152,16 +113,17 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ + "import time\n", "class SafetyHypervisor():\n", " def __init__(self, robot):\n", " self.robot = robot\n", "\n", " def unsafe(self):\n", - " return True\n", + " return False\n", " \n", " def controlTimeout(self):\n", " if time.time() - robot.latest_command_stamp > 0.1:\n", @@ -175,7 +137,16 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "safety.controlTimeout()" + ] + }, + { + "cell_type": "code", + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -186,17 +157,12 @@ "\n", "\n", "class FSM:\n", - " def __init__(self, robot, remote, safety_hypervisor):\n", + " def __init__(self, robot, remote, safety_hypervisor, user_controller_callback=None):\n", " self.robot = robot\n", " self.remote = remote\n", + " self.remote.flushStates()\n", " self.safety = safety_hypervisor\n", - "\n", - " self.states = [ \"pre_standing\", \\\n", - " \"standing\", \\\n", - " \"locked_stance\", \\\n", - " \"user_loop\" ,\\\n", - " \"sitting\", \\\n", - " \"damping\"]\n", + " self.user_controller_callback = user_controller_callback\n", "\n", " self.state = \"damping\"\n", "\n", @@ -205,7 +171,9 @@ " 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", + " self.fsm_dT = 1./50.\n", + " self.control_dT = 1./400.\n", + " self.dT = self.fsm_dT\n", "\n", " self.modes = {\"tracking\":self.trackingControlUpdate,\n", " \"damping\" :self.dampingControlUpdate,\n", @@ -223,6 +191,7 @@ " 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", + " # print(f'setting mode to {mode}')\n", " \n", " def moveTo(self, target, duration=0.5):\n", " # assert self.tracking_complete, 'The previous moveTo command is not completed yet!'\n", @@ -245,7 +214,8 @@ " 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", + " if self.user_controller_callback is not None:\n", + " user_controller_callback(robot, remote)\n", "\n", " def simUpdate(self):\n", " while self.running:\n", @@ -287,6 +257,7 @@ " # print(\"locked_stance\")\n", " if self.remote.startSeq():\n", " self.setMode(\"user\")\n", + " self.dT = self.control_dT\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", @@ -294,12 +265,15 @@ " self.state = \"sitting\"\n", "\n", " def user_loop(self):\n", - " print(\"user_loop\")\n", + " # print(\"user_loop\")\n", " if self.safety.unsafe():\n", + " self.dT = self.fsm_dT\n", " self.setMode(\"damping\")\n", " elif self.remote.standUpDownSeq() or self.safety.controlTimeout():\n", + " self.dT = self.fsm_dT\n", " self.moveTo(robot.standing_q, duration = 1)\n", - " self.state = \"locked_stance\"\n", + " self.timer = time.time()\n", + " self.state = \"switch_back_to_locked_stance\"\n", " else:\n", " self.state = \"user_loop\"\n", "\n", @@ -310,6 +284,11 @@ " self.state = 'damping'\n", " else:\n", " self.state = \"sitting\"\n", + "\n", + " def switch_back_to_locked_stance(self):\n", + " if time.time()-self.timer > 0.5:\n", + " # print(\"going back to locked stance\")\n", + " self.state = \"locked_stance\"\n", " \n", " def close(self):\n", " self.running = False" @@ -317,36 +296,9 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": null, "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" - ] - } - ], + "outputs": [], "source": [ "fsm = FSM(robot, remote, safety)" ] @@ -380,7 +332,7 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": null, "metadata": {}, "outputs": [], "source": [