Control Module User Guide — ManipulaPy 1.1.0 Documentation
Description: The ManipulaPy Control Module provides comprehensive control algorithms for robotic manipulators with GPU acceleration using CuPy. This guide covers all available control methods, their parameters,...

# Control Module User Guide

The ManipulaPy Control Module provides comprehensive control algorithms for robotic manipulators with GPU acceleration using CuPy. This guide covers all available control methods, their parameters, and usage examples.

## Overview

The control module implements various control strategies:

- PID Control: Classic feedback control

- Computed Torque Control: Model-based linearizing control

- Adaptive Control: Online parameter adaptation

- Robust Control: Disturbance rejection

- Feedforward Control: Open-loop compensation

- Kalman Filter: State estimation and control

All algorithms are GPU-accelerated and designed for real-time performance.

## Mathematical Foundation

This section summarizes the key control-law equations implemented in the `ManipulatorController` class.

### Computed-Torque (Inverse Dynamics) Control

Compute the required joint-torques to achieve a desired trajectory while compensating for the robot’s nonlinear dynamics:

\[\begin{split}e &= q_d - q, \quad \dot{e} = \dot{q}_d - \dot{q}, \quad \int e\,dt = \sum e\,dt \\ \tau_{ff} &= M(q)\,\bigl(\ddot{q}_d + K_d\,\dot{e} + K_p\,e + K_i\,\!\!\int e\,dt\bigr) + C(q,\dot{q})\,\dot{q} + G(q)\end{split}\]

Where:

- \(q,\dot{q},\ddot{q}\) – actual joint angles, velocities, accelerations

- \(q_d,\dot{q}_d,\ddot{q}_d\) – desired joint trajectory

- \(M(q)\) – mass-inertia matrix

- \(C(q,\dot{q})\) – Coriolis/centrifugal matrix

- \(G(q)\) – gravity vector

- \(K_p,K_i,K_d\) – proportional, integral, derivative gains

### PID Control

A simpler joint-space PID law (no dynamics compensation):

\[\tau_{PID} = K_p\,e + K_i\,\int e\,dt + K_d\,\dot{e}\]

### PD Feedforward Control

Combine a PD feedback term with pure feedforward dynamics:

\[\tau = K_p\,e + K_d\,\dot{e} + \underbrace{M(q)\,\ddot{q}_d + C(q,\dot{q})\,\dot{q}_d + G(q)}_{\text{feedforward torque}}\]

### Adaptive Control

Adjusts internal dynamic parameters online to compensate for modelling errors:

\[\begin{split}\hat{\theta}_{k+1} &= \hat{\theta}_k + \gamma \,e \\ \tau &= M(q)\,\ddot{q}_d + C(q,\dot{q})\,\dot{q}_d + G(q) + Y(q,\dot{q},\ddot{q})\,\hat{\theta}\end{split}\]

Where \(\hat{\theta}\) is the estimated parameter vector and \(\gamma\) the adaptation gain.

### Robust Control

Adds a disturbance-rejection term to a computed-torque core:

\[\tau = M(q)\,\ddot{q} + C(q,\dot{q})\,\dot{q} + G(q) + J(q)^T\,F_{ext} + \alpha\,\hat{d}\]

With \(\hat{d}\) the estimated disturbance and \(\alpha\) its gain.

### Kalman-Filter State Estimation

Predict-update loop for joint state \(x = [q;\dot{q}]\):

1. Prediction

   \[\hat{x}^- = F\,\hat{x} + B\,u, \quad P^- = F\,P\,F^T + Q\]

2. Update

   \[\begin{split}K &= P^-\,H^T(H\,P^-\,H^T + R)^{-1} \\ \hat{x} &= \hat{x}^- + K(y - H\,\hat{x}^-), \quad P = (I - K\,H)P^-\end{split}\]

Where \(Q,R\) are process/measurement covariances.

These formulas correspond exactly to the methods in `ManipulatorController`.

## Installation and Setup

### Prerequisites

    pip install cupy-cuda11x  # or cupy-cuda12x for CUDA 12
    pip install numpy matplotlib

### Basic Import

    import numpy as np
    import cupy as cp
    from ManipulaPy.control import ManipulatorController
    from ManipulaPy.dynamics import ManipulatorDynamics

## Quick Start

