Skip to content

feat(dephy): Added torque_constant parameter to DephyActuator. This allows the users to override the motor torque constant (NM_PER_AMP) when constructing DephyActuator in the control script #523

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

Open
VarunSatyadevShetty wants to merge 5 commits into
base: main
Choose a base branch
from
Open

feat(dephy): Added torque_constant parameter to DephyActuator. This allows the users to override the motor torque constant (NM_PER_AMP) when constructing DephyActuator in the control script #523

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
aacac41
feat(dephy): added torque_constant parameter to DephyActuator and Dep…
VarunSatyadevShetty May 5, 2026
111d24a
Merge branch 'main' into actuator_torque_constant_feature
VarunSatyadevShetty May 5, 2026
be6a8fc
feat(dephy): added torque_constant parameter to DephyActuator and Dep…
VarunSatyadevShetty May 5, 2026
7eb3011
FSM tested
VarunSatyadevShetty Jul 3, 2026
16cb7dd
Merge pull request #528 from neurobionics/FSM-TESTING
VarunSatyadevShetty Jul 3, 2026
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
28 changes: 25 additions & 3 deletions opensourceleg/actuators/dephy.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@
IMPEDANCE_A = 0.00028444
IMPEDANCE_C = 0.0007812

DEFAULT_MIN_TORQUE_CONSTANT = 0.08
DEFAULT_MAX_TORQUE_CONSTANT = 0.25

DEPHY_ACTUATOR_CONSTANTS = MOTOR_CONSTANTS(
MOTOR_COUNT_PER_REV=16384,
Expand Down Expand Up @@ -138,21 +140,31 @@ def __init__(
baud_rate: int = 230400,
frequency: int = 500,
debug_level: int = 4,
torque_constant: float = 0.1133, # new motor constant 0.145 Nm/A
dephy_log: bool = False,
offline: bool = False,
stop_motor_on_disconnect: bool = True,
) -> None:
if torque_constant < DEFAULT_MIN_TORQUE_CONSTANT or torque_constant > DEFAULT_MAX_TORQUE_CONSTANT:
LOGGER.error(
f"[{str.upper(tag)}] torque_constant={torque_constant} Nm/A is outside the valid range. "
"Low torque constant will cause the motor to draw more current for the same torque. "
"Verify your value: typical Dephy torque constants are 0.1133 Nm/A (older motors)"
" and 0.145 Nm/A (newer motors)."
)
raise ValueError("Torque constant is outside the valid range")
motor_constants = dataclasses.replace(DEPHY_ACTUATOR_CONSTANTS, NM_PER_AMP=torque_constant)
ActuatorBase.__init__(
self,
tag=tag,
gear_ratio=gear_ratio,
motor_constants=DEPHY_ACTUATOR_CONSTANTS,
motor_constants=motor_constants,
frequency=frequency,
offline=offline,
)

# Override motor constants to ensure setter is called
self.MOTOR_CONSTANTS = DEPHY_ACTUATOR_CONSTANTS
self.MOTOR_CONSTANTS = motor_constants

self._debug_level: int = debug_level if dephy_log else 6
self._dephy_log: bool = dephy_log
Expand Down Expand Up @@ -1224,14 +1236,24 @@ def __init__(
baud_rate: int = 230400,
frequency: int = 500,
debug_level: int = 4,
torque_constant: float = 0.1133, # new motor constant 0.145 Nm/A
dephy_log: bool = False,
offline: bool = False,
) -> None:
if torque_constant < DEFAULT_MIN_TORQUE_CONSTANT or torque_constant > DEFAULT_MAX_TORQUE_CONSTANT:
LOGGER.error(
f"[{str.upper(tag)}] torque_constant={torque_constant} Nm/A is outside the valid range. "
"Low torque constant will cause the motor to draw more current for the same torque. "
"Verify your value: typical Dephy torque constants are 0.1133 Nm/A (older motors)"
" and 0.145 Nm/A (newer motors)."
)
raise ValueError("Torque constant is outside the valid range")
motor_constants = dataclasses.replace(DEPHY_ACTUATOR_CONSTANTS, NM_PER_AMP=torque_constant)
ActuatorBase.__init__(
self,
tag=tag,
gear_ratio=gear_ratio,
motor_constants=DEPHY_ACTUATOR_CONSTANTS,
motor_constants=motor_constants,
frequency=frequency,
offline=offline,
)
Expand Down
Binary file added tutorials/control/torque_trajectory/plot.png
View file
Open in desktop
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
12 changes: 6 additions & 6 deletions tutorials/control/torque_trajectory/torque_trajectory.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,10 +99,10 @@ def plot_data(plotting_data: list) -> None:
parser = argparse.ArgumentParser(description="Torque trajectory control for Open Source Leg")
parser.add_argument("--mass", type=float, default=1.0, help="User mass in kg (default: 1.0)", required=False)
parser.add_argument(
"--stride-time", type=float, default=1, help="Stride time in seconds (default: 1)", required=False
"--stride-time", type=float, default=1.0, help="Stride time in seconds (default: 1)", required=False
)
parser.add_argument(
"--frequency", type=float, default=200.0, help="Control loop frequency in Hz (default: 200.0)", required=False
"--frequency", type=int, default=200, help="Control loop frequency in Hz (default: 200.0)", required=False
)

# Parse arguments
Expand Down Expand Up @@ -140,17 +140,17 @@ def plot_data(plotting_data: list) -> None:
sensors = {
"joint_encoder_knee": AS5048B(
tag="joint_encoder_knee",
bus=1,
A1_adr_pin=True,
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=1,
bus="/dev/i2c-3",
A1_adr_pin=False,
A2_adr_pin=True,
A2_adr_pin=False,
zero_position=0,
enable_diagnostics=False,
),
Expand Down
Loading