forked from maxshep/Exoboot_Code
-
Notifications
You must be signed in to change notification settings - Fork 1
/
state_machines.py
165 lines (138 loc) · 6.96 KB
/
state_machines.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
from typing import Type
import numpy as np
import constants
import controllers
import filters
import gait_state_estimators
from exoboot import Exo
import util
class HighLevelController():
'''A class that steps through controllers depending on, for instance, gait events.'''
def __init__(self,
exo: Type[Exo]):
self.exo = exo
def step(self, read_only):
'''Primary function to step through mid-level controllers.'''
raise ValueError('step() not written yet for this controller')
def update_ctrl_params_from_config(self, config):
'''A function to update mid-level control params from the config object.'''
raise ValueError(
'update_ctrl_params_from_config() not written yet for this controller')
class StandingPerturbationResponse(HighLevelController):
'''Pass through high level controller that implements standing perturbation response.'''
def __init__(self,
exo: Type[Exo],
standing_controller: Type[controllers.Controller],
slip_controller: Type[controllers.Controller],
slip_recovery_time: float = 1.5):
self.exo = exo
self.standing_controller = standing_controller
self.slip_controller = slip_controller
self.slip_ctrl_timer = util.DelayTimer(delay_time=slip_recovery_time)
self.controller_now = self.standing_controller
def step(self, read_only):
'''uses slip detector to detect slip onset, uses timer to stop slip controller.'''
if self.slip_ctrl_timer.check():
if self.exo.side == constants.Side.LEFT:
print('slip timeout--moving back now')
# If slip controller time has elapsed (goes True) and we need to switch back
self.slip_ctrl_timer.reset()
self.controller_now = self.standing_controller
did_controllers_switch = True
elif self.exo.data.did_slip:
self.slip_ctrl_timer.start()
self.controller_now = self.slip_controller
did_controllers_switch = True
else:
did_controllers_switch = False
if not read_only:
self.controller_now.command(reset=did_controllers_switch)
def update_ctrl_params_from_config(self, config):
self.slip_controller.update_ctrl_params_from_config(config=config)
class StanceSwingStateMachine(HighLevelController):
'''Unilateral state machine that takes in data, segments strides, and applies controllers'''
def __init__(self,
exo: Type[Exo],
stance_controller: Type[controllers.Controller],
swing_controller: Type[controllers.Controller]
):
'''A state machine object is associated with an exo, and reads/stores exo data, applies logic to
determine gait states and phases, chooses the correct controllers, and applies the
controller.'''
self.exo = exo
self.stance_controller = stance_controller
self.swing_controller = swing_controller
self.controller_now = self.swing_controller
def step(self, read_only=False):
# Check state machine transition criteria, switching controller_now if criteria are met
if (self.controller_now == self.swing_controller and
self.exo.data.did_heel_strike and
self.exo.data.gait_phase is not None):
self.controller_now = self.stance_controller
did_controllers_switch = True
elif self.exo.data.did_toe_off or self.exo.data.gait_phase is None:
self.controller_now = self.swing_controller
did_controllers_switch = True
else:
did_controllers_switch = False
if not read_only:
self.controller_now.command(reset=did_controllers_switch)
def update_ctrl_params_from_config(self, config):
self.stance_controller.update_ctrl_params_from_config(config=config)
class StanceSwingReeloutReelinStateMachine(HighLevelController):
'''Unilateral state machine that takes in data, segments strides, and applies controllers'''
def __init__(self,
exo: Exo,
stance_controller: Type[controllers.Controller],
swing_controller: Type[controllers.Controller],
reel_out_controller: Type[controllers.Controller],
reel_in_controller: Type[controllers.Controller],
swing_only=False
):
'''A state machine object is associated with an exo, and reads/stores exo data, applies logic to
determine gait states and phases, chooses the correct controllers, and applies the
controller.'''
self.exo = exo
self.stance_controller = stance_controller
self.swing_controller = swing_controller
self.reel_out_controller = reel_out_controller
self.reel_in_controller = reel_in_controller
self.controller_now = self.reel_out_controller
self.just_starting = True
self.swing_only = swing_only
def step(self, read_only=False):
# Check state machine transition criteria, switching controller_now if criteria are met
if self.just_starting:
self.controller_now = self.reel_out_controller
self.just_starting = False
did_controllers_switch = True
elif self.swing_only: # for swing only
self.controller_now = self.swing_controller
did_controllers_switch = True # update gains every time for now
elif (self.controller_now == self.swing_controller and
self.exo.data.did_heel_strike and
self.exo.data.gait_phase is not None):
self.controller_now = self.reel_in_controller
did_controllers_switch = True
self.exo.data.gen_var1 = 0
elif self.controller_now == self.reel_in_controller and self.reel_in_controller.check_completion_status():
self.controller_now = self.stance_controller
did_controllers_switch = True
self.exo.data.gen_var1 = 1
elif self.controller_now == self.stance_controller and (self.exo.data.did_toe_off or self.exo.data.gait_phase is None):
self.controller_now = self.reel_out_controller
did_controllers_switch = True
self.exo.data.gen_var1 = 2
elif self.controller_now == self.reel_out_controller and self.reel_out_controller.check_completion_status():
self.controller_now = self.swing_controller
did_controllers_switch = True
self.exo.data.gen_var1 = 3
else:
did_controllers_switch = False
if not read_only:
self.controller_now.command(reset=did_controllers_switch)
def update_ctrl_params_from_config(self, config):
self.stance_controller.update_ctrl_params_from_config(config=config)
if self.swing_only != config.SWING_ONLY:
self.swing_only = config.SWING_ONLY
print('Updated swing only to: ', self.swing_only)