### Initialize Controller

    # Assuming you have a dynamics object
    controller = ManipulatorController(dynamics)

    # Basic PID control example
    Kp = np.array([10.0, 8.0, 5.0, 3.0, 2.0, 1.0])  # Proportional gains
    Ki = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])   # Integral gains
    Kd = np.array([1.0, 0.8, 0.5, 0.3, 0.2, 0.1])   # Derivative gains

    # Desired and current states
    thetalistd = np.array([0.5, 0.3, -0.2, 0.1, 0.0, 0.0])  # Desired angles
    thetalist = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])    # Current angles
    dthetalistd = np.zeros(6)  # Desired velocities
    dthetalist = np.zeros(6)   # Current velocities

    # Control signal
    tau = controller.pid_control(
        thetalistd, dthetalistd, thetalist, dthetalist,
        dt=0.01, Kp=Kp, Ki=Ki, Kd=Kd
    )

## Controller Types

### PID Control

Description: Classic Proportional-Integral-Derivative control for joint space regulation.

Example:

    # Define gains for 6-DOF manipulator
    Kp = np.array([15.0, 12.0, 8.0, 5.0, 3.0, 2.0])
    Ki = np.array([0.5, 0.4, 0.3, 0.2, 0.1, 0.1])
    Kd = np.array([2.0, 1.5, 1.0, 0.8, 0.5, 0.3])

    tau = controller.pid_control(
        thetalistd=desired_angles,
        dthetalistd=desired_velocities,
        thetalist=current_angles,
        dthetalist=current_velocities,
        dt=0.01,
        Kp=Kp, Ki=Ki, Kd=Kd
    )

### PD Control

Description: Proportional-Derivative control without integral term.

Example:

    Kp = np.array([20.0, 15.0, 10.0, 8.0, 5.0, 3.0])
    Kd = np.array([3.0, 2.5, 2.0, 1.5, 1.0, 0.5])

    tau = controller.pd_control(
        desired_position=desired_angles,
        desired_velocity=desired_velocities,
        current_position=current_angles,
        current_velocity=current_velocities,
        Kp=Kp, Kd=Kd
    )

### Computed Torque Control

Description: Model-based control that linearizes the nonlinear robot dynamics.

Example:

    # Higher gains can be used due to linearization
    Kp = np.array([50.0, 40.0, 30.0, 20.0, 15.0, 10.0])
    Ki = np.array([1.0, 0.8, 0.6, 0.4, 0.3, 0.2])
    Kd = np.array([8.0, 6.0, 4.0, 3.0, 2.0, 1.0])

    gravity = np.array([0, 0, -9.81])

    tau = controller.computed_torque_control(
        thetalistd=desired_angles,
        dthetalistd=desired_velocities,
        ddthetalistd=desired_accelerations,
        thetalist=current_angles,
        dthetalist=current_velocities,
        g=gravity,
        dt=0.01,
        Kp=Kp, Ki=Ki, Kd=Kd
    )

### Feedforward Control

Description: Open-loop control based on desired trajectory dynamics.

Example:

    gravity = np.array([0, 0, -9.81])
    external_forces = np.zeros(6)  # No external forces

    tau_ff = controller.feedforward_control(
        desired_position=trajectory_positions[i],
        desired_velocity=trajectory_velocities[i],
        desired_acceleration=trajectory_accelerations[i],
        g=gravity,
        Ftip=external_forces
    )

### PD + Feedforward Control

Description: Combines feedback PD control with feedforward compensation.

Example:

    tau = controller.pd_feedforward_control(
        desired_position=trajectory_positions[i],
        desired_velocity=trajectory_velocities[i],
        desired_acceleration=trajectory_accelerations[i],
        current_position=current_angles,
        current_velocity=current_velocities,
        Kp=Kp, Kd=Kd,
        g=gravity,
        Ftip=external_forces
    )

### Robust Control

Description: Control with disturbance rejection capabilities.

Example:

    disturbance_estimate = np.array([0.1, -0.05, 0.08, 0.0, 0.02, -0.01])
    adaptation_gain = 0.5

    tau = controller.robust_control(
        thetalist=current_angles,
        dthetalist=current_velocities,
        ddthetalist=desired_accelerations,
        g=gravity,
        Ftip=external_forces,
        disturbance_estimate=disturbance_estimate,
        adaptation_gain=adaptation_gain
    )

### Adaptive Control

Description: Control with online parameter adaptation.

Example:

    measurement_error = current_angles - estimated_angles
    adaptation_gain = 0.1

    tau = controller.adaptive_control(
        thetalist=current_angles,
        dthetalist=current_velocities,
        ddthetalist=desired_accelerations,
        g=gravity,
        Ftip=external_forces,
        measurement_error=measurement_error,
        adaptation_gain=adaptation_gain
    )

