Skip to main content

Control Systems Fundamentals

Introduction to Control Systems in Physical AI​

Control systems are the backbone of Physical AI, translating high-level decisions into precise physical actions. Unlike traditional control systems that operate in well-defined industrial environments, Physical AI control systems must operate in uncertain, dynamic environments while maintaining safety and performance.

Key Characteristics of Physical AI Control Systems​

  1. Adaptive: Must adapt to changing environmental conditions
  2. Robust: Must maintain performance despite uncertainties and disturbances
  3. Safe: Must ensure safe operation in human environments
  4. Real-time: Must respond within strict timing constraints
  5. Multi-modal: Must coordinate multiple types of actuators and sensors

Control System Architecture​

Hierarchical Control Structure​

Physical AI systems typically employ a hierarchical control structure:

┌─────────────────────────────────────────────────────────┐
│                    Task Level                           │
│  (What to do - planning, decision making)              │
├─────────────────────────────────────────────────────────┤
│                    Motion Level                         │
│  (How to do it - trajectory generation, coordination)  │
├─────────────────────────────────────────────────────────┤
│                    Balance Level                        │
│  (Stability - balance, posture control)                │
├─────────────────────────────────────────────────────────┤
│                    Joint Level                          │
│  (Execution - individual joint control)                │
└─────────────────────────────────────────────────────────┘

Control Loop Fundamentals​

The basic control loop in Physical AI systems:

┌─────────────────┐    ┌─────────────┐    ┌─────────────┐
│   Reference     │───▶│ Controller  │───▶│  Physical   │
│   (Desired)     │    │             │    │   System    │
└─────────────────┘    └─────────────┘    └─────────────┘
         ▲                       │                   │
         │                       │                   │
         └───────────────────────┼───────────────────┘
                                 ▼
                           ┌─────────────┐
                           │   Sensor    │
                           │   Feedback  │
                           └─────────────┘

Classical Control Methods​

Proportional-Integral-Derivative (PID) Control​

PID control is fundamental to many Physical AI systems:

class PIDController:
    def __init__(self, kp, ki, kd, output_limits=(None, None)):
        self.kp = kp  # Proportional gain
        self.ki = ki  # Integral gain
        self.kd = kd  # Derivative gain
        self.output_limits = output_limits

        self.previous_error = 0
        self.integral = 0
        self.last_time = None

    def compute(self, setpoint, measured_value, dt=None):
        current_time = time.time()
        if dt is None and self.last_time is not None:
            dt = current_time - self.last_time
        elif dt is None:
            dt = 0.001  # Default small time step

        error = setpoint - measured_value

        # Proportional term
        p_term = self.kp * error

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

        # Derivative term
        if dt > 0:
            derivative = (error - self.previous_error) / dt
        else:
            derivative = 0
        d_term = self.kd * derivative

        # Total output
        output = p_term + i_term + d_term

        # Apply output limits
        min_out, max_out = self.output_limits
        if min_out is not None:
            output = max(output, min_out)
        if max_out is not None:
            output = min(output, max_out)

        self.previous_error = error
        self.last_time = current_time

        return output

Cascade Control​

For complex systems, cascade control uses multiple PID controllers in series:

class CascadeController:
    def __init__(self):
        # Outer loop: position control
        self.position_controller = PIDController(kp=2.0, ki=0.1, kd=0.05)
        # Inner loop: velocity control
        self.velocity_controller = PIDController(kp=1.5, ki=0.05, kd=0.02)

    def compute(self, desired_position, current_position, current_velocity):
        # Position error -> desired velocity
        desired_velocity = self.position_controller.compute(
            desired_position, current_position, dt=0.01
        )

        # Velocity error -> control output
        control_output = self.velocity_controller.compute(
            desired_velocity, current_velocity, dt=0.01
        )

        return control_output

Advanced Control Techniques​

Impedance Control​

Impedance control regulates the relationship between force and position, essential for safe human-robot interaction:

class ImpedanceController:
    def __init__(self, mass, damping, stiffness):
        self.M = mass      # Inertia matrix
        self.B = damping   # Damping matrix
        self.K = stiffness # Stiffness matrix

    def compute(self, desired_pos, current_pos, desired_vel, current_vel, external_force):
        # Error terms
        pos_error = desired_pos - current_pos
        vel_error = desired_vel - current_vel

        # Impedance equation: M*a + B*v + K*x = F
        acceleration = (external_force - self.B * vel_error - self.K * pos_error) / self.M

        return acceleration

Admittance Control​

