Finite State Machine Controller#
Overview#
The library ships with three example implementations of the same finite state machine walking controller.
The first implementation is all in Python, and it uses the state_machine modules from the controls subpackage of this library.
If you plan to write your controllers exclusively in Python, this example would be a good place to start.
The library also provides support for using compiled C and C++ library functions via the compiled_controller modules.
You can see a very basic example usage of this module on the tutorials page, which may be helpful to walk through before starting with this example (Compiled Controllers).
The source code for the compiled controllers (C++ and MATLAB implementations) is available in this repository.
Please refer to the documentation in that repository for information on how to compile both the C++ and MATLAB source code.
Python Implementation#
Setup and Configuraiton#
First, we’ll perform some standard imports.
import numpy as np
import opensourceleg.tools.units as units
from opensourceleg.control.state_machine import Event, State, StateMachine
from opensourceleg.osl import OpenSourceLeg
offline_mode = False # Set to true for debugging without hardware
Next, we’ll define all of the tunable FSM parameters. These include the impedance parameters for each state as well as the transitions between states.
# ------------- TUNABLE FSM PARAMETERS ---------------- #
BODY_WEIGHT = 60 * 9.8
# STATE 1: EARLY STANCE
KNEE_K_ESTANCE = 99.372
KNEE_B_ESTANCE = 3.180
KNEE_THETA_ESTANCE = 5
ANKLE_K_ESTANCE = 19.874
ANKLE_B_ESTANCE = 0
ANKLE_THETA_ESTANCE = -2
LOAD_LSTANCE: float = -1.0 * BODY_WEIGHT * 0.25
ANKLE_THETA_ESTANCE_TO_LSTANCE = 6.0
# STATE 2: LATE STANCE
KNEE_K_LSTANCE = 99.372
KNEE_B_LSTANCE = 1.272
KNEE_THETA_LSTANCE = 8
ANKLE_K_LSTANCE = 79.498
ANKLE_B_LSTANCE = 0.063
ANKLE_THETA_LSTANCE = -20
LOAD_ESWING: float = -1.0 * BODY_WEIGHT * 0.15
# STATE 3: EARLY SWING
KNEE_K_ESWING = 39.749
KNEE_B_ESWING = 0.063
KNEE_THETA_ESWING = 60
ANKLE_K_ESWING = 7.949
ANKLE_B_ESWING = 0.0
ANKLE_THETA_ESWING = 25
KNEE_THETA_ESWING_TO_LSWING = 50
KNEE_DTHETA_ESWING_TO_LSWING = 3
# STATE 4: LATE SWING
KNEE_K_LSWING = 15.899
KNEE_B_LSWING = 3.816
KNEE_THETA_LSWING = 5
ANKLE_K_LSWING = 7.949
ANKLE_B_LSWING = 0.0
ANKLE_THETA_LSWING = 15
LOAD_ESTANCE: float = -1.0 * BODY_WEIGHT * 0.4
KNEE_THETA_LSWING_TO_ESTANCE = 30
# ---------------------------------------------------- #
Note
These parameters were roughly tuned for a moderately paced walking gait. You may want to tune them to better suit your intended use case.
Next, we enter our main function for this script, run_FSM_controller(). We first instantiate an OSL object, add the joints, and add a loadcell.
def run_FSM_controller():
"""
This is the main function for this script.
It creates an OSL object and builds a state machine with 4 states.
It runs a main loop that updates the state machine based on the
hardware information and sends updated commands to the motors.
"""
osl = OpenSourceLeg(frequency=200)
osl.add_joint(name="knee", gear_ratio=41.4999, offline_mode=offline_mode)
osl.add_joint(name="ankle", gear_ratio=41.4999, offline_mode=offline_mode)
LOADCELL_MATRIX = np.array(
[
(-38.72600, -1817.74700, 9.84900, 43.37400, -44.54000, 1824.67000),
(-8.61600, 1041.14900, 18.86100, -2098.82200, 31.79400, 1058.6230),
(-1047.16800, 8.63900, -1047.28200, -20.70000, -1073.08800, -8.92300),
(20.57600, -0.04000, -0.24600, 0.55400, -21.40800, -0.47600),
(-12.13400, -1.10800, 24.36100, 0.02300, -12.14100, 0.79200),
(-0.65100, -28.28700, 0.02200, -25.23000, 0.47300, -27.3070),
]
)
osl.add_loadcell(
dephy_mode=False,
offline_mode=offline_mode,
loadcell_matrix=LOADCELL_MATRIX,
)
Note
If instantiating the OSL object is unfamiliar, check out the Adding an Actuator and Adding a Loadcell tutorial pages.
Then, we create a StateMachine instance. We’ve written a helper function to do this just to keep the code tidy, and we’ll get to that function later on.
fsm = build_4_state_FSM(osl)
Next, we configure the OSL log:
osl.log.add_attributes(container=osl, attributes=["timestamp"])
osl.log.add_attributes(
container=osl.knee,
attributes=[
"output_position",
"motor_current",
"joint_torque",
"motor_voltage",
"accelx",
],
)
osl.log.add_attributes(
container=osl.ankle,
attributes=[
"output_position",
"motor_current",
"joint_torque",
"motor_voltage",
"accelx",
],
)
osl.log.add_attributes(container=osl.loadcell, attributes=["fz"])
osl.log.add_attributes(container=fsm.current_state, attributes=["name"])
Main Loop#
Now that everything is set up, we will home the OSL and enter the main loop. During each iteration of the main loop, we call the update method for both the OSL and the FSM. We then write the current impedance parameters for each joint to the hardware. A print statement is also included for debugging.
with osl:
osl.home()
fsm.start()
for t in osl.clock:
osl.update()
fsm.update()
if osl.knee.mode != osl.knee.control_modes.impedance:
osl.knee.set_mode(mode=osl.knee.control_modes.impedance)
osl.knee.set_impedance_gains()
osl.knee.set_joint_impedance(
K=units.convert_to_default(
fsm.current_state.knee_stiffness,
units.stiffness.N_m_per_rad,
),
B=units.convert_to_default(
fsm.current_state.knee_damping,
units.damping.N_m_per_rad_per_s,
),
)
osl.knee.set_output_position(
position=units.convert_to_default(
fsm.current_state.knee_theta, units.position.deg
),
)
if osl.ankle.mode != osl.ankle.control_modes.impedance:
osl.ankle.set_mode(osl.ankle.control_modes.impedance)
osl.ankle.set_impedance_gains()
osl.ankle.set_joint_impedance(
K=units.convert_to_default(
fsm.current_state.ankle_stiffness,
units.stiffness.N_m_per_rad,
),
B=units.convert_to_default(
fsm.current_state.ankle_damping,
units.damping.N_m_per_rad_per_s,
),
)
osl.ankle.set_output_position(
position=units.convert_to_default(
fsm.current_state.ankle_theta, units.position.deg
),
)
print(
"Current time in state {}: {:.2f} seconds, Knee Eq {:.2f}, Ankle Eq {:.2f}, Fz {:.2f}".format(
fsm.current_state.name,
fsm.current_state.current_time_in_state,
fsm.current_state.knee_theta,
fsm.current_state.ankle_theta,
osl.loadcell.fz,
),
end="\r",
)
Note
The OSL library provides sensor values in default units. If your library expects other units,
you need to convert the values prior to assigning them. You can use the units module in the tools subpackage to do this.
For example you can convert from radians (the default) to degrees using ankle_angle_in_deg = units.convert_from_default(osl.ankle.output_position, units.position.deg).
Building the State Machine#
The function build_4_state_FSM() uses the StateMachine functionality of the opensourceleg.control module to make a state machine with 4 states.
First, we create the states using the State class.
def build_4_state_FSM(osl: OpenSourceLeg) -> StateMachine:
"""This method builds a state machine with 4 states.
The states are early stance, late stance, early swing, and late swing.
It uses the impedance parameters and transition criteria above.
Inputs:
OSL instance
Returns:
FSM object"""
early_stance = State(name="e_stance")
late_stance = State(name="l_stance")
early_swing = State(name="e_swing")
late_swing = State(name="l_swing")
Then, we assign impedance values for each state.
early_stance.set_knee_impedance_paramters(
theta=KNEE_THETA_ESTANCE, k=KNEE_K_ESTANCE, b=KNEE_B_ESTANCE
)
early_stance.make_knee_active()
early_stance.set_ankle_impedance_paramters(
theta=ANKLE_THETA_ESTANCE, k=ANKLE_K_ESTANCE, b=ANKLE_B_ESTANCE
)
early_stance.make_ankle_active()
late_stance.set_knee_impedance_paramters(
theta=KNEE_THETA_LSTANCE, k=KNEE_K_LSTANCE, b=KNEE_B_LSTANCE
)
late_stance.make_knee_active()
late_stance.set_ankle_impedance_paramters(
theta=ANKLE_THETA_LSTANCE, k=ANKLE_K_LSTANCE, b=ANKLE_B_LSTANCE
)
late_stance.make_ankle_active()
early_swing.set_knee_impedance_paramters(
theta=KNEE_THETA_ESWING, k=KNEE_K_ESWING, b=KNEE_B_ESWING
)
early_swing.make_knee_active()
early_swing.set_ankle_impedance_paramters(
theta=ANKLE_THETA_ESWING, k=ANKLE_K_ESWING, b=ANKLE_B_ESWING
)
early_swing.make_ankle_active()
late_swing.set_knee_impedance_paramters(
theta=KNEE_THETA_LSWING, k=KNEE_K_LSWING, b=KNEE_B_LSWING
)
late_swing.make_knee_active()
late_swing.set_ankle_impedance_paramters(
theta=ANKLE_THETA_LSWING, k=ANKLE_K_LSWING, b=ANKLE_B_LSWING
)
late_swing.make_ankle_active()
After the states have been defined, we define transition functions. These functions take the osl instance as arguments and return a boolean when transition criteria are met.
For example, we first define the transition from early stance to late stance based on the loadcell z force and the ankle angle as:
def estance_to_lstance(osl: OpenSourceLeg) -> bool:
"""
Transition from early stance to late stance when the loadcell
reads a force greater than a threshold.
"""
assert osl.loadcell is not None
return bool(
osl.loadcell.fz < LOAD_LSTANCE
and osl.ankle.output_position > ANKLE_THETA_ESTANCE_TO_LSTANCE
)
The remaining transition functions are defined similarly.
def estance_to_lstance(osl: OpenSourceLeg) -> bool:
"""
Transition from early stance to late stance when the loadcell
reads a force greater than a threshold.
"""
assert osl.loadcell is not None
return bool(
osl.loadcell.fz < LOAD_LSTANCE
and osl.ankle.output_position > ANKLE_THETA_ESTANCE_TO_LSTANCE
)
def lstance_to_eswing(osl: OpenSourceLeg) -> bool:
"""
Transition from late stance to early swing when the loadcell
reads a force less than a threshold.
"""
assert osl.loadcell is not None
return bool(osl.loadcell.fz > LOAD_ESWING)
def eswing_to_lswing(osl: OpenSourceLeg) -> bool:
"""
Transition from early swing to late swing when the knee angle
is greater than a threshold and the knee velocity is less than
a threshold.
"""
assert osl.knee is not None
return bool(
osl.knee.output_position > KNEE_THETA_ESWING_TO_LSWING
and osl.knee.output_velocity < KNEE_DTHETA_ESWING_TO_LSWING
)
def lswing_to_estance(osl: OpenSourceLeg) -> bool:
"""
Transition from late swing to early stance when the loadcell
reads a force greater than a threshold or the knee angle is
less than a threshold.
"""
assert osl.knee is not None and osl.loadcell is not None
return bool(
osl.loadcell.fz < LOAD_ESTANCE
or osl.knee.output_position < KNEE_THETA_LSWING_TO_ESTANCE
)
Next, we define events corresponding to the state transtions using the Event class.
foot_flat = Event(name="foot_flat")
heel_off = Event(name="heel_off")
toe_off = Event(name="toe_off")
pre_heel_strike = Event(name="pre_heel_strike")
heel_strike = Event(name="heel_strike")
Finally, we make an instance of the StateMachine class and add the states, events, and transitions that we’ve created.
The add_transition() method takes arguments of a source state, a destination state, an event, and the callback function defining when that transition occurs.
After that, the FSM is fully built and can be returned.
fsm = StateMachine(osl=osl, spoof=offline_mode)
fsm.add_state(state=early_stance, initial_state=True)
fsm.add_state(state=late_stance)
fsm.add_state(state=early_swing)
fsm.add_state(state=late_swing)
fsm.add_event(event=foot_flat)
fsm.add_event(event=heel_off)
fsm.add_event(event=toe_off)
fsm.add_event(event=pre_heel_strike)
fsm.add_event(event=heel_strike)
fsm.add_transition(
source=early_stance,
destination=late_stance,
event=foot_flat,
callback=estance_to_lstance,
)
fsm.add_transition(
source=late_stance,
destination=early_swing,
event=heel_off,
callback=lstance_to_eswing,
)
fsm.add_transition(
source=early_swing,
destination=late_swing,
event=toe_off,
callback=eswing_to_lswing,
)
fsm.add_transition(
source=late_swing,
destination=early_stance,
event=heel_strike,
callback=lswing_to_estance,
)
return fsm
Finally, we call our main function:
if __name__ == "__main__":
run_FSM_controller()
Full Code for This Example#
1"""
2Example FSM walking controller for the OSL.
3This implementation is all in python.
4
5Senthur Raj Ayyappan, Kevin Best
6Neurobionics Lab
7Robotics Department
8University of Michigan
9November 14, 2023
10"""
11import numpy as np
12
13import opensourceleg.tools.units as units
14from opensourceleg.control.state_machine import Event, State, StateMachine
15from opensourceleg.osl import OpenSourceLeg
16
17offline_mode = False # Set to true for debugging without hardware
18
19# ------------- TUNABLE FSM PARAMETERS ---------------- #
20BODY_WEIGHT = 60 * 9.8
21
22# STATE 1: EARLY STANCE
23KNEE_K_ESTANCE = 99.372
24KNEE_B_ESTANCE = 3.180
25KNEE_THETA_ESTANCE = 5
26ANKLE_K_ESTANCE = 19.874
27ANKLE_B_ESTANCE = 0
28ANKLE_THETA_ESTANCE = -2
29LOAD_LSTANCE: float = -1.0 * BODY_WEIGHT * 0.25
30ANKLE_THETA_ESTANCE_TO_LSTANCE = 6.0
31
32# STATE 2: LATE STANCE
33KNEE_K_LSTANCE = 99.372
34KNEE_B_LSTANCE = 1.272
35KNEE_THETA_LSTANCE = 8
36ANKLE_K_LSTANCE = 79.498
37ANKLE_B_LSTANCE = 0.063
38ANKLE_THETA_LSTANCE = -20
39LOAD_ESWING: float = -1.0 * BODY_WEIGHT * 0.15
40
41# STATE 3: EARLY SWING
42KNEE_K_ESWING = 39.749
43KNEE_B_ESWING = 0.063
44KNEE_THETA_ESWING = 60
45ANKLE_K_ESWING = 7.949
46ANKLE_B_ESWING = 0.0
47ANKLE_THETA_ESWING = 25
48KNEE_THETA_ESWING_TO_LSWING = 50
49KNEE_DTHETA_ESWING_TO_LSWING = 3
50
51# STATE 4: LATE SWING
52KNEE_K_LSWING = 15.899
53KNEE_B_LSWING = 3.816
54KNEE_THETA_LSWING = 5
55ANKLE_K_LSWING = 7.949
56ANKLE_B_LSWING = 0.0
57ANKLE_THETA_LSWING = 15
58LOAD_ESTANCE: float = -1.0 * BODY_WEIGHT * 0.4
59KNEE_THETA_LSWING_TO_ESTANCE = 30
60# ---------------------------------------------------- #
61
62
63def run_FSM_controller():
64 """
65 This is the main function for this script.
66 It creates an OSL object and builds a state machine with 4 states.
67 It runs a main loop that updates the state machine based on the
68 hardware information and sends updated commands to the motors.
69 """
70 osl = OpenSourceLeg(frequency=200)
71 osl.add_joint(name="knee", gear_ratio=41.4999, offline_mode=offline_mode)
72 osl.add_joint(name="ankle", gear_ratio=41.4999, offline_mode=offline_mode)
73 LOADCELL_MATRIX = np.array(
74 [
75 (-38.72600, -1817.74700, 9.84900, 43.37400, -44.54000, 1824.67000),
76 (-8.61600, 1041.14900, 18.86100, -2098.82200, 31.79400, 1058.6230),
77 (-1047.16800, 8.63900, -1047.28200, -20.70000, -1073.08800, -8.92300),
78 (20.57600, -0.04000, -0.24600, 0.55400, -21.40800, -0.47600),
79 (-12.13400, -1.10800, 24.36100, 0.02300, -12.14100, 0.79200),
80 (-0.65100, -28.28700, 0.02200, -25.23000, 0.47300, -27.3070),
81 ]
82 )
83 osl.add_loadcell(
84 dephy_mode=False,
85 offline_mode=offline_mode,
86 loadcell_matrix=LOADCELL_MATRIX,
87 )
88
89 fsm = build_4_state_FSM(osl)
90
91 osl.log.add_attributes(container=osl, attributes=["timestamp"])
92 osl.log.add_attributes(
93 container=osl.knee,
94 attributes=[
95 "output_position",
96 "motor_current",
97 "joint_torque",
98 "motor_voltage",
99 "accelx",
100 ],
101 )
102 osl.log.add_attributes(
103 container=osl.ankle,
104 attributes=[
105 "output_position",
106 "motor_current",
107 "joint_torque",
108 "motor_voltage",
109 "accelx",
110 ],
111 )
112 osl.log.add_attributes(container=osl.loadcell, attributes=["fz"])
113 osl.log.add_attributes(container=fsm.current_state, attributes=["name"])
114
115 with osl:
116 osl.home()
117 fsm.start()
118
119 for t in osl.clock:
120 osl.update()
121 fsm.update()
122
123 if osl.knee.mode != osl.knee.control_modes.impedance:
124 osl.knee.set_mode(mode=osl.knee.control_modes.impedance)
125 osl.knee.set_impedance_gains()
126 osl.knee.set_joint_impedance(
127 K=units.convert_to_default(
128 fsm.current_state.knee_stiffness,
129 units.stiffness.N_m_per_rad,
130 ),
131 B=units.convert_to_default(
132 fsm.current_state.knee_damping,
133 units.damping.N_m_per_rad_per_s,
134 ),
135 )
136 osl.knee.set_output_position(
137 position=units.convert_to_default(
138 fsm.current_state.knee_theta, units.position.deg
139 ),
140 )
141
142 if osl.ankle.mode != osl.ankle.control_modes.impedance:
143 osl.ankle.set_mode(osl.ankle.control_modes.impedance)
144 osl.ankle.set_impedance_gains()
145 osl.ankle.set_joint_impedance(
146 K=units.convert_to_default(
147 fsm.current_state.ankle_stiffness,
148 units.stiffness.N_m_per_rad,
149 ),
150 B=units.convert_to_default(
151 fsm.current_state.ankle_damping,
152 units.damping.N_m_per_rad_per_s,
153 ),
154 )
155 osl.ankle.set_output_position(
156 position=units.convert_to_default(
157 fsm.current_state.ankle_theta, units.position.deg
158 ),
159 )
160 print(
161 "Current time in state {}: {:.2f} seconds, Knee Eq {:.2f}, Ankle Eq {:.2f}, Fz {:.2f}".format(
162 fsm.current_state.name,
163 fsm.current_state.current_time_in_state,
164 fsm.current_state.knee_theta,
165 fsm.current_state.ankle_theta,
166 osl.loadcell.fz,
167 ),
168 end="\r",
169 )
170
171 fsm.stop()
172 print("")
173
174
175def build_4_state_FSM(osl: OpenSourceLeg) -> StateMachine:
176 """This method builds a state machine with 4 states.
177 The states are early stance, late stance, early swing, and late swing.
178 It uses the impedance parameters and transition criteria above.
179
180 Inputs:
181 OSL instance
182 Returns:
183 FSM object"""
184
185 early_stance = State(name="e_stance")
186 late_stance = State(name="l_stance")
187 early_swing = State(name="e_swing")
188 late_swing = State(name="l_swing")
189
190 early_stance.set_knee_impedance_paramters(
191 theta=KNEE_THETA_ESTANCE, k=KNEE_K_ESTANCE, b=KNEE_B_ESTANCE
192 )
193 early_stance.make_knee_active()
194 early_stance.set_ankle_impedance_paramters(
195 theta=ANKLE_THETA_ESTANCE, k=ANKLE_K_ESTANCE, b=ANKLE_B_ESTANCE
196 )
197 early_stance.make_ankle_active()
198
199 late_stance.set_knee_impedance_paramters(
200 theta=KNEE_THETA_LSTANCE, k=KNEE_K_LSTANCE, b=KNEE_B_LSTANCE
201 )
202 late_stance.make_knee_active()
203 late_stance.set_ankle_impedance_paramters(
204 theta=ANKLE_THETA_LSTANCE, k=ANKLE_K_LSTANCE, b=ANKLE_B_LSTANCE
205 )
206 late_stance.make_ankle_active()
207
208 early_swing.set_knee_impedance_paramters(
209 theta=KNEE_THETA_ESWING, k=KNEE_K_ESWING, b=KNEE_B_ESWING
210 )
211 early_swing.make_knee_active()
212 early_swing.set_ankle_impedance_paramters(
213 theta=ANKLE_THETA_ESWING, k=ANKLE_K_ESWING, b=ANKLE_B_ESWING
214 )
215 early_swing.make_ankle_active()
216
217 late_swing.set_knee_impedance_paramters(
218 theta=KNEE_THETA_LSWING, k=KNEE_K_LSWING, b=KNEE_B_LSWING
219 )
220 late_swing.make_knee_active()
221 late_swing.set_ankle_impedance_paramters(
222 theta=ANKLE_THETA_LSWING, k=ANKLE_K_LSWING, b=ANKLE_B_LSWING
223 )
224 late_swing.make_ankle_active()
225
226 def estance_to_lstance(osl: OpenSourceLeg) -> bool:
227 """
228 Transition from early stance to late stance when the loadcell
229 reads a force greater than a threshold.
230 """
231 assert osl.loadcell is not None
232 return bool(
233 osl.loadcell.fz < LOAD_LSTANCE
234 and osl.ankle.output_position > ANKLE_THETA_ESTANCE_TO_LSTANCE
235 )
236
237 def lstance_to_eswing(osl: OpenSourceLeg) -> bool:
238 """
239 Transition from late stance to early swing when the loadcell
240 reads a force less than a threshold.
241 """
242 assert osl.loadcell is not None
243 return bool(osl.loadcell.fz > LOAD_ESWING)
244
245 def eswing_to_lswing(osl: OpenSourceLeg) -> bool:
246 """
247 Transition from early swing to late swing when the knee angle
248 is greater than a threshold and the knee velocity is less than
249 a threshold.
250 """
251 assert osl.knee is not None
252 return bool(
253 osl.knee.output_position > KNEE_THETA_ESWING_TO_LSWING
254 and osl.knee.output_velocity < KNEE_DTHETA_ESWING_TO_LSWING
255 )
256
257 def lswing_to_estance(osl: OpenSourceLeg) -> bool:
258 """
259 Transition from late swing to early stance when the loadcell
260 reads a force greater than a threshold or the knee angle is
261 less than a threshold.
262 """
263 assert osl.knee is not None and osl.loadcell is not None
264 return bool(
265 osl.loadcell.fz < LOAD_ESTANCE
266 or osl.knee.output_position < KNEE_THETA_LSWING_TO_ESTANCE
267 )
268
269 foot_flat = Event(name="foot_flat")
270 heel_off = Event(name="heel_off")
271 toe_off = Event(name="toe_off")
272 pre_heel_strike = Event(name="pre_heel_strike")
273 heel_strike = Event(name="heel_strike")
274
275 fsm = StateMachine(osl=osl, spoof=offline_mode)
276 fsm.add_state(state=early_stance, initial_state=True)
277 fsm.add_state(state=late_stance)
278 fsm.add_state(state=early_swing)
279 fsm.add_state(state=late_swing)
280
281 fsm.add_event(event=foot_flat)
282 fsm.add_event(event=heel_off)
283 fsm.add_event(event=toe_off)
284 fsm.add_event(event=pre_heel_strike)
285 fsm.add_event(event=heel_strike)
286
287 fsm.add_transition(
288 source=early_stance,
289 destination=late_stance,
290 event=foot_flat,
291 callback=estance_to_lstance,
292 )
293 fsm.add_transition(
294 source=late_stance,
295 destination=early_swing,
296 event=heel_off,
297 callback=lstance_to_eswing,
298 )
299 fsm.add_transition(
300 source=early_swing,
301 destination=late_swing,
302 event=toe_off,
303 callback=eswing_to_lswing,
304 )
305 fsm.add_transition(
306 source=late_swing,
307 destination=early_stance,
308 event=heel_strike,
309 callback=lswing_to_estance,
310 )
311 return fsm
312
313
314if __name__ == "__main__":
315 run_FSM_controller()
C++ and MATLAB Implementation#
To get started, make sure you have compiled either the C++ or the MATLAB source code and have a FSMController.so library. If not, please see the source repository for compilation instructions. To run this example as is, copy the library you generated to the examples directory. Alternatively, you can modify the search path for the library when loading the controller (see below).
Load Compiled Library#
First, we’ll perform standard imports, handle some paths, and setup our OSL object.
1import inspect
2import os
3
4import numpy as np
5
6from opensourceleg.control.compiled_controller import CompiledController
7from opensourceleg.osl import OpenSourceLeg
8from opensourceleg.tools import units
9
10osl = OpenSourceLeg(frequency=200)
11use_offline_mode = False
12osl.add_joint("knee", gear_ratio=9 * 83 / 18, offline_mode=use_offline_mode)
13osl.add_joint("ankle", gear_ratio=9 * 83 / 18, offline_mode=use_offline_mode)
14LOADCELL_MATRIX = np.array(
15 [
16 (-38.72600, -1817.74700, 9.84900, 43.37400, -44.54000, 1824.67000),
17 (-8.61600, 1041.14900, 18.86100, -2098.82200, 31.79400, 1058.6230),
18 (-1047.16800, 8.63900, -1047.28200, -20.70000, -1073.08800, -8.92300),
19 (20.57600, -0.04000, -0.24600, 0.55400, -21.40800, -0.47600),
20 (-12.13400, -1.10800, 24.36100, 0.02300, -12.14100, 0.79200),
21 (-0.65100, -28.28700, 0.02200, -25.23000, 0.47300, -27.3070),
22 ]
23)
24osl.add_loadcell(
25 joint=osl.knee, offline_mode=use_offline_mode, loadcell_matrix=LOADCELL_MATRIX
26)
Note
If instantiating the OSL object is unfamiliar, check out the Adding an Actuator and Adding a Loadcell tutorial pages.
Next, we’ll instantiate a CompiledController wrapper object. This takes arguments of
the name of the library (without extension), the path at which it is located (which in this case is the current directory),
and the names of the main, initializaiton, and cleanup functions.
1currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) # type: ignore
2controller = CompiledController(
3 library_name="FSMController",
4 library_path=currentdir,
5 main_function_name="FSMController",
6 initialization_function_name="FSMController_initialize",
7 cleanup_function_name="FSMController_terminate",
8)
Define Custom Datatypes#
Next, we define the data structures used in the controller. These must exactly match (size and order) what was used to create the library.
First, we define a type for a single set of impedance parameters called impedance_param_type, where stiffness, damping, and eq_angle are each doubles.
The CompiledController object provides building block types within its types attribute.
1controller.define_type(
2 "impedance_param_type",
3 [
4 ("stiffness", controller.types.c_double),
5 ("damping", controller.types.c_double),
6 ("eq_angle", controller.types.c_double),
7 ],
8)
We then define the type joint_impedance_set which contains a set of impedance parameters for each of the four states.
Because we have already defined the impedance_param_type in the previous lines, we can now use it to define additional types.
1controller.define_type(
2 "joint_impedance_set",
3 [
4 ("early_stance", controller.types.impedance_param_type),
5 ("late_stance", controller.types.impedance_param_type),
6 ("early_swing", controller.types.impedance_param_type),
7 ("late_swing", controller.types.impedance_param_type),
8 ],
9)
We then similarly define the transition_parameters type as well as the overall UserParameters type.
1controller.define_type(
2 "transition_parameters",
3 [
4 ("min_time_in_state", controller.types.c_double),
5 ("loadLStance", controller.types.c_double),
6 ("ankleThetaEstanceToLstance", controller.types.c_double),
7 ("kneeThetaESwingToLSwing", controller.types.c_double),
8 ("kneeDthetaESwingToLSwing", controller.types.c_double),
9 ("loadESwing", controller.types.c_double),
10 ("loadEStance", controller.types.c_double),
11 ("kneeThetaLSwingToEStance", controller.types.c_double),
12 ],
13)
14controller.define_type(
15 "UserParameters",
16 [
17 ("body_weight", controller.types.c_double),
18 ("knee_impedance", controller.types.joint_impedance_set),
19 ("ankle_impedance", controller.types.joint_impedance_set),
20 ("transition_parameters", controller.types.transition_parameters),
21 ],
22)
We also define a sensors type. We’re using the default sensors, which the CompiledController class provides in list form already setup correctly.
1controller.define_type("sensors", controller.DEFAULT_SENSOR_LIST)
The final type definitions are for the input and output types.
1controller.define_inputs(
2 [
3 ("parameters", controller.types.UserParameters),
4 ("sensors", controller.types.sensors),
5 ("time", controller.types.c_double),
6 ]
7)
8controller.define_outputs(
9 [
10 ("current_state", controller.types.c_int),
11 ("time_in_current_state", controller.types.c_double),
12 ("knee_impedance", controller.types.impedance_param_type),
13 ("ankle_impedance", controller.types.impedance_param_type),
14 ]
15)
Configure Impedance and Transition Parameters#
The next section of code configures the impedance and transition paramters based on a pre-defined tuning. Feel free to play with these values to get the behavior you want.
1controller.inputs.parameters.knee_impedance.early_stance.stiffness = 99.372 # type: ignore
2controller.inputs.parameters.knee_impedance.early_stance.damping = 3.180 # type: ignore
3controller.inputs.parameters.knee_impedance.early_stance.eq_angle = 5 # type: ignore
4controller.inputs.parameters.knee_impedance.late_stance.stiffness = 99.372 # type: ignore
5controller.inputs.parameters.knee_impedance.late_stance.damping = 1.272 # type: ignore
6controller.inputs.parameters.knee_impedance.late_stance.eq_angle = 8 # type: ignore
7controller.inputs.parameters.knee_impedance.early_swing.stiffness = 39.746 # type: ignore
8controller.inputs.parameters.knee_impedance.early_swing.damping = 0.063 # type: ignore
9controller.inputs.parameters.knee_impedance.early_swing.eq_angle = 60 # type: ignore
10controller.inputs.parameters.knee_impedance.late_swing.stiffness = 15.899 # type: ignore
11controller.inputs.parameters.knee_impedance.late_swing.damping = 3.186 # type: ignore
12controller.inputs.parameters.knee_impedance.late_swing.eq_angle = 5 # type: ignore
13controller.inputs.parameters.ankle_impedance.early_stance.stiffness = 19.874 # type: ignore
14controller.inputs.parameters.ankle_impedance.early_stance.damping = 0 # type: ignore
15controller.inputs.parameters.ankle_impedance.early_stance.eq_angle = -2 # type: ignore
16controller.inputs.parameters.ankle_impedance.late_stance.stiffness = 79.498 # type: ignore
17controller.inputs.parameters.ankle_impedance.late_stance.damping = 0.063 # type: ignore
18controller.inputs.parameters.ankle_impedance.late_stance.eq_angle = -20 # type: ignore
19controller.inputs.parameters.ankle_impedance.early_swing.stiffness = 7.949 # type: ignore
20controller.inputs.parameters.ankle_impedance.early_swing.damping = 0 # type: ignore
21controller.inputs.parameters.ankle_impedance.early_swing.eq_angle = 25 # type: ignore
22controller.inputs.parameters.ankle_impedance.late_swing.stiffness = 7.949 # type: ignore
23controller.inputs.parameters.ankle_impedance.late_swing.damping = 0.0 # type: ignore
24controller.inputs.parameters.ankle_impedance.late_swing.eq_angle = 15 # type: ignore
25
26# Configure state machine
27body_weight = 82 # kg
28controller.inputs.parameters.body_weight = body_weight # type: ignore
29controller.inputs.parameters.transition_parameters.min_time_in_state = 0.20 # type: ignore
30controller.inputs.parameters.transition_parameters.loadLStance = -body_weight * 0.25 # type: ignore
31controller.inputs.parameters.transition_parameters.ankleThetaEstanceToLstance = 6.0 # type: ignore
32controller.inputs.parameters.transition_parameters.loadESwing = -body_weight * 0.15 # type: ignore
33controller.inputs.parameters.transition_parameters.kneeThetaESwingToLSwing = 50 # type: ignore
34controller.inputs.parameters.transition_parameters.kneeDthetaESwingToLSwing = 3 # type: ignore
35controller.inputs.parameters.transition_parameters.loadEStance = -body_weight * 0.4 # type: ignore
36controller.inputs.parameters.transition_parameters.kneeThetaLSwingToEStance = 30 # type: ignore
Main Loop#
Now that the controller is configured, we can home the osl, calibrate the loadcell, set both joints to impedance mode, and begin running.
Note that within the main loop after calling osl.update(), we assign the relevant sensor values from the osl object to the controller inputs.
We also write to any other inputs that change every loop, such as the current time t.
Once all inputs are assigned, we call the run() method, which calls our compiled library with the configured inputs.
The run method returns the outputs object, which is populated by the compiled library function.
1with osl:
2 osl.home()
3 osl.update()
4 osl.knee.set_mode(osl.knee.control_modes.impedance)
5 osl.ankle.set_mode(osl.ankle.control_modes.impedance)
6
7 # Main Loop
8 for t in osl.clock:
9 osl.update()
10
11 controller.inputs.sensors.knee_angle = ( # type: ignore
12 units.convert_from_default(osl.knee.output_position, units.position.deg)
13 )
14 controller.inputs.sensors.ankle_angle = units.convert_from_default(osl.ankle.output_position, units.position.deg) # type: ignore
15 controller.inputs.sensors.knee_velocity = units.convert_from_default(osl.knee.output_velocity, units.velocity.deg_per_s) # type: ignore
16 controller.inputs.sensors.ankle_velocity = units.convert_from_default(osl.ankle.output_velocity, units.velocity.deg_per_s) # type: ignore
17 controller.inputs.sensors.Fz = osl.loadcell.fz # type: ignore
18
19 # Update any control inputs that change every loop
20 controller.inputs.time = t # type: ignore
21
22 # Call the controller
23 outputs = controller.run()
24
25 # Test print to ensure external library call works
26 print(
27 "Current time in state {}: {:.2f} seconds, Knee Eq {:.2f}, Ankle Eq {:.2f}, Fz {:.2f}".format(
28 outputs.current_state,
29 outputs.time_in_current_state,
30 outputs.knee_impedance.eq_angle,
31 outputs.ankle_impedance.eq_angle,
32 osl.loadcell.fz,
33 ),
34 end="\r",
35 )
Note
The OSL library provides sensor values in default units. If your library expects other units,
you need to convert the values prior to assigning them. You can use the units module in the tools subpackage to do this.
For example you can convert from radians (the default) to degrees using ankle_angle_in_deg = units.convert_from_default(osl.ankle.output_position, units.position.deg).
Finally, we write from the outputs structure to the hardware. Be careful with units at this step as well, making sure your values are either in default units, or that you call appropriate conversion functions. As our library was in degrees, we convert to radians.
1 osl.knee.set_joint_impedance(
2 K=units.convert_to_default(
3 outputs.knee_impedance.stiffness, units.stiffness.N_m_per_rad
4 ),
5 B=units.convert_to_default(
6 outputs.knee_impedance.damping, units.damping.N_m_per_rad_per_s
7 ),
8 )
9 osl.knee.set_output_position(
10 position=units.convert_to_default(
11 outputs.knee_impedance.eq_angle, units.position.deg
12 )
13 )
14 osl.ankle.set_joint_impedance(
15 K=units.convert_to_default(
16 outputs.ankle_impedance.stiffness, units.stiffness.N_m_per_rad
17 ),
18 B=units.convert_to_default(
19 outputs.ankle_impedance.damping, units.damping.N_m_per_rad_per_s
20 ),
21 )
22 osl.ankle.set_output_position(
23 position=units.convert_to_default(
24 outputs.ankle_impedance.eq_angle, units.position.deg
25 )
26 )
Full Code for This Example#
1"""
2Example walking controller for the OSL.
3
4Senthur Raj Ayyappan, Kevin Best
5Neurobionics Lab
6Robotics Department
7University of Michigan
8October 9, 2023
9"""
10
11import inspect
12import os
13
14import numpy as np
15
16from opensourceleg.control.compiled_controller import CompiledController
17from opensourceleg.osl import OpenSourceLeg
18from opensourceleg.tools import units
19
20osl = OpenSourceLeg(frequency=200)
21use_offline_mode = False
22osl.add_joint("knee", gear_ratio=9 * 83 / 18, offline_mode=use_offline_mode)
23osl.add_joint("ankle", gear_ratio=9 * 83 / 18, offline_mode=use_offline_mode)
24LOADCELL_MATRIX = np.array(
25 [
26 (-38.72600, -1817.74700, 9.84900, 43.37400, -44.54000, 1824.67000),
27 (-8.61600, 1041.14900, 18.86100, -2098.82200, 31.79400, 1058.6230),
28 (-1047.16800, 8.63900, -1047.28200, -20.70000, -1073.08800, -8.92300),
29 (20.57600, -0.04000, -0.24600, 0.55400, -21.40800, -0.47600),
30 (-12.13400, -1.10800, 24.36100, 0.02300, -12.14100, 0.79200),
31 (-0.65100, -28.28700, 0.02200, -25.23000, 0.47300, -27.3070),
32 ]
33)
34osl.add_loadcell(
35 joint=osl.knee, offline_mode=use_offline_mode, loadcell_matrix=LOADCELL_MATRIX
36)
37
38currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) # type: ignore
39controller = CompiledController(
40 library_name="FSMController",
41 library_path=currentdir,
42 main_function_name="FSMController",
43 initialization_function_name="FSMController_initialize",
44 cleanup_function_name="FSMController_terminate",
45)
46
47controller.define_type(
48 "impedance_param_type",
49 [
50 ("stiffness", controller.types.c_double),
51 ("damping", controller.types.c_double),
52 ("eq_angle", controller.types.c_double),
53 ],
54)
55controller.define_type(
56 "joint_impedance_set",
57 [
58 ("early_stance", controller.types.impedance_param_type),
59 ("late_stance", controller.types.impedance_param_type),
60 ("early_swing", controller.types.impedance_param_type),
61 ("late_swing", controller.types.impedance_param_type),
62 ],
63)
64controller.define_type(
65 "transition_parameters",
66 [
67 ("min_time_in_state", controller.types.c_double),
68 ("loadLStance", controller.types.c_double),
69 ("ankleThetaEstanceToLstance", controller.types.c_double),
70 ("kneeThetaESwingToLSwing", controller.types.c_double),
71 ("kneeDthetaESwingToLSwing", controller.types.c_double),
72 ("loadESwing", controller.types.c_double),
73 ("loadEStance", controller.types.c_double),
74 ("kneeThetaLSwingToEStance", controller.types.c_double),
75 ],
76)
77controller.define_type(
78 "UserParameters",
79 [
80 ("body_weight", controller.types.c_double),
81 ("knee_impedance", controller.types.joint_impedance_set),
82 ("ankle_impedance", controller.types.joint_impedance_set),
83 ("transition_parameters", controller.types.transition_parameters),
84 ],
85)
86controller.define_type("sensors", controller.DEFAULT_SENSOR_LIST)
87
88controller.define_inputs(
89 [
90 ("parameters", controller.types.UserParameters),
91 ("sensors", controller.types.sensors),
92 ("time", controller.types.c_double),
93 ]
94)
95controller.define_outputs(
96 [
97 ("current_state", controller.types.c_int),
98 ("time_in_current_state", controller.types.c_double),
99 ("knee_impedance", controller.types.impedance_param_type),
100 ("ankle_impedance", controller.types.impedance_param_type),
101 ]
102)
103
104# Populate Controller inputs as needed
105controller.inputs.parameters.knee_impedance.early_stance.stiffness = 99.372 # type: ignore
106controller.inputs.parameters.knee_impedance.early_stance.damping = 3.180 # type: ignore
107controller.inputs.parameters.knee_impedance.early_stance.eq_angle = 5 # type: ignore
108controller.inputs.parameters.knee_impedance.late_stance.stiffness = 99.372 # type: ignore
109controller.inputs.parameters.knee_impedance.late_stance.damping = 1.272 # type: ignore
110controller.inputs.parameters.knee_impedance.late_stance.eq_angle = 8 # type: ignore
111controller.inputs.parameters.knee_impedance.early_swing.stiffness = 39.746 # type: ignore
112controller.inputs.parameters.knee_impedance.early_swing.damping = 0.063 # type: ignore
113controller.inputs.parameters.knee_impedance.early_swing.eq_angle = 60 # type: ignore
114controller.inputs.parameters.knee_impedance.late_swing.stiffness = 15.899 # type: ignore
115controller.inputs.parameters.knee_impedance.late_swing.damping = 3.186 # type: ignore
116controller.inputs.parameters.knee_impedance.late_swing.eq_angle = 5 # type: ignore
117controller.inputs.parameters.ankle_impedance.early_stance.stiffness = 19.874 # type: ignore
118controller.inputs.parameters.ankle_impedance.early_stance.damping = 0 # type: ignore
119controller.inputs.parameters.ankle_impedance.early_stance.eq_angle = -2 # type: ignore
120controller.inputs.parameters.ankle_impedance.late_stance.stiffness = 79.498 # type: ignore
121controller.inputs.parameters.ankle_impedance.late_stance.damping = 0.063 # type: ignore
122controller.inputs.parameters.ankle_impedance.late_stance.eq_angle = -20 # type: ignore
123controller.inputs.parameters.ankle_impedance.early_swing.stiffness = 7.949 # type: ignore
124controller.inputs.parameters.ankle_impedance.early_swing.damping = 0 # type: ignore
125controller.inputs.parameters.ankle_impedance.early_swing.eq_angle = 25 # type: ignore
126controller.inputs.parameters.ankle_impedance.late_swing.stiffness = 7.949 # type: ignore
127controller.inputs.parameters.ankle_impedance.late_swing.damping = 0.0 # type: ignore
128controller.inputs.parameters.ankle_impedance.late_swing.eq_angle = 15 # type: ignore
129
130# Configure state machine
131body_weight = 82 # kg
132controller.inputs.parameters.body_weight = body_weight # type: ignore
133controller.inputs.parameters.transition_parameters.min_time_in_state = 0.20 # type: ignore
134controller.inputs.parameters.transition_parameters.loadLStance = -body_weight * 0.25 # type: ignore
135controller.inputs.parameters.transition_parameters.ankleThetaEstanceToLstance = 6.0 # type: ignore
136controller.inputs.parameters.transition_parameters.loadESwing = -body_weight * 0.15 # type: ignore
137controller.inputs.parameters.transition_parameters.kneeThetaESwingToLSwing = 50 # type: ignore
138controller.inputs.parameters.transition_parameters.kneeDthetaESwingToLSwing = 3 # type: ignore
139controller.inputs.parameters.transition_parameters.loadEStance = -body_weight * 0.4 # type: ignore
140controller.inputs.parameters.transition_parameters.kneeThetaLSwingToEStance = 30 # type: ignore
141
142with osl:
143 osl.home()
144 osl.update()
145 osl.knee.set_mode(osl.knee.control_modes.impedance)
146 osl.ankle.set_mode(osl.ankle.control_modes.impedance)
147
148 # Main Loop
149 for t in osl.clock:
150 osl.update()
151
152 controller.inputs.sensors.knee_angle = ( # type: ignore
153 units.convert_from_default(osl.knee.output_position, units.position.deg)
154 )
155 controller.inputs.sensors.ankle_angle = units.convert_from_default(osl.ankle.output_position, units.position.deg) # type: ignore
156 controller.inputs.sensors.knee_velocity = units.convert_from_default(osl.knee.output_velocity, units.velocity.deg_per_s) # type: ignore
157 controller.inputs.sensors.ankle_velocity = units.convert_from_default(osl.ankle.output_velocity, units.velocity.deg_per_s) # type: ignore
158 controller.inputs.sensors.Fz = osl.loadcell.fz # type: ignore
159
160 # Update any control inputs that change every loop
161 controller.inputs.time = t # type: ignore
162
163 # Call the controller
164 outputs = controller.run()
165
166 # Test print to ensure external library call works
167 print(
168 "Current time in state {}: {:.2f} seconds, Knee Eq {:.2f}, Ankle Eq {:.2f}, Fz {:.2f}".format(
169 outputs.current_state,
170 outputs.time_in_current_state,
171 outputs.knee_impedance.eq_angle,
172 outputs.ankle_impedance.eq_angle,
173 osl.loadcell.fz,
174 ),
175 end="\r",
176 )
177
178 # Write to the hardware
179 osl.knee.set_joint_impedance(
180 K=units.convert_to_default(
181 outputs.knee_impedance.stiffness, units.stiffness.N_m_per_rad
182 ),
183 B=units.convert_to_default(
184 outputs.knee_impedance.damping, units.damping.N_m_per_rad_per_s
185 ),
186 )
187 osl.knee.set_output_position(
188 position=units.convert_to_default(
189 outputs.knee_impedance.eq_angle, units.position.deg
190 )
191 )
192 osl.ankle.set_joint_impedance(
193 K=units.convert_to_default(
194 outputs.ankle_impedance.stiffness, units.stiffness.N_m_per_rad
195 ),
196 B=units.convert_to_default(
197 outputs.ankle_impedance.damping, units.damping.N_m_per_rad_per_s
198 ),
199 )
200 osl.ankle.set_output_position(
201 position=units.convert_to_default(
202 outputs.ankle_impedance.eq_angle, units.position.deg
203 )
204 )
205
206 print("\n")