## Advanced Features

### Joint and Torque Limit Enforcement

Example:

    # Define limits
    joint_limits = np.array([
        [-np.pi, np.pi],     # Joint 1
        [-np.pi/2, np.pi/2], # Joint 2
        [-np.pi, np.pi],     # Joint 3
        [-np.pi, np.pi],     # Joint 4
        [-np.pi/2, np.pi/2], # Joint 5
        [-np.pi, np.pi]      # Joint 6
    ])

    torque_limits = np.array([
        [-50, 50],  # Joint 1 torque limits (Nm)
        [-40, 40],  # Joint 2
        [-30, 30],  # Joint 3
        [-20, 20],  # Joint 4
        [-15, 15],  # Joint 5
        [-10, 10]   # Joint 6
    ])

    # Apply limits
    safe_angles, safe_velocities, safe_torques = controller.enforce_limits(
        thetalist=current_angles,
        dthetalist=current_velocities,
        tau=computed_torques,
        joint_limits=joint_limits,
        torque_limits=torque_limits
    )

### Kalman Filter State Estimation

Example:

    # Define noise covariances
    Q = np.eye(12) * 0.01  # Process noise (12x12 for 6 positions + 6 velocities)
    R = np.eye(12) * 0.1   # Measurement noise

    # Use Kalman filter for state estimation
    estimated_angles, estimated_velocities = controller.kalman_filter_control(
        thetalistd=desired_angles,
        dthetalistd=desired_velocities,
        thetalist=measured_angles,
        dthetalist=measured_velocities,
        taulist=applied_torques,
        g=gravity,
        Ftip=external_forces,
        dt=0.01,
        Q=Q, R=R
    )

## Tuning Methods

### Ziegler-Nichols Tuning

Example:

    # Find ultimate gain and period first
    ultimate_gain, ultimate_period, gain_history, error_history = controller.find_ultimate_gain_and_period(
        thetalist=initial_angles,
        desired_joint_angles=target_angles,
        dt=0.01,
        max_steps=1000
    )

    # Tune controller gains
    Kp, Ki, Kd = controller.ziegler_nichols_tuning(
        Ku=ultimate_gain,
        Tu=ultimate_period,
        kind="PID"
    )

    print(f"Tuned gains - Kp: {Kp}, Ki: {Ki}, Kd: {Kd}")

### Automatic Ultimate Gain Finding

Example:

    # Automatically find critical parameters
    results = controller.find_ultimate_gain_and_period(
        thetalist=np.zeros(6),
        desired_joint_angles=np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]),
        dt=0.01,
        max_steps=500
    )

    ultimate_gain, ultimate_period, gain_history, error_history = results

    # Use results for tuning
    Kp, Ki, Kd = controller.tune_controller(ultimate_gain, ultimate_period, kind="PID")

## Plotting and Analysis

### Steady-State Response Analysis

Example:

    # Simulate control response
    time_steps = np.linspace(0, 5, 500)
    response_data = []  # Your simulation data
    set_point = 0.5

    controller.plot_steady_state_response(
        time=time_steps,
        response=response_data,
        set_point=set_point,
        title="Joint 1 Step Response"
    )

### Performance Metrics

Calculate control performance metrics:

Example:

    # Calculate individual metrics
    rise_time = controller.calculate_rise_time(time, response, set_point)
    overshoot = controller.calculate_percent_overshoot(response, set_point)
    settling_time = controller.calculate_settling_time(time, response, set_point)
    steady_error = controller.calculate_steady_state_error(response, set_point)

    print(f"Rise Time: {rise_time:.3f}s")
    print(f"Overshoot: {overshoot:.2f}%")
    print(f"Settling Time: {settling_time:.3f}s")
    print(f"Steady-State Error: {steady_error:.4f}")

## Cartesian Space Control

### Joint Space Control

### Cartesian Space Control

## Examples