Admittance control is the dual of impedance control, where motion is generated in response to applied forces:

class AdmittanceController:
    def __init__(self, stiffness, damping, mass):
        self.alpha = 1 / stiffness  # Compliance (inverse of stiffness)
        self.beta = 1 / damping     # Inverse damping
        self.gamma = 1 / mass       # Inverse mass

    def compute(self, applied_force, current_pos, current_vel):
        # Admittance: motion response to force
        desired_acceleration = self.gamma * applied_force
        desired_velocity = current_vel + self.beta * applied_force
        desired_position = current_pos + self.alpha * applied_force

        return desired_position, desired_velocity, desired_acceleration

Model Predictive Control (MPC)​

MPC optimizes control actions over a prediction horizon:

import numpy as np
from scipy.optimize import minimize

class ModelPredictiveController:
    def __init__(self, prediction_horizon, control_horizon, system_model):
        self.Np = prediction_horizon  # Prediction horizon
        self.Nc = control_horizon     # Control horizon
        self.model = system_model     # System model

    def compute(self, current_state, reference_trajectory):
        def cost_function(control_sequence):
            # Simulate system response to control sequence
            state = current_state.copy()
            total_cost = 0

            for k in range(self.Np):
                # Apply control action
                if k < self.Nc:
                    control = control_sequence[k]
                else:
                    control = control_sequence[-1]  # Hold last value

                # Predict next state
                state = self.model.predict(state, control)

                # Add tracking cost
                tracking_error = state - reference_trajectory[k]
                total_cost += tracking_error.T @ tracking_error

            # Add control effort penalty
            control_effort = control_sequence.T @ control_sequence
            total_cost += 0.1 * control_effort

            return total_cost

        # Initial guess for control sequence
        initial_controls = np.zeros(self.Nc)

        # Optimize control sequence
        result = minimize(cost_function, initial_controls, method='SLSQP')

        # Return first control action
        return result.x[0] if len(result.x) > 0 else 0.0

Joint-Level Control​

Position Control​

Position control is the most common form of joint control:

class JointPositionController:
    def __init__(self, joint_name, kp=10.0, ki=0.1, kd=0.5):
        self.joint_name = joint_name
        self.pid = PIDController(kp, ki, kd, output_limits=(-100, 100))  # Torque limits
        self.desired_position = 0.0
        self.current_position = 0.0

    def update(self, desired_position, current_position, dt=0.01):
        self.desired_position = desired_position
        self.current_position = current_position

        torque_command = self.pid.compute(desired_position, current_position, dt)
        return torque_command

Velocity Control​

Velocity control is useful for coordinated motion:

class JointVelocityController:
    def __init__(self, joint_name, kp=5.0, ki=0.05, kd=0.1):
        self.joint_name = joint_name
        self.pid = PIDController(kp, ki, kd, output_limits=(-50, 50))
        self.desired_velocity = 0.0
        self.current_velocity = 0.0

    def update(self, desired_velocity, current_velocity, dt=0.01):
        self.desired_velocity = desired_velocity
        self.current_velocity = current_velocity

        torque_command = self.pid.compute(desired_velocity, current_velocity, dt)
        return torque_command

Impedance Control for Joints​

For safe interaction, joints may use impedance control:

class JointImpedanceController:
    def __init__(self, joint_name, stiffness=100.0, damping=10.0):
        self.joint_name = joint_name
        self.stiffness = stiffness
        self.damping = damping
        self.equilibrium_position = 0.0

    def update(self, equilibrium_pos, current_pos, current_vel, external_torque=0.0):
        self.equilibrium_position = equilibrium_pos

        # Spring-damper model
        position_error = equilibrium_pos - current_pos
        spring_force = self.stiffness * position_error
        damping_force = self.damping * current_vel

        total_torque = spring_force - damping_force + external_torque

        return total_torque

Whole-Body Control​

Operational Space Control​

Operational space control allows control of end-effector motion while maintaining joint-level constraints:

import numpy as np

class OperationalSpaceController:
    def __init__(self, robot_model):
        self.model = robot_model  # Contains kinematics and dynamics

    def compute(self, task_desired_acceleration, current_joint_pos, current_joint_vel):
        # Get Jacobian and mass matrix
        J = self.model.get_jacobian()
        M = self.model.get_mass_matrix()

        # Compute operational space mass matrix
        Lambda = np.linalg.inv(J @ np.linalg.inv(M) @ J.T)

        # Compute operational space forces
        task_force = Lambda @ task_desired_acceleration

        # Compute joint torques
        J_T = J.T  # Transpose of Jacobian
        joint_torques = J_T @ task_force

        # Add Coriolis and gravity compensation
        C = self.model.get_coriolis_matrix(current_joint_pos, current_joint_vel)
        g = self.model.get_gravity_vector(current_joint_pos)

        total_torques = joint_torques + C @ current_joint_vel + g

        return total_torques

