STS Hardware Interface

ros2_control SystemInterface for Feetech STS series servo motors (STS3215 and compatible).

⚠️ Status: Modes 0 (Position) and 1 (Velocity) have been tested and validated. Mode 2 (PWM) is implemented but currently untested.

Features

• Scalable Multi-Motor Support: Control 1 to 253 motors on a single serial bus
• Three Operating Modes: Position (servo, closed-loop), Velocity (wheel, closed-loop), and PWM (effort, open-loop) control per motor
• Mixed-Mode Operation: Different motors in different modes on the same serial bus
• REP-103 Compliance: Automatic direction inversion for counter-clockwise positive rotation convention
• Multi-Motor Coordination: Efficient SyncWrite for chains of motors
• Safety Features: Broadcast emergency stop, hardware limits, automatic error recovery
• Full State Feedback: Position, velocity, load, voltage, temperature, current, motion status
• Mock Mode: Hardware-free simulation for development and testing
• Motor Diagnostics Node: Real-time health/stall/voltage/current/temperature monitoring for STS motors

Command Interfaces

Mode 0 (Position/Servo):

• position - Target position (radians)
• velocity (optional) - Maximum speed for the position move (rad/s). Behaviour depends on proportional_vel_max and use_sync_write:
  • use_sync_write=true and proportional_vel_max != 0: proportional velocity always wins; this command interface is ignored.
  • use_sync_write=true and proportional_vel_max == 0: per-joint commanded max speed is used (0 = hardware max speed).
  • use_sync_write=false: per-joint commanded max speed is always used directly, proportional_vel_max has no effect.
• acceleration (optional) - Acceleration (0-254, unitless protocol value). When omitted: ACC 0 (hardware default).

Mode 1 (Velocity):

• velocity - Target velocity (rad/s)
• acceleration (optional) - Acceleration (0-254, unitless protocol value). Behaviour depends on proportional_acc_max and use_sync_write:
  • use_sync_write=true and proportional_acc_max != 0 (default): proportional acceleration always wins; this command interface is ignored.
  • use_sync_write=true and proportional_acc_max == 0: per-joint commanded acceleration is used.
  • use_sync_write=false: per-joint commanded acceleration is always used directly, proportional_acc_max has no effect.

Mode 2 (PWM/Effort):

• effort - PWM duty cycle (unitless, -1.0 to +1.0 representing ±100% duty cycle)

State Interfaces

The hardware always exports all 7 state interfaces for every joint (regardless of operating mode):

• position - Current position (radians)
• velocity - Current velocity (rad/s)
• effort - Motor load percentage (-1.0 to +1.0, normalized)
• voltage - Supply voltage (volts)
• temperature - Internal temperature (°C)
• current - Motor current draw (amperes)
• is_moving - Motion status (1.0 = moving, 0.0 = stopped)

Note: Unlike command interfaces (which must match the operating mode), all state interfaces are always exported regardless of URDF configuration. URDF state interface declarations are optional but recommended for documentation.

Emergency Stop

Emergency stop functionality is available via ROS 2 service (std_srvs/SetBool):

# Activate emergency stop (stops ALL motors, disables torque)
ros2 service call /emergency_stop std_srvs/srv/SetBool "{data: true}"

# Release emergency stop
ros2 service call /emergency_stop std_srvs/srv/SetBool "{data: false}"

When activated, ALL motors stop immediately and torque is disabled. The service returns success: true and a confirmation message on both activate and release.

Motor Diagnostics Node

This package provides a real-time diagnostics node for STS motors, monitoring temperature, voltage, current, and detecting internal stalls using feedback from the hardware interface.

Features:

• Publishes standard ROS 2 diagnostics to /diagnostics
• Detects over-temperature, over-current, under/over-voltage, and stall conditions
• Configurable thresholds via YAML
• Can be run standalone or alongside any STS hardware interface setup

Usage:

To launch the diagnostics node with the default config:

ros2 launch sts_hardware_interface motor_diagnostics.launch.py

To use a custom config file:

ros2 launch sts_hardware_interface motor_diagnostics.launch.py config_file:=/path/to/your_config.yaml

Configuration Example:

See config/motor_diagnostics_config.yaml:

motor_diagnostics_node:
  ros__parameters:
    effort_threshold: 0.5          # N·m - effort threshold for stall detection
    current_threshold: 1.5         # A - current threshold for stall detection
    voltage_min: 6.0               # V - minimum acceptable supply voltage
    voltage_max: 14.0              # V - maximum acceptable supply voltage
    temp_warn: 60.0                # °C - temperature warning threshold
    temp_error: 75.0               # °C - temperature error threshold
    current_max: 3.0               # A - maximum acceptable motor current

