Python → ROS Control via rclpy

Chapter Objectives

• Master Python-based ROS 2 development using rclpy
• Implement robot control interfaces in Python
• Create control nodes for humanoid robot actuators
• Integrate with ROS 2 control frameworks

Introduction to rclpy

rclpy is the Python client library for ROS 2. It provides a Python API for creating ROS 2 nodes, publishers, subscribers, services, and actions.

Key Features of rclpy

• Object-oriented API that follows Python conventions
• Asynchronous programming support with asyncio
• Integration with Python's logging system
• Support for all ROS 2 communication patterns

Installation and Setup

rclpy is included with ROS 2 installations, but you can install additional Python packages:

pip3 install rclpy
pip3 install transforms3d  # For 3D transformations
pip3 install numpy        # For numerical computations

Basic Node Structure with rclpy

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
import sys

class RobotController(Node):
    def __init__(self):
        super().__init__('robot_controller')

        # Create a QoS profile for reliable communication
        qos_profile = QoSProfile(depth=10)

        # Initialize controller parameters
        self.joint_positions = {}
        self.target_positions = {}

        self.get_logger().info('Robot Controller initialized')

def main(args=None):
    rclpy.init(args=args)
    controller = RobotController()

    try:
        rclpy.spin(controller)
    except KeyboardInterrupt:
        controller.get_logger().info('Shutting down robot controller...')
    finally:
        controller.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Implementing Joint Control

For humanoid robots, we need to control multiple joints. Let's create a joint controller:

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from control_msgs.msg import JointTrajectoryControllerState
import math
import time

class JointController(Node):
    def __init__(self):
        super().__init__('joint_controller')

        # Joint names for a humanoid robot
        self.joint_names = [
            'left_hip_joint', 'left_knee_joint', 'left_ankle_joint',
            'right_hip_joint', 'right_knee_joint', 'right_ankle_joint',
            'left_shoulder_joint', 'left_elbow_joint', 'left_wrist_joint',
            'right_shoulder_joint', 'right_elbow_joint', 'right_wrist_joint'
        ]

        # Current joint states
        self.current_positions = {name: 0.0 for name in self.joint_names}
        self.current_velocities = {name: 0.0 for name in self.joint_names}
        self.current_efforts = {name: 0.0 for name in self.joint_names}

        # Publishers and subscribers
        self.joint_state_pub = self.create_publisher(JointState, 'joint_states', 10)
        self.joint_command_sub = self.create_subscription(
            JointTrajectory, 'joint_trajectory', self.joint_command_callback, 10)

        # Timer for publishing joint states
        self.timer = self.create_timer(0.05, self.publish_joint_states)  # 20 Hz

        self.get_logger().info(f'Joint Controller initialized with {len(self.joint_names)} joints')

    def joint_command_callback(self, msg):
        """Callback for joint trajectory commands"""
        self.get_logger().info(f'Received trajectory with {len(msg.points)} points')

        # For now, just execute the first point
        if msg.points:
            point = msg.points[0]
            for i, joint_name in enumerate(msg.joint_names):
                if joint_name in self.current_positions:
                    self.current_positions[joint_name] = point.positions[i]
                    if len(point.velocities) > i:
                        self.current_velocities[joint_name] = point.velocities[i]
                    if len(point.effort) > i:
                        self.current_efforts[joint_name] = point.effort[i]

    def publish_joint_states(self):
        """Publish current joint states"""
        msg = JointState()
        msg.name = list(self.current_positions.keys())
        msg.position = list(self.current_positions.values())
        msg.velocity = list(self.current_velocities.values())
        msg.effort = list(self.current_efforts.values())
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'base_link'

        self.joint_state_pub.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    controller = JointController()

    try:
        rclpy.spin(controller)
    except KeyboardInterrupt:
        controller.get_logger().info('Shutting down joint controller...')
    finally:
        controller.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Control Algorithms in Python

Let's implement a PID controller for joint position control:

class PIDController:
    def __init__(self, kp=1.0, ki=0.0, kd=0.0, dt=0.01):
        self.kp = kp  # Proportional gain
        self.ki = ki  # Integral gain
        self.kd = kd  # Derivative gain
        self.dt = dt  # Time step

        self.prev_error = 0.0
        self.integral = 0.0

    def compute(self, setpoint, measurement):
        """Compute control output using PID algorithm"""
        error = setpoint - measurement

        # Proportional term
        p_term = self.kp * error

        # Integral term
        self.integral += error * self.dt
        i_term = self.ki * self.integral

        # Derivative term
        derivative = (error - self.prev_error) / self.dt
        d_term = self.kd * derivative

        # Store error for next iteration
        self.prev_error = error

        # Compute output
        output = p_term + i_term + d_term

        return output

class JointPIDController(Node):
    def __init__(self):
        super().__init__('joint_pid_controller')

        # Initialize PID controllers for each joint
        self.pid_controllers = {}
        self.joint_names = [
            'left_hip_joint', 'left_knee_joint', 'left_ankle_joint',
            'right_hip_joint', 'right_knee_joint', 'right_ankle_joint'
        ]

        for joint_name in self.joint_names:
            # Different PID parameters for different joints
            if 'hip' in joint_name:
                self.pid_controllers[joint_name] = PIDController(kp=2.0, ki=0.1, kd=0.05)
            elif 'knee' in joint_name:
                self.pid_controllers[joint_name] = PIDController(kp=1.5, ki=0.05, kd=0.03)
            else:  # ankle
                self.pid_controllers[joint_name] = PIDController(kp=1.0, ki=0.02, kd=0.01)

        # Current states
        self.current_positions = {name: 0.0 for name in self.joint_names}
        self.target_positions = {name: 0.0 for name in self.joint_names}
        self.control_outputs = {name: 0.0 for name in self.joint_names}

        # Publishers and subscribers
        self.joint_state_sub = self.create_subscription(
            JointState, 'joint_states', self.joint_state_callback, 10)
        self.control_pub = self.create_publisher(JointTrajectory, 'joint_trajectory', 10)

        # Timer for control loop (100 Hz)
        self.control_timer = self.create_timer(0.01, self.control_loop)

        self.get_logger().info('Joint PID Controller initialized')

    def joint_state_callback(self, msg):
        """Update current joint positions"""
        for i, name in enumerate(msg.name):
            if name in self.current_positions:
                self.current_positions[name] = msg.position[i]

    def control_loop(self):
        """Main control loop"""
        for joint_name in self.joint_names:
            current_pos = self.current_positions[joint_name]
            target_pos = self.target_positions[joint_name]

            # Compute control output using PID
            control_output = self.pid_controllers[joint_name].compute(target_pos, current_pos)
            self.control_outputs[joint_name] = control_output

            # For this example, we'll just publish the target positions
            # In a real system, you would convert the control output to actual motor commands

        # Publish the desired trajectory
        self.publish_trajectory_command()

    def publish_trajectory_command(self):
        """Publish trajectory command"""
        msg = JointTrajectory()
        msg.joint_names = self.joint_names

        point = JointTrajectoryPoint()
        point.positions = [self.target_positions[name] for name in self.joint_names]
        point.velocities = [0.0] * len(self.joint_names)  # Zero velocity
        point.time_from_start.sec = 0
        point.time_from_start.nanosec = 50000000  # 50ms

        msg.points = [point]
        self.control_pub.publish(msg)

    def set_target_positions(self, targets):
        """Set target positions for joints"""
        for joint_name, position in targets.items():
            if joint_name in self.target_positions:
                self.target_positions[joint_name] = position

Working with ROS 2 Control

ROS 2 Control is the standard framework for robot control. Let's see how to integrate with it:

# my_robot_package/my_robot_package/humanoid_controller.py
import rclpy
from rclpy.node import Node
from controller_manager_msgs.srv import SwitchController
from hardware_interface_msgs.msg import HardwareInterface
import time

