Skip to content

Torque Trajectory Control Tutorial

This opensourceleg.control module provides functiionality for doing torque control. This tutorial demonstrates how to control the Open Source Leg (OSL) using torque trajectories for both the knee and ankle joints.

Warnings:

  1. This example is not meant to be used as a walking controller. The goal of this example is to provide a reference for how a torque trajectory can be loaded and commanded.
  2. While runnig this script make sure to have load on the actuators.
  3. Please be cautious while changing mass parameters.

Overview

The script implements a torque control system that:

  1. Loads pre-defined torque trajectories for knee and ankle joints
  2. Applies these trajectories in a cyclic manner
  3. Logs the performance data
  4. Generates visualization plots

Prerequisites

  • OpenSourceLeg hardware setup

  • Python environment with required dependencies:

    • numpy
    • matplotlib

  • Access to torque trajectory files:

    • ankle.pkl
    • knee.pkl

Command Line Arguments

The script accepts the following command line arguments:

  • --mass: User mass in kg (default: 1.0)
  • --stride-time: Stride time in seconds (default: 1.0)
  • --frequency: Control loop frequency in Hz (default: 200.0)

Key Parameters

  • USER_MASS: The mass of the user in kg (configurable via command line)
  • STRIDE_TIME: The stride time in seconds (configurable via command line)
  • FREQUENCY: The control loop frequency in Hz (configurable via command line)
  • TRAJECTORY_LEN: Fixed at 150 points. This is the length of the torque trajectories in the ankle.pkl and knee.pkl files.
  • GEAR_RATIO: Set to 9 * (83/18)

Hardware Configuration

The script configures two DephyActuators and two AS5048B encoders:

  1. Actuators:

    • Knee actuator (port=/dev/ttyACM0)
    • Ankle actuator (port=/dev/ttyACM1)

Both configured with:

  - Specified gear ratio
  - User-defined frequency
  - Dephy logging disabled

  1. Encoders:

    • Knee joint encoder (bus=1, A1=True, A2=False)
    • Ankle joint encoder (bus=1, A1=False, A2=True)

Functions

Get Torque

from opensourceleg.utilities import SoftRealtimeLoop

ANKLE_TRAJECTORY_PATH = "./ankle.pkl"
KNEE_TRAJECTORY_PATH = "./knee.pkl"


def get_torque(t: float, data: list, user_mass: float, stride_time: float, trajectory_len: int) -> int:
    """Calculate the torque setpoint for a given time point in the trajectory.

    Args:
        t (float): Current time in seconds
        data (list): List containing the torque trajectory data points
        user_mass (float): The mass of the user in kg
        stride_time (float): The stride time in seconds
        trajectory_len (int): The length of the trajectory

    Returns:
        int: Torque setpoint scaled by user mass

    The function:
    1. Calculates walking_time by taking modulo of current time with stride_time

Calculates the torque setpoint for a given time point:

  • t: Current time in seconds
  • data: List containing torque trajectory data points
  • user_mass: Mass of the user in kg
  • stride_time: Stride time in seconds
  • trajectory_len: Length of the trajectory

Returns torque setpoint scaled by user mass

Plot Data

    """
    walking_time = t % stride_time
    index = int(walking_time * trajectory_len)
    return user_mass * data[index]


def plot_data(plotting_data: list) -> None:
    try:
        import matplotlib.pyplot as plt
    except ImportError:
        print("matplotlib is not installed, skipping plot")
        return

    # plot the data using matplotlib
    plt.figure(figsize=(12, 10))

    # Create three subplots for better visualization
    # Ankle Torque
    plt.subplot(3, 1, 1)
    plt.plot(plotting_data[0], plotting_data[1], label="Ankle Torque Set Point")
    plt.plot(plotting_data[0], plotting_data[3], label="Ankle Joint Torque")
    plt.xlabel("Time (s)")
    plt.ylabel("Torque (Nm)")
    plt.title("Ankle Torque Trajectory")
    plt.legend()
    plt.grid(True)

    # Knee Torque
    plt.subplot(3, 1, 2)
    plt.plot(plotting_data[0], plotting_data[2], label="Knee Torque Set Point")
    plt.plot(plotting_data[0], plotting_data[4], label="Knee Joint Torque")
    plt.xlabel("Time (s)")
    plt.ylabel("Torque (Nm)")
    plt.title("Knee Torque Trajectory")
    plt.legend()
    plt.grid(True)

    # Joint Positions
    plt.subplot(3, 1, 3)
    plt.plot(plotting_data[0], np.rad2deg(plotting_data[5]), label="Knee Position")
    plt.plot(plotting_data[0], np.rad2deg(plotting_data[6]), label="Ankle Position")
    plt.xlabel("Time (s)")
    plt.ylabel("Joint Position (deg)")
    plt.title("Joint Positions")

