FSM Walking Ankle Controller¶
Overview¶
This example demonstrates how to use a finite state machine (FSM) to control the OSL ankle joint with impedance control. The FSM uses load cell feedback and the motor encoder (or optionally the joint encoder) to transition between states, applying different impedance parameters for each phase of the gait cycle. This script is intended for testing and development with the OSL hardware.
The FSM implements four distinct gait phases:
- Early Stance: Initial foot contact with moderate stiffness for shock absorption
- Late Stance: High stiffness for push-off power generation during heel rise
- Early Swing: Low stiffness allowing rapid dorsiflexion for ground clearance
- Late Swing: Maintains dorsiflexion position preparing for next heel strike
The figure below shows the basic execution of the controller:
The implementation is entirely in Python and uses the StateMachine
class from the control subpackage of this library.
Python Implementation¶
Setup and Configuration¶
First, perform the necessary imports and define the hardware and control parameters:
"""
A finite state machine to control the OSL ankle with impedance control.
This script can be helpful to test the OSL ankle using the load cell.
The fsm runs with the motor encoder, but the joint encoder can be also used.
Matteo Crotti
Soft Robotics for Human Cooperation and Rehabilitation Lab/Neurobionics Lab
Istituto Italiano di Tecnologia/University of Michigan
June 6, 2025
"""
import numpy as np
from opensourceleg.actuators.base import CONTROL_MODES
from opensourceleg.actuators.dephy import DephyActuator
from opensourceleg.control.fsm import State, StateMachine
from opensourceleg.logging.logger import Logger
from opensourceleg.robots.osl import OpenSourceLeg
from opensourceleg.sensors.loadcell import NBLoadcellDAQ
from opensourceleg.utilities import SoftRealtimeLoop
GEAR_RATIO = 9 * (83 / 18)
FREQUENCY = 200
LOADCELL_CALIBRATION_MATRIX_M3554E = np.array([
(-943.401, 4.143, 8.825, -16.57, 952.216, 10.892),
(539.853, 14.985, -1111.656, -0.812, 546.9, -18.949),
(13.155, 533.082, -4.582, 534.843, 10.827, 536.327),
(0.138, -10.419, 0.202, 0.14, 0.063, 10.518),
(-0.075, 6.213, -0.239, -12.094, 0.181, 6.156),
(-19.912, 0.082, -20.347, 0.022, -19.486, 0.013),
])
Next, define the tunable FSM parameters for each state and the transition thresholds:
BODY_WEIGHT = 10 * 9.8
# STATE 1: EARLY STANCE
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 = np.deg2rad(8.0)
# STATE 2: LATE STANCE
ANKLE_K_LSTANCE = 81 # 79.498
ANKLE_B_LSTANCE = 0.063
ANKLE_THETA_LSTANCE = -20
LOAD_ESWING: float = 1.0 * BODY_WEIGHT * 0.15
# STATE 3: EARLY SWING
ANKLE_K_ESWING = 7.949
ANKLE_B_ESWING = 0.0
ANKLE_THETA_ESWING = 25
ANKLE_DTHETA_ESWING_TO_LSWING = 3
ANKLE_THETA_ESWING_TO_LSWING = np.deg2rad(15.0)
# STATE 4: LATE SWING
ANKLE_K_LSWING = 7.949
ANKLE_B_LSWING = 0.06 # 0.0
ANKLE_THETA_LSWING = 15
LOAD_ESTANCE: float = 1.0 * BODY_WEIGHT * 0.4
ANKLE_THETA_LSWING_TO_ESTANCE = np.deg2rad(16.0)
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.
FSM Definition¶
The FSM consists of four states representing the phases of the gait cycle, each with specific impedance parameters. We first define the transition criteria functions that determine when to switch between states based on sensor feedback:
Transition Functions¶
The transition criteria are implemented as functions that take the OSL object and return a boolean indicating whether the transition should occur:
- Early Stance → Late Stance: Triggered when load cell force exceeds threshold AND ankle position is dorsiflexed enough
- Late Stance → Early Swing: Triggered when load cell force drops below threshold (heel off)
- Early Swing → Late Swing: Triggered when ankle reaches target dorsiflexion AND velocity decreases
- Late Swing → Early Stance: Triggered when load cell force increases (heel strike) OR ankle plantarflexes sufficiently
State Machine Creation¶
Next, we create a function that returns an instance of the StateMachine
class. Create the FSM by defining the states, their impedance parameters, and the transition criteria functions. Each transition is based on sensor feedback or joint state:
# ---------------------------------------------------- #
def estance_to_lstance(osl: OpenSourceLeg) -> bool:
"""
Transition from early stance to late stance when the loadcell
reads a force greater than a threshold and the ankle position
is greater than a threshold.
"""
if osl.loadcell is None:
raise ValueError("Loadcell is not connected")
if osl.ankle is None:
raise ValueError("Ankle is not connected")
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.
"""
if osl.loadcell is None:
raise ValueError("Loadcell is not connected")
return bool(-(osl.loadcell.fz) < LOAD_ESWING)
def eswing_to_lswing(osl: OpenSourceLeg) -> bool:
"""
Transition from early swing to late swing when the ankle angle
is greater than a threshold and the ankle velocity is less than
a threshold.
"""
if osl.ankle is None:
raise ValueError("Ankle is not connected")
return bool(
osl.ankle.output_position > ANKLE_THETA_ESWING_TO_LSWING
and osl.ankle.output_velocity < ANKLE_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 ankle angle is
less than a threshold.
"""
if osl.loadcell is None:
raise ValueError("Loadcell is not connected")
if osl.ankle is None:
raise ValueError("Ankle is not connected")
return bool(-osl.loadcell.fz > LOAD_ESTANCE or osl.ankle.output_position < ANKLE_THETA_LSWING_TO_ESTANCE)
def create_simple_walking_fsm(osl: OpenSourceLeg) -> StateMachine:
e_stance = State(
name="e_stance",
ankle_theta=ANKLE_THETA_ESTANCE,
ankle_stiffness=ANKLE_K_ESTANCE,
ankle_damping=ANKLE_B_ESTANCE,
)
l_stance = State(
name="l_stance",
ankle_theta=ANKLE_THETA_LSTANCE,
ankle_stiffness=ANKLE_K_LSTANCE,
ankle_damping=ANKLE_B_LSTANCE,
)
e_swing = State(
name="e_swing",
ankle_theta=ANKLE_THETA_ESWING,
ankle_stiffness=ANKLE_K_ESWING,
ankle_damping=ANKLE_B_ESWING,
)
l_swing = State(
name="l_swing",
ankle_theta=ANKLE_THETA_LSWING,
ankle_stiffness=ANKLE_K_LSWING,
ankle_damping=ANKLE_B_LSWING,
)
fsm = StateMachine(
states=[
e_stance,
l_stance,
e_swing,
l_swing,
],
initial_state_name="e_stance",
)
fsm.add_transition(
source=e_stance,
destination=l_stance,
event_name="foot_flat",
criteria=estance_to_lstance,
)
fsm.add_transition(
source=l_stance,
destination=e_swing,
event_name="heel_off",
criteria=lstance_to_eswing,
)
fsm.add_transition(
source=e_swing,
destination=l_swing,
event_name="toe_off",
criteria=eswing_to_lswing,
)
fsm.add_transition(
source=l_swing,
destination=e_stance,
event_name="heel_strike",
criteria=lswing_to_estance,
)
return fsm
Note: If instantiating the OSL hardware and sensors is unfamiliar, check out the the tutorials pages.
Hardware Initialization¶
Initialize the required hardware components including the Dephy actuator, load cell sensor, data logger, and real-time control loop. The system uses a 200Hz control frequency for responsive control:
if __name__ == "__main__":
actuators = {
"ankle": DephyActuator(
tag="ankle",
port="/dev/ttyACM0",
gear_ratio=GEAR_RATIO,
frequency=FREQUENCY,
debug_level=0,
dephy_log=False,
),
}
sensors = {
"loadcell": NBLoadcellDAQ(
LOADCELL_CALIBRATION_MATRIX_M3554E,
tag="loadcell",
excitation_voltage=5.0,
amp_gain=[34] * 3 + [151] * 3,
spi_bus=1,
),
}
clock = SoftRealtimeLoop(dt=1 / FREQUENCY)
fsm_logger = Logger(
log_path="./logs",
file_name="fsm.log",
)
osl = OpenSourceLeg(
tag="osl",
actuators=actuators,
sensors=sensors,
)
osl_fsm = create_simple_walking_fsm(osl)
Main Loop¶
The main control loop homes the OSL, sets the ankle to impedance mode, and runs the control algorithm. In each iteration, the system updates hardware sensors, evaluates FSM transitions, applies impedance control, and logs relevant data:
Full Code¶
"""
A finite state machine to control the OSL ankle with impedance control.
This script can be helpful to test the OSL ankle using the load cell.
The fsm runs with the motor encoder, but the joint encoder can be also used.
Matteo Crotti
Soft Robotics for Human Cooperation and Rehabilitation Lab/Neurobionics Lab
Istituto Italiano di Tecnologia/University of Michigan
June 6, 2025
"""
import numpy as np
from opensourceleg.actuators.base import CONTROL_MODES
from opensourceleg.actuators.dephy import DephyActuator
from opensourceleg.control.fsm import State, StateMachine
from opensourceleg.logging.logger import Logger
from opensourceleg.robots.osl import OpenSourceLeg
from opensourceleg.sensors.loadcell import NBLoadcellDAQ
from opensourceleg.utilities import SoftRealtimeLoop
GEAR_RATIO = 9 * (83 / 18)
FREQUENCY = 200
LOADCELL_CALIBRATION_MATRIX_M3554E = np.array([
(-943.401, 4.143, 8.825, -16.57, 952.216, 10.892),
(539.853, 14.985, -1111.656, -0.812, 546.9, -18.949),
(13.155, 533.082, -4.582, 534.843, 10.827, 536.327),
(0.138, -10.419, 0.202, 0.14, 0.063, 10.518),
(-0.075, 6.213, -0.239, -12.094, 0.181, 6.156),
(-19.912, 0.082, -20.347, 0.022, -19.486, 0.013),
])
# ------------- TUNABLE FSM PARAMETERS ---------------- #
BODY_WEIGHT = 10 * 9.8
# STATE 1: EARLY STANCE
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 = np.deg2rad(8.0)
# STATE 2: LATE STANCE
ANKLE_K_LSTANCE = 81 # 79.498
ANKLE_B_LSTANCE = 0.063
ANKLE_THETA_LSTANCE = -20
LOAD_ESWING: float = 1.0 * BODY_WEIGHT * 0.15
# STATE 3: EARLY SWING
ANKLE_K_ESWING = 7.949
ANKLE_B_ESWING = 0.0
ANKLE_THETA_ESWING = 25
ANKLE_DTHETA_ESWING_TO_LSWING = 3
ANKLE_THETA_ESWING_TO_LSWING = np.deg2rad(15.0)
# STATE 4: LATE SWING
ANKLE_K_LSWING = 7.949
ANKLE_B_LSWING = 0.06 # 0.0
ANKLE_THETA_LSWING = 15
LOAD_ESTANCE: float = 1.0 * BODY_WEIGHT * 0.4
ANKLE_THETA_LSWING_TO_ESTANCE = np.deg2rad(16.0)
# ---------------------------------------------------- #
def estance_to_lstance(osl: OpenSourceLeg) -> bool:
"""
Transition from early stance to late stance when the loadcell
reads a force greater than a threshold and the ankle position
is greater than a threshold.
"""
if osl.loadcell is None:
raise ValueError("Loadcell is not connected")
if osl.ankle is None:
raise ValueError("Ankle is not connected")
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.
"""
if osl.loadcell is None:
raise ValueError("Loadcell is not connected")
return bool(-(osl.loadcell.fz) < LOAD_ESWING)
def eswing_to_lswing(osl: OpenSourceLeg) -> bool:
"""
Transition from early swing to late swing when the ankle angle
is greater than a threshold and the ankle velocity is less than
a threshold.
"""
if osl.ankle is None:
raise ValueError("Ankle is not connected")
return bool(
osl.ankle.output_position > ANKLE_THETA_ESWING_TO_LSWING
and osl.ankle.output_velocity < ANKLE_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 ankle angle is
less than a threshold.
"""
if osl.loadcell is None:
raise ValueError("Loadcell is not connected")
if osl.ankle is None:
raise ValueError("Ankle is not connected")
return bool(-osl.loadcell.fz > LOAD_ESTANCE or osl.ankle.output_position < ANKLE_THETA_LSWING_TO_ESTANCE)
def create_simple_walking_fsm(osl: OpenSourceLeg) -> StateMachine:
e_stance = State(
name="e_stance",
ankle_theta=ANKLE_THETA_ESTANCE,
ankle_stiffness=ANKLE_K_ESTANCE,
ankle_damping=ANKLE_B_ESTANCE,
)
l_stance = State(
name="l_stance",
ankle_theta=ANKLE_THETA_LSTANCE,
ankle_stiffness=ANKLE_K_LSTANCE,
ankle_damping=ANKLE_B_LSTANCE,
)
e_swing = State(
name="e_swing",
ankle_theta=ANKLE_THETA_ESWING,
ankle_stiffness=ANKLE_K_ESWING,
ankle_damping=ANKLE_B_ESWING,
)
l_swing = State(
name="l_swing",
ankle_theta=ANKLE_THETA_LSWING,
ankle_stiffness=ANKLE_K_LSWING,
ankle_damping=ANKLE_B_LSWING,
)
fsm = StateMachine(
states=[
e_stance,
l_stance,
e_swing,
l_swing,
],
initial_state_name="e_stance",
)
fsm.add_transition(
source=e_stance,
destination=l_stance,
event_name="foot_flat",
criteria=estance_to_lstance,
)
fsm.add_transition(
source=l_stance,
destination=e_swing,
event_name="heel_off",
criteria=lstance_to_eswing,
)
fsm.add_transition(
source=e_swing,
destination=l_swing,
event_name="toe_off",
criteria=eswing_to_lswing,
)
fsm.add_transition(
source=l_swing,
destination=e_stance,
event_name="heel_strike",
criteria=lswing_to_estance,
)
return fsm
if __name__ == "__main__":
actuators = {
"ankle": DephyActuator(
tag="ankle",
port="/dev/ttyACM0",
gear_ratio=GEAR_RATIO,
frequency=FREQUENCY,
debug_level=0,
dephy_log=False,
),
}
sensors = {
"loadcell": NBLoadcellDAQ(
LOADCELL_CALIBRATION_MATRIX_M3554E,
tag="loadcell",
excitation_voltage=5.0,
amp_gain=[34] * 3 + [151] * 3,
spi_bus=1,
),
}
clock = SoftRealtimeLoop(dt=1 / FREQUENCY)
fsm_logger = Logger(
log_path="./logs",
file_name="fsm.log",
)
osl = OpenSourceLeg(
tag="osl",
actuators=actuators,
sensors=sensors,
)
osl_fsm = create_simple_walking_fsm(osl)
with fsm_logger, osl, osl_fsm:
osl.update()
osl.home()
input("Press Enter to start walking...")
# ankle
osl.ankle.set_control_mode(mode=CONTROL_MODES.IMPEDANCE)
osl.ankle.set_impedance_gains()
for t in clock:
osl.update()
osl_fsm.update(osl=osl)
osl.ankle.set_output_impedance(
k=osl_fsm.current_state.ankle_stiffness,
b=osl_fsm.current_state.ankle_damping,
)
osl.ankle.set_output_position(np.deg2rad(osl_fsm.current_state.ankle_theta))
fsm_logger.info(
f"T: {t:.3f}s, "
f"Current state: {osl_fsm.current_state.name}; "
f"Loadcell Fz: {osl.loadcell.fz:.3f} N; "
f"Ankle theta: {np.rad2deg(osl.ankle.output_position):.3f} deg; "
f"Ankle winding temperature: {osl.ankle.winding_temperature:.3f} c; "
)
Notes¶
Hardware Requirements¶
- This example is designed for use with the OSL hardware and requires the appropriate sensors and actuators to be connected.
- A 6-axis load cell (NBLoadcellDAQ) is required for ground reaction force measurements.
- The Dephy actuator must be properly configured and connected via USB.
Parameter Tuning¶
- The FSM transitions and impedance parameters may need to be tuned for your specific application or hardware setup.
- Body weight scaling affects the load thresholds - adjust
BODY_WEIGHT
parameter accordingly
Additional Resources¶
- For more information on the OSL library and hardware setup, refer to the tutorials.