class HumanoidController(Node):
    def __init__(self):
        super().__init__('humanoid_controller')

        # Controller management client
        self.switch_controller_client = self.create_client(
            SwitchController, '/controller_manager/switch_controller')

        # Wait for controller manager service
        while not self.switch_controller_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Controller manager service not available, waiting...')

        self.get_logger().info('Humanoid Controller initialized')

    def switch_controllers(self, start_controllers, stop_controllers, strictness=1):
        """Switch between controllers"""
        request = SwitchController.Request()
        request.start_controllers = start_controllers
        request.stop_controllers = stop_controllers
        request.strictness = strictness

        future = self.switch_controller_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)

        if future.result() is not None:
            response = future.result()
            if response.ok:
                self.get_logger().info(f'Successfully switched controllers')
                return True
            else:
                self.get_logger().error(f'Failed to switch controllers: {response.error_message}')
                return False
        else:
            self.get_logger().error('Failed to call controller manager service')
            return False

    def initialize_robot(self):
        """Initialize the robot by starting necessary controllers"""
        self.get_logger().info('Initializing humanoid robot...')

        # Start joint state broadcaster
        success = self.switch_controllers(
            start_controllers=['joint_state_broadcaster'],
            stop_controllers=[]
        )

        if success:
            time.sleep(1.0)  # Wait for controller to start

            # Start position controllers for all joints
            position_controllers = [
                'left_leg_position_controller',
                'right_leg_position_controller',
                'left_arm_position_controller',
                'right_arm_position_controller',
                'head_position_controller'
            ]

            success = self.switch_controllers(
                start_controllers=position_controllers,
                stop_controllers=[]
            )

        if success:
            self.get_logger().info('Robot initialized successfully')
        else:
            self.get_logger().error('Failed to initialize robot')

        return success

Asynchronous Programming with rclpy

rclpy supports asyncio for more complex control patterns:

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
import asyncio

class AsyncController(Node):
    def __init__(self):
        super().__init__('async_controller')

        # Create callback groups for threading
        self.group1 = MutuallyExclusiveCallbackGroup()
        self.group2 = MutuallyExclusiveCallbackGroup()

        # Create timers with different callback groups
        self.timer1 = self.create_timer(0.1, self.async_task1, callback_group=self.group1)
        self.timer2 = self.create_timer(0.2, self.async_task2, callback_group=self.group2)

    def async_task1(self):
        """Asynchronous task 1"""
        self.get_logger().info('Executing async task 1')
        # Simulate some work
        time.sleep(0.05)

    def async_task2(self):
        """Asynchronous task 2"""
        self.get_logger().info('Executing async task 2')
        # Simulate some work
        time.sleep(0.03)

def main(args=None):
    rclpy.init(args=args)
    node = AsyncController()

    # Use multi-threaded executor for async operations
    executor = MultiThreadedExecutor(num_threads=4)
    executor.add_node(node)

    try:
        executor.spin()
    except KeyboardInterrupt:
        node.get_logger().info('Shutting down async controller...')
    finally:
        node.destroy_node()
        rclpy.shutdown()

Best Practices for Humanoid Control

Performance Considerations

• Use appropriate control frequencies (typically 100-1000 Hz for joint control)
• Minimize computational overhead in control loops
• Use efficient data structures for joint management
• Consider real-time capabilities for critical control tasks

Safety Features

• Implement joint position and velocity limits
• Add emergency stop functionality
• Monitor control effort to detect issues
• Implement graceful degradation when problems occur

Code Organization

• Separate control logic from ROS 2 communication
• Use configuration files for controller parameters
• Implement proper error handling and logging
• Create reusable controller components

Hands-On Exercise

1. Create a Python node that controls a simple humanoid robot model
2. Implement PID controllers for at least 6 joints
3. Create a trajectory publisher that moves the robot through a simple motion
4. Add safety limits to prevent joint damage

Summary

Python provides a powerful platform for ROS 2 robot control through rclpy. With proper control algorithms and integration with ROS 2 Control, you can create sophisticated humanoid robot controllers. In the next chapter, we'll explore URDF for humanoid robot modeling.

Learning Path Adjustment

Based on your experience level, you may want to focus on:

• Beginner: Focus on basic node creation and simple control loops
• Intermediate: Dive deeper into PID control and trajectory generation
• Advanced: Explore real-time control, advanced control algorithms, and system integration