Generates three plots:

  1. Ankle torque (setpoint vs. actual)
  2. Knee torque (setpoint vs. actual)
  3. Joint positions (knee and ankle in degrees)

Saves the plot as plot.png

Operation Flow

  1. Initialization:

    • Parses command line arguments
    • Sets up logging with specified frequency
    • Configures actuators and sensors
    • Loads trajectory data from pickle files

  2. Control Sequence:

    • Homes the OSL
    • Sets control mode to CURRENT for both actuators
    • Sets current gains
    • Waits for user input
    • Executes torque trajectory
    • Logs performance data

  3. Visualization:

    • Generates plots after completion
    • Saves plots to "plot.png"

Usage

  1. Ensure trajectory files (ankle.pkl and knee.pkl) are in the working directory. We have provided a sample trajectory file in the tutorials/control/torque_trajectory/ directory for both the ankle and knee joints. These trajectories are output of an high-level controller (not provided here) for level walking. The torque trajectories present the joint torque setpoints for one gait cycle, and the units are in Nm/kg.

  2. Run the script with optional arguments: 

    python torque_trajectory.py --mass 10 --stride-time 1 --frequency 200

  3. Press Enter when prompted to start the walking trajectory
  4. The system will execute the trajectory and generate plots upon completion

Safety Notes

  • This example is not meant to be used as a walking controller
  • Please be cautious while changing mass parameters
  • Ensure all connections are secure before operation
  • Keep emergency stop accessible during operation

Output

The script generates:

  1. Real-time logs in the ./logs directory with filename "torque_trajectory"

  2. A plot file (plot.png) showing:

    • Torque trajectories for both joints
    • Actual vs. commanded torques
    • Joint position data in degrees

Full Script for this tutorial

"""
This example demonstrates how torque trajectories can be loaded and commanded on the Open Source Leg (OSL).
This is not a walking controller.

Authors: Senthur Raj Ayyappan <senthura@umich.edu>, Varun Satyadev Shetty <vashetty@umich.edu>
Neurobionics Lab
Robotics Department
University of Michigan
March 25th, 2025
"""

import argparse
import pickle

import numpy as np

from opensourceleg.actuators import CONTROL_MODES
from opensourceleg.actuators.dephy import DephyActuator
from opensourceleg.logging import Logger
from opensourceleg.robots.osl import OpenSourceLeg
from opensourceleg.sensors.encoder import AS5048B
from opensourceleg.utilities import SoftRealtimeLoop

ANKLE_TRAJECTORY_PATH = "./ankle.pkl"
KNEE_TRAJECTORY_PATH = "./knee.pkl"


def get_torque(t: float, data: list, user_mass: float, stride_time: float, trajectory_len: int) -> int:
    """Calculate the torque setpoint for a given time point in the trajectory.

    Args:
        t (float): Current time in seconds
        data (list): List containing the torque trajectory data points
        user_mass (float): The mass of the user in kg
        stride_time (float): The stride time in seconds
        trajectory_len (int): The length of the trajectory

    Returns:
        int: Torque setpoint scaled by user mass

    The function:
    1. Calculates walking_time by taking modulo of current time with stride_time
    2. Converts walking_time to trajectory index based on trajectory_len
    3. Returns torque value at that index scaled by user_mass
    """
    walking_time = t % stride_time
    index = int(walking_time * trajectory_len)
    return user_mass * data[index]