### Complete Control Loop Example

    import numpy as np
    import cupy as cp
    from ManipulaPy.control import ManipulatorController

    def control_loop_example(controller, dynamics):
        # Initialize states
        current_angles = np.zeros(6)
        current_velocities = np.zeros(6)
        desired_angles = np.array([0.5, 0.3, -0.2, 0.1, 0.4, -0.1])

        # Control parameters
        Kp = np.array([20.0, 15.0, 12.0, 8.0, 5.0, 3.0])
        Ki = np.array([0.5, 0.4, 0.3, 0.2, 0.1, 0.1])
        Kd = np.array([2.0, 1.5, 1.2, 0.8, 0.5, 0.3])

        dt = 0.01
        simulation_time = 5.0
        steps = int(simulation_time / dt)

        # Storage for plotting
        time_history = []
        angle_history = []
        torque_history = []

        for step in range(steps):
            # Compute control signal
            tau = controller.computed_torque_control(
                thetalistd=desired_angles,
                dthetalistd=np.zeros(6),
                ddthetalistd=np.zeros(6),
                thetalist=current_angles,
                dthetalist=current_velocities,
                g=np.array([0, 0, -9.81]),
                dt=dt,
                Kp=Kp, Ki=Ki, Kd=Kd
            )

            # Simulate dynamics (simplified)
            accelerations = dynamics.forward_dynamics(
                current_angles, current_velocities,
                cp.asnumpy(tau), np.array([0, 0, -9.81]),
                np.zeros(6)
            )

            # Update states
            current_velocities += accelerations * dt
            current_angles += current_velocities * dt

            # Store data
            time_history.append(step * dt)
            angle_history.append(current_angles.copy())
            torque_history.append(cp.asnumpy(tau))

        return np.array(time_history), np.array(angle_history), np.array(torque_history)

### Trajectory Tracking Example

    def trajectory_tracking_example(controller, trajectory):
        """
        Example of tracking a predefined trajectory using PD+Feedforward control
        """
        positions = trajectory["positions"]
        velocities = trajectory["velocities"]
        accelerations = trajectory["accelerations"]

        # Control gains
        Kp = np.array([25.0, 20.0, 15.0, 10.0, 8.0, 5.0])
        Kd = np.array([3.0, 2.5, 2.0, 1.5, 1.0, 0.8])

        current_state = np.zeros(6)
        current_velocity = np.zeros(6)

        tracking_errors = []

        for i in range(len(positions)):
            # Compute control with feedforward
            tau = controller.pd_feedforward_control(
                desired_position=positions[i],
                desired_velocity=velocities[i],
                desired_acceleration=accelerations[i],
                current_position=current_state,
                current_velocity=current_velocity,
                Kp=Kp, Kd=Kd,
                g=np.array([0, 0, -9.81]),
                Ftip=np.zeros(6)
            )

            # Calculate tracking error
            error = np.linalg.norm(positions[i] - current_state)
            tracking_errors.append(error)

            # Update state (simplified integration)
            # In practice, you would use your robot's dynamics
            current_state = positions[i]  # Perfect tracking for demo

        return tracking_errors

## Troubleshooting

### Common Issues and Solutions

1. Controller Instability

   - Cause: Gains too high or improper tuning

   - Solution: Use Ziegler-Nichols tuning or reduce gains systematically

2. CuPy Memory Errors

   - Cause: GPU memory exhaustion

   - Solution: Use smaller batch sizes or convert to CPU arrays when needed

3. Numerical Issues

   - Cause: Ill-conditioned matrices or extreme values

   - Solution: Add regularization or use more robust numerical methods

4. Slow Performance

   - Cause: Frequent CPU-GPU transfers

   - Solution: Keep computations on GPU as much as possible

### Best Practices

1. Gain Tuning:

   - Start with conservative gains and increase gradually

   - Use automatic tuning methods when possible

   - Test stability margins before deployment

2. GPU Usage:

   - Convert to CuPy arrays early in the pipeline

   - Minimize data transfers between CPU and GPU

   - Use batch operations when possible

3. Safety:

   - Always enforce joint and torque limits

   - Implement emergency stops

   - Validate control signals before application

4. Performance:

   - Profile your control loop to identify bottlenecks

   - Use appropriate time steps for your application

   - Consider predictive control for better performance

### Error Handling

    try:
        tau = controller.computed_torque_control(...)

        # Check for NaN or infinite values
        if not np.all(np.isfinite(cp.asnumpy(tau))):
            raise ValueError("Control signal contains invalid values")

        # Enforce safety limits
        safe_tau = cp.clip(tau, torque_limits[:, 0], torque_limits[:, 1])

    except Exception as e:
        print(f"Control error: {e}")
        # Implement fallback strategy
        tau = np.zeros(6)  # Safe fallback

## API Reference

## Additional Resources

- Trajectory Planning User Guide - Motion planning and trajectory generation

- Control API Reference - Complete API reference

For issues and questions:

- GitHub Issues: ManipulaPy Issues

- Documentation: ManipulaPy Docs