Node Details:

• Node name: motor_diagnostics_node
• Subscribed topics: /dynamic_joint_states (from hardware interface)
• Published topics: /diagnostics (diagnostic_msgs/DiagnosticArray)
• Parameters: See config YAML above

This node is recommended for all robots using STS motors to ensure safe operation and early detection of hardware issues.

Quick Start

cd ~/ros2_ws/src
git clone https://github.com/adityakamath/sts_hardware_interface.git
cd sts_hardware_interface
git submodule update --init --recursive
cd ~/ros2_ws
colcon build --packages-select sts_hardware_interface

See the Quick Start guide for detailed instructions on running the example launch files and configuring your hardware.

Configuration Example

<ros2_control name="sts_system" type="system">
  <hardware>
    <plugin>sts_hardware_interface/STSHardwareInterface</plugin>
    <param name="serial_port">/dev/ttyACM0</param>
    <param name="baud_rate">1000000</param>
    <param name="use_sync_write">true</param>
  </hardware>

  <joint name="wheel_joint">
    <param name="motor_id">1</param>
    <param name="operating_mode">1</param>

    <command_interface name="velocity"/>
    <!-- acceleration is optional for velocity mode.
         With use_sync_write=true and proportional_acc_max != 0 (default): proportional acceleration always wins; this interface is ignored.
         With use_sync_write=true and proportional_acc_max == 0: per-joint commanded acceleration is used.
         With use_sync_write=false: per-joint commanded acceleration is always used. -->
    <!-- <command_interface name="acceleration"/> -->

    <!-- State interfaces (optional declarations for documentation) -->
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <state_interface name="effort"/>
    <state_interface name="temperature"/>
    <!-- Optional: All 7 state interfaces can be enabled if needed -->
  </joint>
</ros2_control>

Documentation

• Quick Start guide - Detailed setup and usage instructions
• Design documentation - Implementation details and design decisions

Hardware Parameters

Parameter	Type	Default	Description
serial_port	string	required	Serial port path (e.g., /dev/ttyACM0)
baud_rate	int	1000000	Baud rate: 9600-1000000
communication_timeout_ms	int	100	Serial timeout: 1-1000 ms
use_sync_write	bool	true	Enable SyncWrite for multi-motor setups
enable_mock_mode	bool	false	Simulation mode (no hardware)
max_velocity_steps	int	3400	Max motor velocity in steps/s (STS3215: 3400, STS3032: 2900)
proportional_vel_max	int	0	SyncWrite only. Velocity value [0–max_velocity_steps] assigned to the joint with the largest |target_position - current_position| delta. All others are scaled proportionally so every joint arrives at its target at the same time. Set to 0 to disable (falls back to per-joint commanded velocity, or raw 0 = hardware max speed if the interface is not declared). Has no effect when use_sync_write=false.
proportional_vel_deadband	double	0.01	SyncWrite only. Minimum max-delta (rad) below which all joints revert to their commanded velocity (steady-state hold, avoids noise-driven re-scaling). Has no effect when use_sync_write=false or proportional_vel_max=0.
proportional_acc_max	int	100	SyncWrite only. Acceleration value [0–254] assigned to the wheel with the largest |target_velocity - current_velocity| delta. All others are scaled proportionally so every wheel finishes ramping at the same time. Set to 0 to disable (falls back to per-joint commanded acceleration, or 0 if the interface is not declared). Has no effect when use_sync_write=false.
proportional_acc_deadband	double	0.05	SyncWrite only. Minimum max-delta (rad/s) below which acceleration 0 is sent to all wheels (steady-state cruise, avoids noise-driven jitter). Has no effect when use_sync_write=false or proportional_acc_max=0.
reset_states_on_activate	bool	true	Reset position/velocity states to zero on activation for clean odometry

Joint Parameters

Parameter	Type	Default	Description
motor_id	int	required	Motor ID on serial bus (1-253)
operating_mode	int	1	0=Position (closed-loop), 1=Velocity (closed-loop), 2=PWM (open-loop)
min_position	double	0.0	Min position limit (radians, Mode 0 only)
max_position	double	6.283	Max position limit (2π radians, Mode 0 only)
max_velocity	double	5.22	Max velocity limit (rad/s, Modes 0 and 1, optional)
max_effort	double	1.0	Maximum allowed effort command ((0.0, 1.0], Mode 2 only). Safety limiter that restricts command range without scaling.

Dependencies

• ROS 2: Tested with Kilted
• ros2_control and ros2_controllers
• SCServo_Linux (included as git submodule)

License

Apache License 2.0 - See LICENSE file.