Chapter 5: Upper-Limb Joint Override Control

1 Interface Overview

The upper-limb joint override control interface is used to control the position, velocity, and torque of the robot's upper-limb joints in real time. Different from the "send one request, wait for completion" pattern of the motion planning interface (Chapter 4), override control requires the developer to continuously publish control commands at a fixed frequency, and supports smooth trajectory generation with quintic polynomial interpolation.

This interface is suitable for scenarios that require real-time control of upper-limb motions during walking, such as waving, pointing, and reciprocating motions. Override control runs in parallel with the lower-limb gait under the lower_body_balance mode.

2 Message Fields

Communication interface: Upper-limb joint override control is accomplished through the following ROS2 topics.

• Override command topic: /joint_override_command, message type: interface_protocol/msg/JointOverrideCommand
• Joint state feedback: /joint_states, message type: sensor_msgs/msg/JointState (ROS2 standard message)

The detailed JointOverrideCommand message field definitions are given below.

JointOverrideCommand.msg field definition (from GitHub):

std_msgs/Header header
float64 weight
int32[] joint_indices
float64[] position
float64[] velocity
float64[] feed_forward_torque
float64[] torque
float64[] stiffness
float64[] damping

Communication Interface:

3 Prerequisites

• Must enter the lower_body_balance (lower-body balance) mode
• Gamepad switch: [LB, CROSS_X_DOWN]
• The recommended publish frequency is 100 Hz to ensure control smoothness
• The indices in joint_indices must be consistent with the robot configuration
• The stiffness and damping parameters need to be adjusted according to the specific joint
• The position unit is radian; pay attention to angle conversion

4 Example: Reciprocating Motion Control

File Name: upper_joint_override_example.py

This example demonstrates how to use the JointOverrideCommand message to control specific joints for reciprocating motion, with a simple quintic polynomial interpolation algorithm built in.

Run Command:

python3 src/interface_example/scripts/upper_joint_override_example.py

Complete import block (must be imported before the core code below):

import sys
from typing import List, Tuple

import numpy as np
import rclpy
from rclpy.node import Node
from std_msgs.msg import Header

from interface_protocol.msg import (
    JointOverrideCommand,  # type: ignore
    MotionState,
)

Note: The three classes QuinticSplineInterpolator, JointTrajectoryPlanner, and JointOverridePublisher referenced by the core code below are defined in the GitHub script upper_joint_override_example.py and can be reused directly.

Core Code:

# Set control frequency
timer_freq = 100.0  # Hz

# Define start and target positions
start_pos = np.array([0.0, 0.0])    # radian
end_pos = np.array([1.57, -1.57])   # radian

# Specify the joint indices to control
joint_indices = [14, 19]

# Set control parameters
stiffness = np.array([20.0, 20.0])
damping = np.array([1.0, 1.0])

# Generate trajectory (duration 5 seconds)
planner = JointTrajectoryPlanner(frequency=timer_freq)
trajectory = planner.plan(start_pos, end_pos, duration=5.0)

# Create publisher node and start reciprocating motion
node = JointOverridePublisher(
    freq=timer_freq,
    trajectory=trajectory,
    joint_indices=joint_indices,
    velocity=np.array([0.0, 0.0]),
    feed_forward_torque=np.array([0.0, 0.0]),
    torque=np.array([0.0, 0.0]),
    stiffness=stiffness,
    damping=damping,
)

rclpy.spin(node)

The reciprocating motion automatically reverses at the trajectory end point, forming a continuous back-and-forth motion.

5 Key Parameters Description

Key Message Fields Parameters (JointOverrideCommand.msg):

Example Script Parameters (JointTrajectoryPlanner / publisher node):

6 Notes

• Ensure the target position is within the joint's motion range
• The stiffness and damping parameters need to be tuned according to the actual robot
• The reciprocating motion automatically reverses at the trajectory end point
• Use Ctrl+C to safely exit the program

📎 Source files in the open-source repository for this chapter (click file name to jump to GitHub): JointMotionPlanState.msg · JointOverrideCommand.msg · upper_joint_override_example.py