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

Command Interfaces

Mode 0 (Position/Servo):

• position - Target position (radians)
• velocity - Maximum speed (rad/s)
• acceleration - Acceleration (0-254, unitless protocol value)

Mode 1 (Velocity):

• velocity - Target velocity (rad/s)
• acceleration - Acceleration (0-254, unitless protocol value)

Mode 2 (PWM/Effort):

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

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.

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.

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"/>
    <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_acc_max	int	100	ACC [0–254] assigned to the wheel with the largest |target_velocity - current_velocity| delta in SyncWriteSpe. All others are scaled proportionally so every wheel finishes ramping in the same time. Set to 0 to disable.
proportional_acc_deadband	double	0.05	Minimum max-delta (rad/s) below which ACC=0 is sent to all wheels (steady-state cruise, avoids noise-driven jitter).
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.