Skip to content

Commit 16cb7dd

Browse files
VarunSatyadevShettyVarunSatyadevShetty
authored
Merge pull request #528 from neurobionics/FSM-TESTING
FSM tested
2 parents be6a8fc + 7eb3011 commit 16cb7dd

2 files changed

Lines changed: 66 additions & 43 deletions

File tree

‎examples/fsm_walking_ankle_python_controller.py‎

Lines changed: 36 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -22,18 +22,17 @@
2222
GEAR_RATIO = 9 * (83 / 18)
2323
FREQUENCY = 200
2424

25-
26-
LOADCELL_CALIBRATION_MATRIX_M3554E = np.array([
27-
(-943.401, 4.143, 8.825, -16.57, 952.216, 10.892),
28-
(539.853, 14.985, -1111.656, -0.812, 546.9, -18.949),
29-
(13.155, 533.082, -4.582, 534.843, 10.827, 536.327),
30-
(0.138, -10.419, 0.202, 0.14, 0.063, 10.518),
31-
(-0.075, 6.213, -0.239, -12.094, 0.181, 6.156),
32-
(-19.912, 0.082, -20.347, 0.022, -19.486, 0.013),
25+
LOADCELL_MATRIX = np.array([
26+
(-9.817417, -1895.974287, 14.899174, -4.333117, -40.684438, 1900.65478),
27+
(-32.435533, 1129.587599, -5.693634, -2206.916117, 14.346322, 1086.297672),
28+
(-963.721193, -4.911412, -965.891326, -1.404768, -981.000498, 8.693936),
29+
(19.854122, -0.21849, 0.25324, 0.734436, -20.471398, -0.232788),
30+
(-11.530613, -0.964467, 22.849398, -0.011803, -11.785854, 1.015088),
31+
(-0.296284, -26.299567, -0.138142, -26.753995, 0.385682, -25.052914),
3332
])
3433

3534
# ------------- TUNABLE FSM PARAMETERS ---------------- #
36-
BODY_WEIGHT = 10 * 9.8
35+
BODY_WEIGHT = 20 * 9.8
3736

3837
# STATE 1: EARLY STANCE
3938
ANKLE_K_ESTANCE = 19.874
@@ -189,18 +188,27 @@ def create_simple_walking_fsm(osl: OpenSourceLeg) -> StateMachine:
189188
gear_ratio=GEAR_RATIO,
190189
frequency=FREQUENCY,
191190
debug_level=0,
191+
torque_constant=0.145,
192192
dephy_log=False,
193193
),
194194
}
195195

196196
sensors = {
197197
"loadcell": NBLoadcellDAQ(
198-
LOADCELL_CALIBRATION_MATRIX_M3554E,
198+
LOADCELL_MATRIX,
199199
tag="loadcell",
200200
excitation_voltage=5.0,
201201
amp_gain=[34] * 3 + [151] * 3,
202202
spi_bus=1,
203203
),
204+
# "joint_encoder_ankle": AS5048B(
205+
# tag="joint_encoder_ankle",
206+
# bus="/dev/i2c-2",
207+
# A1_adr_pin=False,
208+
# A2_adr_pin=False,
209+
# zero_position=0,
210+
# enable_diagnostics=False,
211+
# ),
204212
}
205213

206214
clock = SoftRealtimeLoop(dt=1 / FREQUENCY)
@@ -217,6 +225,22 @@ def create_simple_walking_fsm(osl: OpenSourceLeg) -> StateMachine:
217225

218226
osl_fsm = create_simple_walking_fsm(osl)
219227

228+
# Zeroing the joint encoders
229+
def knee_homing_complete():
230+
osl.joint_encoder_knee.update()
231+
osl.joint_encoder_knee.zero_position = osl.joint_encoder_knee.counts
232+
print("Knee homing complete!")
233+
234+
def ankle_homing_complete():
235+
osl.joint_encoder_ankle.update()
236+
# The hard stop for ankle is at 30 deg from the zero position
237+
osl.joint_encoder_ankle.zero_position = osl.joint_encoder_ankle.counts - osl.joint_encoder_ankle.deg_to_counts(
238+
30
239+
)
240+
print("Ankle homing complete!")
241+
242+
callbacks = {"knee": knee_homing_complete, "ankle": ankle_homing_complete}
243+
220244
with fsm_logger, osl, osl_fsm:
221245
osl.update()
222246
osl.home()
@@ -225,7 +249,8 @@ def create_simple_walking_fsm(osl: OpenSourceLeg) -> StateMachine:
225249

226250
# ankle
227251
osl.ankle.set_control_mode(mode=CONTROL_MODES.IMPEDANCE)
228-
osl.ankle.set_impedance_gains()
252+
osl.ankle.set_impedance_cc_pidf_gains()
253+
osl.ankle.set_output_impedance()
229254