Inverse Kinematics​

Inverse kinematics solves for joint angles to achieve desired end-effector positions:

class InverseKinematicsSolver:
    def __init__(self, robot_model):
        self.model = robot_model

    def solve(self, target_position, target_orientation, current_joint_angles, max_iterations=100, tolerance=1e-4):
        joint_angles = current_joint_angles.copy()

        for i in range(max_iterations):
            # Forward kinematics to get current end-effector pose
            current_pose = self.model.forward_kinematics(joint_angles)

            # Calculate error
            pos_error = target_position - current_pose[:3, 3]
            # For simplicity, assuming orientation is handled separately

            # Check convergence
            if np.linalg.norm(pos_error) < tolerance:
                break

            # Calculate Jacobian
            J = self.model.get_jacobian(joint_angles)

            # Update joint angles using Jacobian transpose or pseudoinverse
            delta_joints = np.linalg.pinv(J[:3, :]) @ pos_error  # Position part only
            joint_angles += delta_joints * 0.1  # Small step size for stability

        return joint_angles

Balance and Locomotion Control​

Zero Moment Point (ZMP) Control​

ZMP control is fundamental for bipedal balance:

class ZMPController:
    def __init__(self, robot_mass, gravity=9.81):
        self.mass = robot_mass
        self.g = gravity
        self.com_height = 0.8  # Center of mass height

    def compute_desired_zmp(self, desired_com_pos, desired_com_vel, desired_com_acc):
        # ZMP = CoM position - (CoM height / gravity) * CoM acceleration
        zmp_x = desired_com_pos[0] - (self.com_height / self.g) * desired_com_acc[0]
        zmp_y = desired_com_pos[1] - (self.com_height / self.g) * desired_com_acc[1]

        return np.array([zmp_x, zmp_y])

Linear Inverted Pendulum Model (LIPM)​

LIPM simplifies balance control:

class LIPMController:
    def __init__(self, com_height, gravity=9.81):
        self.h = com_height
        self.g = gravity
        self.omega = np.sqrt(self.g / self.h)

    def compute_com_trajectory(self, initial_com, initial_vel, final_com, duration, dt=0.01):
        t = np.arange(0, duration, dt)

        # LIPM solution
        a = initial_com - final_com
        b = initial_vel / self.omega - a

        com_trajectory = []
        com_velocity = []

        for ti in t:
            com_pos = final_com + a * np.cosh(self.omega * ti) + b * np.sinh(self.omega * ti)
            com_vel = a * self.omega * np.sinh(self.omega * ti) + b * self.omega * np.cosh(self.omega * ti)

            com_trajectory.append(com_pos)
            com_velocity.append(com_vel)

        return np.array(com_trajectory), np.array(com_velocity)

Force and Impedance Control​

Hybrid Position/Force Control​

Combines position and force control for manipulation tasks:

class HybridPositionForceController:
    def __init__(self, robot_model):
        self.model = robot_model
        self.position_gains = {'kp': 100, 'ki': 1, 'kd': 10}
        self.force_gains = {'kp': 50, 'ki': 0.5, 'kd': 5}

    def compute(self, desired_pos, current_pos, desired_force, current_force, contact_surface_normal):
        # Decompose into position and force control directions
        # Position control in unconstrained directions
        pos_error = desired_pos - current_pos
        pos_control = (self.position_gains['kp'] * pos_error +
                      self.position_gains['kd'] * (0 - 0))  # Assuming zero velocity for simplicity

        # Force control in constrained directions
        force_error = desired_force - current_force
        force_control = self.force_gains['kp'] * force_error

        # Combine controls based on contact constraints
        # This is a simplified version - full implementation would use selection matrices
        total_control = pos_control + force_control

        return total_control

Safety and Compliance​

Safety Controllers​

Safety controllers monitor and limit potentially dangerous behaviors:

