Finite State Machine Controller¶
Overview¶
The library ships with three example implementations of the same finite state machine (FSM) walking controller. The figure below shows the basic execution of the controller:
The first implementation is entirely in Python and uses the StateMachine class from the control subpackage of this library. If you plan to write your controllers exclusively in Python, this example is a good starting point.
The library also supports using compiled C and C++ library functions via the CompiledController class. You can see a basic example of this module on the tutorials page, which may be helpful to review before starting with this example. We've duplicated the FSM behavior in both C++ and MATLAB. The source code for these control implementations is available in this repository. Refer to the documentation in that repository for instructions on compiling both the C++ and the MATLAB source code.
Python Implementation¶
Setup and Configuration¶
First, we'll perform some standard imports:
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.encoder import AS5048B
from opensourceleg.sensors.loadcell import DephyLoadcellAmplifier
from opensourceleg.utilities import SoftRealtimeLoop
GEAR_RATIO = 9 * (83 / 18)
FREQUENCY = 200
LOADCELL_CALIBRATION_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),
])
Next, we'll define all the tunable FSM parameters. These include the impedance parameters for each state as well as the transitions between states:
# ------------- TUNABLE FSM PARAMETERS ---------------- #
BODY_WEIGHT = 30 * 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 = np.deg2rad(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 = np.deg2rad(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 = np.deg2rad(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 create a function that returns an instance of the StateMachine class. We start by making State objects for each of our four states and the include the impedance parameters in each. We also define transition criteria functions that determine when to move between states. We add the states and transition functions to an instance of the StateMachine class, specifiying that the initial state is e_stance.
def create_simple_walking_fsm(osl: OpenSourceLeg) -> StateMachine:
e_stance = State(
name="e_stance",
knee_theta=KNEE_THETA_ESTANCE,
knee_stiffness=KNEE_K_ESTANCE,
knee_damping=KNEE_B_ESTANCE,
ankle_theta=ANKLE_THETA_ESTANCE,
ankle_stiffness=ANKLE_K_ESTANCE,
ankle_damping=ANKLE_B_ESTANCE,
)
l_stance = State(
name="l_stance",
knee_theta=KNEE_THETA_LSTANCE,
knee_stiffness=KNEE_K_LSTANCE,
knee_damping=KNEE_B_LSTANCE,
ankle_theta=ANKLE_THETA_LSTANCE,
ankle_stiffness=ANKLE_K_LSTANCE,
ankle_damping=ANKLE_B_LSTANCE,
)
e_swing = State(
name="e_swing",
knee_theta=KNEE_THETA_ESWING,
knee_stiffness=KNEE_K_ESWING,
knee_damping=KNEE_B_ESWING,
ankle_theta=ANKLE_THETA_ESWING,
ankle_stiffness=ANKLE_K_ESWING,
ankle_damping=ANKLE_B_ESWING,
)
l_swing = State(
name="l_swing",
knee_theta=KNEE_THETA_LSWING,
knee_stiffness=KNEE_K_LSWING,
knee_damping=KNEE_B_LSWING,
ankle_theta=ANKLE_THETA_LSWING,
ankle_stiffness=ANKLE_K_LSWING,
ankle_damping=ANKLE_B_LSWING,
)
def estance_to_lstance(osl: OpenSourceLeg) -> bool:
"""
Transition from early stance to late stance when the loadcell
reads a force greater than a threshold.
"""
if osl.loadcell is None:
raise ValueError("Loadcell 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 knee angle
is greater than a threshold and the knee velocity is less than
a threshold.
"""
if osl.knee is None:
raise ValueError("Knee is not connected")
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.
"""
if osl.knee is None:
raise ValueError("Knee is not connected")
if osl.loadcell is None:
raise ValueError("Loadcell is not connected")
return bool(osl.loadcell.fz > LOAD_ESTANCE or osl.knee.output_position < KNEE_THETA_LSWING_TO_ESTANCE)
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.
Next, we initialize the standard actuators, sensors, logger, and loop classes for the OSL:
if __name__ == "__main__":
actuators = {
"knee": DephyActuator(
tag="knee",
port="/dev/ttyACM0",
gear_ratio=GEAR_RATIO,
frequency=FREQUENCY,
debug_level=0,
dephy_log=False,
),
"ankle": DephyActuator(
tag="ankle",
port="/dev/ttyACM1",
gear_ratio=GEAR_RATIO,
frequency=FREQUENCY,
debug_level=0,
dephy_log=False,
),
}
sensors = {
"loadcell": DephyLoadcellAmplifier(
calibration_matrix=LOADCELL_CALIBRATION_MATRIX,
),
"joint_encoder_knee": AS5048B(
tag="joint_encoder_knee",
bus=1,
A1_adr_pin=True,
A2_adr_pin=False,
zero_position=0,
enable_diagnostics=False,
),
"joint_encoder_ankle": AS5048B(
tag="joint_encoder_ankle",
bus=1,
A1_adr_pin=False,
A2_adr_pin=True,
zero_position=0,
enable_diagnostics=False,
),
}
clock = SoftRealtimeLoop(dt=1 / FREQUENCY)
fsm_logger = Logger(
log_path="./logs",
file_name="fsm.log",
)
osl = OpenSourceLeg(
tag="osl",
actuators=actuators,
sensors=sensors,
)
We then call the FSM definition function we made above and initialize/home the hardware:
osl_fsm = create_simple_walking_fsm(osl)
with osl, osl_fsm:
osl.update()
osl.home()
input("Press Enter to start walking...")
# knee
osl.knee.set_control_mode(mode=CONTROL_MODES.IMPEDANCE)
osl.knee.set_impedance_gains()
# ankle
osl.ankle.set_control_mode(mode=CONTROL_MODES.IMPEDANCE)
osl.ankle.set_impedance_gains()
Main Loop¶
Now that everything is set up, we 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 and logging are also included for debugging:
for t in clock:
osl.update()
osl_fsm.update(osl=osl)
osl.knee.set_output_impedance(
k=osl_fsm.current_state.knee_stiffness,
b=osl_fsm.current_state.knee_damping,
)
osl.ankle.set_output_impedance(
k=osl_fsm.current_state.ankle_stiffness,
b=osl_fsm.current_state.ankle_damping,
)
osl.knee.set_output_position(np.deg2rad(osl_fsm.current_state.knee_theta))
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"Knee theta: {np.rad2deg(osl.knee.output_position):.3f} deg; "
f"Ankle theta: {np.rad2deg(osl.ankle.output_position):.3f} deg; "
f"Knee winding temperature: {osl.knee.winding_temperature:.3f} c; "
f"Ankle winding temperature: {osl.ankle.winding_temperature:.3f} c; "
)
Full Code for The Python Implementation¶
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.encoder import AS5048B
from opensourceleg.sensors.loadcell import DephyLoadcellAmplifier
from opensourceleg.utilities import SoftRealtimeLoop
GEAR_RATIO = 9 * (83 / 18)
FREQUENCY = 200
LOADCELL_CALIBRATION_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),
])
# ------------- TUNABLE FSM PARAMETERS ---------------- #
BODY_WEIGHT = 30 * 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 = np.deg2rad(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 = np.deg2rad(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 = np.deg2rad(30)
# ---------------------------------------------------- #
def create_simple_walking_fsm(osl: OpenSourceLeg) -> StateMachine:
e_stance = State(
name="e_stance",
knee_theta=KNEE_THETA_ESTANCE,
knee_stiffness=KNEE_K_ESTANCE,
knee_damping=KNEE_B_ESTANCE,
ankle_theta=ANKLE_THETA_ESTANCE,
ankle_stiffness=ANKLE_K_ESTANCE,
ankle_damping=ANKLE_B_ESTANCE,
)
l_stance = State(
name="l_stance",
knee_theta=KNEE_THETA_LSTANCE,
knee_stiffness=KNEE_K_LSTANCE,
knee_damping=KNEE_B_LSTANCE,
ankle_theta=ANKLE_THETA_LSTANCE,
ankle_stiffness=ANKLE_K_LSTANCE,
ankle_damping=ANKLE_B_LSTANCE,
)
e_swing = State(
name="e_swing",
knee_theta=KNEE_THETA_ESWING,
knee_stiffness=KNEE_K_ESWING,
knee_damping=KNEE_B_ESWING,
ankle_theta=ANKLE_THETA_ESWING,
ankle_stiffness=ANKLE_K_ESWING,
ankle_damping=ANKLE_B_ESWING,
)
l_swing = State(
name="l_swing",
knee_theta=KNEE_THETA_LSWING,
knee_stiffness=KNEE_K_LSWING,
knee_damping=KNEE_B_LSWING,
ankle_theta=ANKLE_THETA_LSWING,
ankle_stiffness=ANKLE_K_LSWING,
ankle_damping=ANKLE_B_LSWING,
)
def estance_to_lstance(osl: OpenSourceLeg) -> bool:
"""
Transition from early stance to late stance when the loadcell
reads a force greater than a threshold.
"""
if osl.loadcell is None:
raise ValueError("Loadcell 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 knee angle
is greater than a threshold and the knee velocity is less than
a threshold.
"""
if osl.knee is None:
raise ValueError("Knee is not connected")
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.
"""
if osl.knee is None:
raise ValueError("Knee is not connected")
if osl.loadcell is None:
raise ValueError("Loadcell is not connected")
return bool(osl.loadcell.fz > LOAD_ESTANCE or osl.knee.output_position < KNEE_THETA_LSWING_TO_ESTANCE)
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 = {
"knee": DephyActuator(
tag="knee",
port="/dev/ttyACM0",
gear_ratio=GEAR_RATIO,
frequency=FREQUENCY,
debug_level=0,
dephy_log=False,
),
"ankle": DephyActuator(
tag="ankle",
port="/dev/ttyACM1",
gear_ratio=GEAR_RATIO,
frequency=FREQUENCY,
debug_level=0,
dephy_log=False,
),
}
sensors = {
"loadcell": DephyLoadcellAmplifier(
calibration_matrix=LOADCELL_CALIBRATION_MATRIX,
),
"joint_encoder_knee": AS5048B(
tag="joint_encoder_knee",
bus=1,
A1_adr_pin=True,
A2_adr_pin=False,
zero_position=0,
enable_diagnostics=False,
),
"joint_encoder_ankle": AS5048B(
tag="joint_encoder_ankle",
bus=1,
A1_adr_pin=False,
A2_adr_pin=True,
zero_position=0,
enable_diagnostics=False,
),
}
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 osl, osl_fsm:
osl.update()
osl.home()
input("Press Enter to start walking...")
# knee
osl.knee.set_control_mode(mode=CONTROL_MODES.IMPEDANCE)
osl.knee.set_impedance_gains()
# 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.knee.set_output_impedance(
k=osl_fsm.current_state.knee_stiffness,
b=osl_fsm.current_state.knee_damping,
)
osl.ankle.set_output_impedance(
k=osl_fsm.current_state.ankle_stiffness,
b=osl_fsm.current_state.ankle_damping,
)
osl.knee.set_output_position(np.deg2rad(osl_fsm.current_state.knee_theta))
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"Knee theta: {np.rad2deg(osl.knee.output_position):.3f} deg; "
f"Ankle theta: {np.rad2deg(osl.ankle.output_position):.3f} deg; "
f"Knee winding temperature: {osl.knee.winding_temperature:.3f} c; "
f"Ankle winding temperature: {osl.ankle.winding_temperature:.3f} c; "
)
C++ and MATLAB Implementation¶
To get started, ensure you have compiled either the C++ or MATLAB source code and have a FSMController.so library. If not, refer to the source repository for compilation instructions. To run this example, make sure the generated library is in the same directory as your script, or modify the search path for the library when loading the controller.
Load Compiled Library¶
First, perform standard imports, handle paths, and set up the hardware for the OSL joints and sensors
import inspect
import os
import numpy as np
from opensourceleg.actuators.dephy import DephyActuator
from opensourceleg.control.compiled import CompiledController
from opensourceleg.sensors.loadcell import DephyLoadcellAmplifier
from opensourceleg.utilities import SoftRealtimeLoop, units
use_offline_mode = False
FREQUENCY = 200
clock = SoftRealtimeLoop(dt=1 / FREQUENCY)
knee = DephyActuator(
tag="knee",
firmware_version="7.2.0",
port="/dev/ttyACM0",
gear_ratio=9 * 83 / 18,
frequency=FREQUENCY,
)
ankle = DephyActuator(
tag="ankle",
firmware_version="7.2.0",
port="/dev/ttyACM1",
gear_ratio=9 * 83 / 18,
frequency=FREQUENCY,
)
actuators = [knee, ankle]
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),
])
loadcell = DephyLoadcellAmplifier(
calibration_matrix=LOADCELL_MATRIX,
bus=1,
)
sensors = [loadcell]
Note: If instantiating the OSL hardware and sensors is unfamiliar, check out the the tutorials pages.
Next, instantiate a CompiledController wrapper object:
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
controller = CompiledController(
library_name="FSMController",
library_path=currentdir,
main_function_name="FSMController",
initialization_function_name="FSMController_initialize",
cleanup_function_name="FSMController_terminate",
)
Define Custom Datatypes¶
Define the data structures used in the controller. These must match the size and order of the structures used to create the library.
controller.define_type(
"impedance_param_type",
[
("stiffness", controller.types.c_double),
("damping", controller.types.c_double),
("eq_angle", controller.types.c_double),
],
)
controller.define_type(
"joint_impedance_set",
[
("early_stance", controller.types.impedance_param_type),
("late_stance", controller.types.impedance_param_type),
("early_swing", controller.types.impedance_param_type),
("late_swing", controller.types.impedance_param_type),
],
)
controller.define_type(
"transition_parameters",
[
("min_time_in_state", controller.types.c_double),
("loadLStance", controller.types.c_double),
("ankleThetaEstanceToLstance", controller.types.c_double),
("kneeThetaESwingToLSwing", controller.types.c_double),
("kneeDthetaESwingToLSwing", controller.types.c_double),
("loadESwing", controller.types.c_double),
("loadEStance", controller.types.c_double),
("kneeThetaLSwingToEStance", controller.types.c_double),
],
)
controller.define_type(
"UserParameters",
[
("body_weight", controller.types.c_double),
("knee_impedance", controller.types.joint_impedance_set),
("ankle_impedance", controller.types.joint_impedance_set),
("transition_parameters", controller.types.transition_parameters),
],
)
controller.define_type("sensors", controller.DEFAULT_SENSOR_LIST)
controller.define_inputs([
("parameters", controller.types.UserParameters),
("sensors", controller.types.sensors),
("time", controller.types.c_double),
])
controller.define_outputs([
("current_state", controller.types.c_int),
("time_in_current_state", controller.types.c_double),
("knee_impedance", controller.types.impedance_param_type),
("ankle_impedance", controller.types.impedance_param_type),
])
Configure Impedance and Transition Parameters¶
Configure the impedance and transition parameters based on predefined tuning. Adjust these values to achieve the desired behavior:
controller.inputs.parameters.knee_impedance.early_stance.stiffness = 99.372
controller.inputs.parameters.knee_impedance.early_stance.damping = 3.180
controller.inputs.parameters.knee_impedance.early_stance.eq_angle = 5
controller.inputs.parameters.knee_impedance.late_stance.stiffness = 99.372
controller.inputs.parameters.knee_impedance.late_stance.damping = 1.272
controller.inputs.parameters.knee_impedance.late_stance.eq_angle = 8
controller.inputs.parameters.knee_impedance.early_swing.stiffness = 39.746
controller.inputs.parameters.knee_impedance.early_swing.damping = 0.063
controller.inputs.parameters.knee_impedance.early_swing.eq_angle = 60
controller.inputs.parameters.knee_impedance.late_swing.stiffness = 15.899
controller.inputs.parameters.knee_impedance.late_swing.damping = 3.186
controller.inputs.parameters.knee_impedance.late_swing.eq_angle = 5
controller.inputs.parameters.ankle_impedance.early_stance.stiffness = 19.874
controller.inputs.parameters.ankle_impedance.early_stance.damping = 0
controller.inputs.parameters.ankle_impedance.early_stance.eq_angle = -2
controller.inputs.parameters.ankle_impedance.late_stance.stiffness = 79.498
controller.inputs.parameters.ankle_impedance.late_stance.damping = 0.063
controller.inputs.parameters.ankle_impedance.late_stance.eq_angle = -20
controller.inputs.parameters.ankle_impedance.early_swing.stiffness = 7.949
controller.inputs.parameters.ankle_impedance.early_swing.damping = 0
controller.inputs.parameters.ankle_impedance.early_swing.eq_angle = 25
controller.inputs.parameters.ankle_impedance.late_swing.stiffness = 7.949
controller.inputs.parameters.ankle_impedance.late_swing.damping = 0.0
controller.inputs.parameters.ankle_impedance.late_swing.eq_angle = 15
# Configure state machine
body_weight = 82 # kg
controller.inputs.parameters.body_weight = body_weight
controller.inputs.parameters.transition_parameters.min_time_in_state = 0.20
controller.inputs.parameters.transition_parameters.loadLStance = -body_weight * 0.25
controller.inputs.parameters.transition_parameters.ankleThetaEstanceToLstance = 6.0
controller.inputs.parameters.transition_parameters.loadESwing = -body_weight * 0.15
controller.inputs.parameters.transition_parameters.kneeThetaESwingToLSwing = 50
controller.inputs.parameters.transition_parameters.kneeDthetaESwingToLSwing = 3
controller.inputs.parameters.transition_parameters.loadEStance = -body_weight * 0.4
controller.inputs.parameters.transition_parameters.kneeThetaLSwingToEStance = 30
Main Loop¶
After configuration, home the OSL joints, calibrate the loadcell, set the joints to impedance mode, and begin running the controller. During each loop iteration, update the inputs, call the run() method, and write outputs to the hardware:
with knee, ankle, loadcell:
knee.home()
ankle.home()
knee.set_control_mode(knee.CONTROL_MODES.IMPEDANCE)
ankle.set_control_mode(ankle.CONTROL_MODES.IMPEDANCE)
# Main Loop
for t in clock:
knee.update()
ankle.update()
loadcell.update()
controller.inputs.sensors.knee_angle = units.convert_from_default(knee.output_position, units.position.deg)
controller.inputs.sensors.ankle_angle = units.convert_from_default(ankle.output_position, units.position.deg)
controller.inputs.sensors.knee_velocity = units.convert_from_default(
knee.output_velocity, units.velocity.deg_per_s
)
controller.inputs.sensors.ankle_velocity = units.convert_from_default(
ankle.output_velocity, units.velocity.deg_per_s
)
controller.inputs.sensors.Fz = loadcell.fz
# Update any control inputs that change every loop
controller.inputs.time = t
# Call the controller
outputs = controller.run()
# Test print to ensure external library call works
print(
f"Current time in state {outputs.current_state}: {outputs.time_in_current_state:.2f} seconds, \
Knee Eq {outputs.knee_impedance.eq_angle:.2f}, \
Ankle Eq {outputs.ankle_impedance.eq_angle:.2f}, \
Fz {loadcell.fz:.2f}",
end="\r",
)
# Write to the hardware
knee.set_output_impedance(
k=units.convert_to_default(outputs.knee_impedance.stiffness, units.stiffness.N_m_per_rad),
b=units.convert_to_default(outputs.knee_impedance.damping, units.damping.N_m_per_rad_per_s),
)
knee.set_output_position(value=units.convert_to_default(outputs.knee_impedance.eq_angle, units.position.deg))
ankle.set_output_impedance(
k=units.convert_to_default(outputs.ankle_impedance.stiffness, units.stiffness.N_m_per_rad),
b=units.convert_to_default(outputs.ankle_impedance.damping, units.damping.N_m_per_rad_per_s),
)
ankle.set_output_position(value=units.convert_to_default(outputs.ankle_impedance.eq_angle, units.position.deg))
print("\n")
Note: Be careful with units when writing outputs to the hardware. Convert values to the appropriate units if necessary.
Full Code for The Compiled Controller Implementation¶
"""
Example walking controller for the OSL.
Senthur Raj Ayyappan, Kevin Best
Neurobionics Lab
Robotics Department
University of Michigan
October 9, 2023
"""
import inspect
import os
import numpy as np
from opensourceleg.actuators.dephy import DephyActuator
from opensourceleg.control.compiled import CompiledController
from opensourceleg.sensors.loadcell import DephyLoadcellAmplifier
from opensourceleg.utilities import SoftRealtimeLoop, units
use_offline_mode = False
FREQUENCY = 200
clock = SoftRealtimeLoop(dt=1 / FREQUENCY)
knee = DephyActuator(
tag="knee",
firmware_version="7.2.0",
port="/dev/ttyACM0",
gear_ratio=9 * 83 / 18,
frequency=FREQUENCY,
)
ankle = DephyActuator(
tag="ankle",
firmware_version="7.2.0",
port="/dev/ttyACM1",
gear_ratio=9 * 83 / 18,
frequency=FREQUENCY,
)
actuators = [knee, ankle]
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),
])
loadcell = DephyLoadcellAmplifier(
calibration_matrix=LOADCELL_MATRIX,
bus=1,
)
sensors = [loadcell]
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
controller = CompiledController(
library_name="FSMController",
library_path=currentdir,
main_function_name="FSMController",
initialization_function_name="FSMController_initialize",
cleanup_function_name="FSMController_terminate",
)
controller.define_type(
"impedance_param_type",
[
("stiffness", controller.types.c_double),
("damping", controller.types.c_double),
("eq_angle", controller.types.c_double),
],
)
controller.define_type(
"joint_impedance_set",
[
("early_stance", controller.types.impedance_param_type),
("late_stance", controller.types.impedance_param_type),
("early_swing", controller.types.impedance_param_type),
("late_swing", controller.types.impedance_param_type),
],
)
controller.define_type(
"transition_parameters",
[
("min_time_in_state", controller.types.c_double),
("loadLStance", controller.types.c_double),
("ankleThetaEstanceToLstance", controller.types.c_double),
("kneeThetaESwingToLSwing", controller.types.c_double),
("kneeDthetaESwingToLSwing", controller.types.c_double),
("loadESwing", controller.types.c_double),
("loadEStance", controller.types.c_double),
("kneeThetaLSwingToEStance", controller.types.c_double),
],
)
controller.define_type(
"UserParameters",
[
("body_weight", controller.types.c_double),
("knee_impedance", controller.types.joint_impedance_set),
("ankle_impedance", controller.types.joint_impedance_set),
("transition_parameters", controller.types.transition_parameters),
],
)
controller.define_type("sensors", controller.DEFAULT_SENSOR_LIST)
controller.define_inputs([
("parameters", controller.types.UserParameters),
("sensors", controller.types.sensors),
("time", controller.types.c_double),
])
controller.define_outputs([
("current_state", controller.types.c_int),
("time_in_current_state", controller.types.c_double),
("knee_impedance", controller.types.impedance_param_type),
("ankle_impedance", controller.types.impedance_param_type),
])
# Populate Controller inputs as needed
controller.inputs.parameters.knee_impedance.early_stance.stiffness = 99.372
controller.inputs.parameters.knee_impedance.early_stance.damping = 3.180
controller.inputs.parameters.knee_impedance.early_stance.eq_angle = 5
controller.inputs.parameters.knee_impedance.late_stance.stiffness = 99.372
controller.inputs.parameters.knee_impedance.late_stance.damping = 1.272
controller.inputs.parameters.knee_impedance.late_stance.eq_angle = 8
controller.inputs.parameters.knee_impedance.early_swing.stiffness = 39.746
controller.inputs.parameters.knee_impedance.early_swing.damping = 0.063
controller.inputs.parameters.knee_impedance.early_swing.eq_angle = 60
controller.inputs.parameters.knee_impedance.late_swing.stiffness = 15.899
controller.inputs.parameters.knee_impedance.late_swing.damping = 3.186
controller.inputs.parameters.knee_impedance.late_swing.eq_angle = 5
controller.inputs.parameters.ankle_impedance.early_stance.stiffness = 19.874
controller.inputs.parameters.ankle_impedance.early_stance.damping = 0
controller.inputs.parameters.ankle_impedance.early_stance.eq_angle = -2
controller.inputs.parameters.ankle_impedance.late_stance.stiffness = 79.498
controller.inputs.parameters.ankle_impedance.late_stance.damping = 0.063
controller.inputs.parameters.ankle_impedance.late_stance.eq_angle = -20
controller.inputs.parameters.ankle_impedance.early_swing.stiffness = 7.949
controller.inputs.parameters.ankle_impedance.early_swing.damping = 0
controller.inputs.parameters.ankle_impedance.early_swing.eq_angle = 25
controller.inputs.parameters.ankle_impedance.late_swing.stiffness = 7.949
controller.inputs.parameters.ankle_impedance.late_swing.damping = 0.0
controller.inputs.parameters.ankle_impedance.late_swing.eq_angle = 15
# Configure state machine
body_weight = 82 # kg
controller.inputs.parameters.body_weight = body_weight
controller.inputs.parameters.transition_parameters.min_time_in_state = 0.20
controller.inputs.parameters.transition_parameters.loadLStance = -body_weight * 0.25
controller.inputs.parameters.transition_parameters.ankleThetaEstanceToLstance = 6.0
controller.inputs.parameters.transition_parameters.loadESwing = -body_weight * 0.15
controller.inputs.parameters.transition_parameters.kneeThetaESwingToLSwing = 50
controller.inputs.parameters.transition_parameters.kneeDthetaESwingToLSwing = 3
controller.inputs.parameters.transition_parameters.loadEStance = -body_weight * 0.4
controller.inputs.parameters.transition_parameters.kneeThetaLSwingToEStance = 30
with knee, ankle, loadcell:
knee.home()
ankle.home()
knee.set_control_mode(knee.CONTROL_MODES.IMPEDANCE)
ankle.set_control_mode(ankle.CONTROL_MODES.IMPEDANCE)
# Main Loop
for t in clock:
knee.update()
ankle.update()
loadcell.update()
controller.inputs.sensors.knee_angle = units.convert_from_default(knee.output_position, units.position.deg)
controller.inputs.sensors.ankle_angle = units.convert_from_default(ankle.output_position, units.position.deg)
controller.inputs.sensors.knee_velocity = units.convert_from_default(
knee.output_velocity, units.velocity.deg_per_s
)
controller.inputs.sensors.ankle_velocity = units.convert_from_default(
ankle.output_velocity, units.velocity.deg_per_s
)
controller.inputs.sensors.Fz = loadcell.fz
# Update any control inputs that change every loop
controller.inputs.time = t
# Call the controller
outputs = controller.run()
# Test print to ensure external library call works
print(
f"Current time in state {outputs.current_state}: {outputs.time_in_current_state:.2f} seconds, \
Knee Eq {outputs.knee_impedance.eq_angle:.2f}, \
Ankle Eq {outputs.ankle_impedance.eq_angle:.2f}, \
Fz {loadcell.fz:.2f}",
end="\r",
)
# Write to the hardware
knee.set_output_impedance(
k=units.convert_to_default(outputs.knee_impedance.stiffness, units.stiffness.N_m_per_rad),
b=units.convert_to_default(outputs.knee_impedance.damping, units.damping.N_m_per_rad_per_s),
)
knee.set_output_position(value=units.convert_to_default(outputs.knee_impedance.eq_angle, units.position.deg))
ankle.set_output_impedance(
k=units.convert_to_default(outputs.ankle_impedance.stiffness, units.stiffness.N_m_per_rad),
b=units.convert_to_default(outputs.ankle_impedance.damping, units.damping.N_m_per_rad_per_s),
)
ankle.set_output_position(value=units.convert_to_default(outputs.ankle_impedance.eq_angle, units.position.deg))
print("\n")