230255
for t in clock:
231256
osl.update()

‎examples/fsm_walking_python_controller.py‎

Lines changed: 30 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
from opensourceleg.control.fsm import State, StateMachine
66
from opensourceleg.logging.logger import Logger
77
from opensourceleg.robots.osl import OpenSourceLeg
8-
from opensourceleg.sensors.encoder import AS5048B
98
from opensourceleg.sensors.loadcell import NBLoadcellDAQ
109
from opensourceleg.utilities import SoftRealtimeLoop
1110

@@ -21,7 +20,7 @@
2120
])
2221

2322
# ------------- TUNABLE FSM PARAMETERS ---------------- #
24-
BODY_WEIGHT = 10 * 9.8 # 30 * 9.8
23+
BODY_WEIGHT = 30 * 9.8
2524

2625
# STATE 1: EARLY STANCE
2726
KNEE_K_ESTANCE = 99.372
@@ -217,22 +216,22 @@ def lswing_to_estance(osl: OpenSourceLeg) -> bool:
217216
amp_gain=[34] * 3 + [151] * 3,
218217
spi_bus=1,
219218
),
220-
"joint_encoder_knee": AS5048B(
221-
tag="joint_encoder_knee",
222-
bus="/dev/i2c-2",
223-
A1_adr_pin=False,
224-
A2_adr_pin=False,
225-
zero_position=0,
226-
enable_diagnostics=False,
227-
),
228-
"joint_encoder_ankle": AS5048B(
229-
tag="joint_encoder_ankle",
230-
bus="/dev/i2c-3",
231-
A1_adr_pin=False,
232-
A2_adr_pin=False,
233-
zero_position=0,
234-
enable_diagnostics=False,
235-
),
219+
# "joint_encoder_knee": AS5048B(
220+
# tag="joint_encoder_knee",
221+
# bus="/dev/i2c-2",
222+
# A1_adr_pin=False,
223+
# A2_adr_pin=False,
224+
# zero_position=0,
225+
# enable_diagnostics=False,
226+
# ),
227+
# "joint_encoder_ankle": AS5048B(
228+
# tag="joint_encoder_ankle",
229+
# bus="/dev/i2c-3",
230+
# A1_adr_pin=False,
231+
# A2_adr_pin=False,
232+
# zero_position=0,
233+
# enable_diagnostics=False,
234+
# ),
236235
}
237236

238237
clock = SoftRealtimeLoop(dt=1 / FREQUENCY)
@@ -267,7 +266,7 @@ def ankle_homing_complete():
267266

268267
with osl, osl_fsm:
269268
osl.update()
270-
osl.home(callbacks=callbacks)
269+
osl.home()
271270
input("Press Enter to start walking...")
272271

273272
# knee
@@ -280,25 +279,24 @@ def ankle_homing_complete():
280279
osl.ankle.set_impedance_cc_pidf_gains()
281280
osl.ankle.set_output_impedance()
282281

282+
# The FSM is expecting the loadcell values to be positive
283283
osl.loadcell.reset()
284284
osl.loadcell.calibrate()
285285

286286
for t in clock:
287287
osl.update()
288-
print("Ankle position", np.rad2deg(osl.sensors["joint_encoder_ankle"].position))
289-
print("Knee position", np.rad2deg(osl.sensors["joint_encoder_knee"].position))
290288
osl_fsm.update(osl=osl)
291-
# osl.knee.set_output_impedance(
292-
# k=osl_fsm.current_state.knee_stiffness,
293-
# b=osl_fsm.current_state.knee_damping,
294-
# )
295-
# osl.ankle.set_output_impedance(
296-
# k=osl_fsm.current_state.ankle_stiffness,
297-
# b=osl_fsm.current_state.ankle_damping,
298-
# )
299-
300-
# osl.knee.set_output_position(np.deg2rad(osl_fsm.current_state.knee_theta))
301-
# osl.ankle.set_output_position(np.deg2rad(osl_fsm.current_state.ankle_theta))
289+
osl.knee.set_output_impedance(
290+
k=osl_fsm.current_state.knee_stiffness,
291+
b=osl_fsm.current_state.knee_damping,
292+
)
293+
osl.ankle.set_output_impedance(
294+
k=osl_fsm.current_state.ankle_stiffness,
295+
b=osl_fsm.current_state.ankle_damping,
296+
)
297+
298+
osl.knee.set_output_position(np.deg2rad(osl_fsm.current_state.knee_theta))
299+
osl.ankle.set_output_position(np.deg2rad(osl_fsm.current_state.ankle_theta))
302300

303301
fsm_logger.info(
304302
f"T: {t:.3f}s, "

0 commit comments

Comments
 (0)