class SafetyController:
    def __init__(self):
        self.joint_limits = {}
        self.velocity_limits = {}
        self.torque_limits = {}
        self.workspace_limits = {}

    def check_safety(self, joint_angles, joint_velocities, joint_torques, end_effector_pose):
        # Check joint limits
        for i, angle in enumerate(joint_angles):
            if (angle < self.joint_limits[i][0] or angle > self.joint_limits[i][1]):
                return False, f"Joint {i} limit exceeded"

        # Check velocity limits
        for i, vel in enumerate(joint_velocities):
            if abs(vel) > self.velocity_limits[i]:
                return False, f"Joint {i} velocity limit exceeded"

        # Check torque limits
        for i, torque in enumerate(joint_torques):
            if abs(torque) > self.torque_limits[i]:
                return False, f"Joint {i} torque limit exceeded"

        # Check workspace limits
        if (end_effector_pose[0] < self.workspace_limits[0][0] or
            end_effector_pose[0] > self.workspace_limits[0][1]):
            return False, "End effector out of workspace (X)"

        return True, "Safe"

Control System Design Considerations​

Sampling Rates and Timing​

Different control loops require different update rates:

  • High-level planning: 1-10 Hz
  • Trajectory generation: 10-50 Hz
  • Balance control: 50-200 Hz
  • Joint control: 200-1000 Hz

Sensor Integration​

Control systems must integrate multiple sensor types:

  • Proprioceptive: Joint encoders, IMUs, force/torque sensors
  • Exteroceptive: Cameras, LIDAR, tactile sensors
  • Fusion: Combine sensor data for robust state estimation

System Identification​

Understanding system dynamics is crucial for effective control:

def identify_system_dynamics(excitation_signal, response_data):
    """
    System identification to determine model parameters
    """
    # For a simple second-order system: M*q_ddot + B*q_dot + K*q = tau
    # We can use least squares to estimate M, B, K

    # Prepare regression matrix
    Y = response_data['acceleration']
    PHI = np.column_stack([
        response_data['position'],
        response_data['velocity'],
        excitation_signal
    ])

    # Solve for parameters
    params = np.linalg.lstsq(PHI, Y, rcond=None)[0]

    stiffness = params[0]
    damping = params[1]
    inertia_inv = 1.0 / params[2]  # Since tau = ... + 1/M * tau_applied

    return {'inertia': inertia_inv, 'damping': damping, 'stiffness': stiffness}

Implementation Challenges​

Computational Constraints​

Real-time control requires efficient algorithms:

  • Pre-computation: Calculate expensive operations offline
  • Approximation: Use simplified models where appropriate
  • Parallelization: Distribute computation across multiple cores

Communication Delays​

Networked control systems must account for communication delays:

  • Predictive control: Predict system state accounting for delays
  • Event-triggered control: Update control only when necessary
  • Delay compensation: Compensate for known delays

Model Uncertainty​

Real systems differ from models:

  • Robust control: Design for worst-case model errors
  • Adaptive control: Adjust parameters based on system response
  • Learning-based control: Improve models through experience

Integration with Higher-Level Systems​

Control systems must interface with:

  • Planning systems: Receive trajectory commands
  • Perception systems: Use sensor data for feedback
  • Decision-making systems: Coordinate with high-level behaviors
  • Human interfaces: Provide transparency and safety

Testing and Validation​

Simulation Testing​

Test control systems in simulation before physical deployment:

  • Gazebo: Physics-based simulation
  • PyBullet: Realistic physics simulation
  • Custom simulators: Tailored to specific applications

Hardware-in-the-Loop​

Test with real hardware in controlled environments:

  • Safety-rated environments: Controlled testing spaces
  • Gradual complexity: Start simple, increase complexity
  • Monitoring: Comprehensive data logging

Future Directions​

Learning-Based Control​

Integration of machine learning with classical control:

  • Neural network controllers: Learn complex control policies
  • Reinforcement learning: Optimize control through interaction
  • Imitation learning: Learn from expert demonstrations

Distributed Control​

For multi-robot systems:

  • Consensus algorithms: Coordinate multiple agents
  • Distributed optimization: Solve control problems jointly
  • Communication-aware control: Account for network constraints

Summary​

Control systems in Physical AI require careful consideration of safety, performance, and real-time constraints. Successful implementations combine classical control theory with advanced techniques like impedance control, whole-body control, and safety monitoring. The field continues to evolve with new approaches in learning-based control and distributed systems.

The next chapter will explore path planning implementation, which works closely with control systems to generate feasible trajectories for Physical AI systems.

Exercises​

  1. Implement a PID controller for a single joint and tune the parameters for optimal performance.
  2. Design an impedance controller for safe human-robot interaction.
  3. Create a simple operational space controller for end-effector positioning.
  4. Simulate a balance control system using the Linear Inverted Pendulum Model.