Skip to content

FSM tested #528

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
VarunSatyadevShetty merged 1 commit into from
Jul 3, 2026
Merged

FSM tested #528

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 36 additions & 11 deletions examples/fsm_walking_ankle_python_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,18 +22,17 @@
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),
LOADCELL_MATRIX = np.array([
(-9.817417, -1895.974287, 14.899174, -4.333117, -40.684438, 1900.65478),
(-32.435533, 1129.587599, -5.693634, -2206.916117, 14.346322, 1086.297672),
(-963.721193, -4.911412, -965.891326, -1.404768, -981.000498, 8.693936),
(19.854122, -0.21849, 0.25324, 0.734436, -20.471398, -0.232788),
(-11.530613, -0.964467, 22.849398, -0.011803, -11.785854, 1.015088),
(-0.296284, -26.299567, -0.138142, -26.753995, 0.385682, -25.052914),
])

# ------------- TUNABLE FSM PARAMETERS ---------------- #
BODY_WEIGHT = 10 * 9.8
BODY_WEIGHT = 20 * 9.8

# STATE 1: EARLY STANCE
ANKLE_K_ESTANCE = 19.874
Expand Down Expand Up @@ -189,18 +188,27 @@ def create_simple_walking_fsm(osl: OpenSourceLeg) -> StateMachine:
gear_ratio=GEAR_RATIO,
frequency=FREQUENCY,
debug_level=0,
torque_constant=0.145,
dephy_log=False,
),
}

sensors = {
"loadcell": NBLoadcellDAQ(
LOADCELL_CALIBRATION_MATRIX_M3554E,
LOADCELL_MATRIX,
tag="loadcell",
excitation_voltage=5.0,
amp_gain=[34] * 3 + [151] * 3,
spi_bus=1,
),
# "joint_encoder_ankle": AS5048B(
# tag="joint_encoder_ankle",
# bus="/dev/i2c-2",
# A1_adr_pin=False,
# A2_adr_pin=False,
# zero_position=0,
# enable_diagnostics=False,
# ),
}

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

osl_fsm = create_simple_walking_fsm(osl)

# Zeroing the joint encoders
def knee_homing_complete():
osl.joint_encoder_knee.update()
osl.joint_encoder_knee.zero_position = osl.joint_encoder_knee.counts
print("Knee homing complete!")

def ankle_homing_complete():
osl.joint_encoder_ankle.update()
# The hard stop for ankle is at 30 deg from the zero position
osl.joint_encoder_ankle.zero_position = osl.joint_encoder_ankle.counts - osl.joint_encoder_ankle.deg_to_counts(
30
)
print("Ankle homing complete!")

callbacks = {"knee": knee_homing_complete, "ankle": ankle_homing_complete}

with fsm_logger, osl, osl_fsm:
osl.update()
osl.home()
Expand All @@ -225,7 +249,8 @@ def create_simple_walking_fsm(osl: OpenSourceLeg) -> StateMachine:

# ankle
osl.ankle.set_control_mode(mode=CONTROL_MODES.IMPEDANCE)
osl.ankle.set_impedance_gains()
osl.ankle.set_impedance_cc_pidf_gains()
osl.ankle.set_output_impedance()

for t in clock:
osl.update()
Expand Down
62 changes: 30 additions & 32 deletions examples/fsm_walking_python_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
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 NBLoadcellDAQ
from opensourceleg.utilities import SoftRealtimeLoop

Expand All @@ -21,7 +20,7 @@
])

# ------------- TUNABLE FSM PARAMETERS ---------------- #
BODY_WEIGHT = 10 * 9.8 # 30 * 9.8
BODY_WEIGHT = 30 * 9.8

# STATE 1: EARLY STANCE
KNEE_K_ESTANCE = 99.372
Expand Down Expand Up @@ -217,22 +216,22 @@ def lswing_to_estance(osl: OpenSourceLeg) -> bool:
amp_gain=[34] * 3 + [151] * 3,
spi_bus=1,
),
"joint_encoder_knee": AS5048B(
tag="joint_encoder_knee",
bus="/dev/i2c-2",
A1_adr_pin=False,
A2_adr_pin=False,
zero_position=0,
enable_diagnostics=False,
),
"joint_encoder_ankle": AS5048B(
tag="joint_encoder_ankle",
bus="/dev/i2c-3",
A1_adr_pin=False,
A2_adr_pin=False,
zero_position=0,
enable_diagnostics=False,
),
# "joint_encoder_knee": AS5048B(
# tag="joint_encoder_knee",
# bus="/dev/i2c-2",
# A1_adr_pin=False,
# A2_adr_pin=False,
# zero_position=0,
# enable_diagnostics=False,
# ),
# "joint_encoder_ankle": AS5048B(
# tag="joint_encoder_ankle",
# bus="/dev/i2c-3",
# A1_adr_pin=False,
# A2_adr_pin=False,
# zero_position=0,
# enable_diagnostics=False,
# ),
}

clock = SoftRealtimeLoop(dt=1 / FREQUENCY)
Expand Down Expand Up @@ -267,7 +266,7 @@ def ankle_homing_complete():

with osl, osl_fsm:
osl.update()
osl.home(callbacks=callbacks)
osl.home()
input("Press Enter to start walking...")

# knee
Expand All @@ -280,25 +279,24 @@ def ankle_homing_complete():
osl.ankle.set_impedance_cc_pidf_gains()
osl.ankle.set_output_impedance()

# The FSM is expecting the loadcell values to be positive
osl.loadcell.reset()
osl.loadcell.calibrate()

for t in clock:
osl.update()
print("Ankle position", np.rad2deg(osl.sensors["joint_encoder_ankle"].position))
print("Knee position", np.rad2deg(osl.sensors["joint_encoder_knee"].position))
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))
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, "
Expand Down
Loading