def plot_data(plotting_data: list) -> None:
    try:
        import matplotlib.pyplot as plt
    except ImportError:
        print("matplotlib is not installed, skipping plot")
        return

    # plot the data using matplotlib
    plt.figure(figsize=(12, 10))

    # Create three subplots for better visualization
    # Ankle Torque
    plt.subplot(3, 1, 1)
    plt.plot(plotting_data[0], plotting_data[1], label="Ankle Torque Set Point")
    plt.plot(plotting_data[0], plotting_data[3], label="Ankle Joint Torque")
    plt.xlabel("Time (s)")
    plt.ylabel("Torque (Nm)")
    plt.title("Ankle Torque Trajectory")
    plt.legend()
    plt.grid(True)

    # Knee Torque
    plt.subplot(3, 1, 2)
    plt.plot(plotting_data[0], plotting_data[2], label="Knee Torque Set Point")
    plt.plot(plotting_data[0], plotting_data[4], label="Knee Joint Torque")
    plt.xlabel("Time (s)")
    plt.ylabel("Torque (Nm)")
    plt.title("Knee Torque Trajectory")
    plt.legend()
    plt.grid(True)

    # Joint Positions
    plt.subplot(3, 1, 3)
    plt.plot(plotting_data[0], np.rad2deg(plotting_data[5]), label="Knee Position")
    plt.plot(plotting_data[0], np.rad2deg(plotting_data[6]), label="Ankle Position")
    plt.xlabel("Time (s)")
    plt.ylabel("Joint Position (deg)")
    plt.title("Joint Positions")
    plt.legend()
    plt.grid(True)

    plt.tight_layout()
    plt.savefig("plot.png")
    plt.show()


if __name__ == "__main__":
    # Set up argument parser
    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
    )
    parser.add_argument(
        "--frequency", type=float, default=200.0, help="Control loop frequency in Hz (default: 200.0)", required=False
    )

    # Parse arguments
    args = parser.parse_args()

    # Set global parameters from arguments
    USER_MASS = args.mass
    STRIDE_TIME = args.stride_time
    FREQUENCY = args.frequency

    # Fixed parameters
    TRAJECTORY_LEN = 150
    GEAR_RATIO = 9 * (83 / 18)

    torque_logger = Logger(log_path="./logs", file_name="torque_trajectory")
    clock = SoftRealtimeLoop(dt=1 / FREQUENCY)

    actuators = {
        "knee": DephyActuator(
            tag="knee",
            port="/dev/ttyACM0",
            gear_ratio=GEAR_RATIO,
            frequency=FREQUENCY,
            dephy_log=False,
        ),
        "ankle": DephyActuator(
            tag="ankle",
            port="/dev/ttyACM1",
            gear_ratio=GEAR_RATIO,
            frequency=FREQUENCY,
            dephy_log=False,
        ),
    }

    sensors = {
        "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,
        ),
    }

    osl = OpenSourceLeg(
        tag="osl",
        actuators=actuators,
        sensors=sensors,
    )

    plotting_data = [
        [],
        [],
        [],
        [],
        [],
        [],
        [],
    ]  # Time, ankle_sp, knee_sp, ankle_torque, knee_torque, knee_pos, ankle_pos

    with open(ANKLE_TRAJECTORY_PATH, "rb") as f:
        ankle_data = pickle.load(f)  # noqa: S301

    with open(KNEE_TRAJECTORY_PATH, "rb") as f:
        knee_data = pickle.load(f)  # noqa: S301

    with osl:
        osl.home()

        osl.knee.set_control_mode(CONTROL_MODES.CURRENT)
        osl.knee.set_current_gains()
        osl.knee.set_output_torque(0)

        osl.ankle.set_control_mode(CONTROL_MODES.CURRENT)
        osl.ankle.set_current_gains()
        osl.ankle.set_output_torque(0)

        input("Press Enter to start walking")

        for t in clock:
            osl.update()

            ankle_torque_sp = get_torque(t, ankle_data, USER_MASS, STRIDE_TIME, TRAJECTORY_LEN)
            knee_torque_sp = get_torque(t, knee_data, USER_MASS, STRIDE_TIME, TRAJECTORY_LEN)

            osl.ankle.set_output_torque(ankle_torque_sp)
            osl.knee.set_output_torque(knee_torque_sp)

            torque_logger.info(
                f"t: {t}, ankle_torque_sp: {ankle_torque_sp}, knee_torque_sp: {knee_torque_sp}, "
                f"ankle_output_torque: {osl.ankle.output_torque}, "
                f"knee_output_torque: {osl.knee.output_torque} "
                f"knee_position: {osl.knee.output_position}, ankle_position: {osl.ankle.output_position}"
            )
            plotting_data[0].append(t)
            plotting_data[1].append(ankle_torque_sp)
            plotting_data[2].append(knee_torque_sp)
            plotting_data[3].append(osl.ankle.output_torque)
If you have any questions or need further assistance, please post on the Open Source Leg community forum.