fsm notebook updated
This commit is contained in:
Rooholla-KhorramBakht
parent
592dfb3eb8
commit
1226e32616
|
|
@ -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",
|
||||
|
|
@ -311,42 +285,20 @@
|
|||
" 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"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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": [
|
||||
|
|
|
|||
Loading…
Reference in New Issue