========================== 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. .. contents:: Table of Contents :depth: 3 :local: :backlinks: none 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: .. math:: 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) Where: - :math:`q,\dot{q},\ddot{q}` – actual joint angles, velocities, accelerations - :math:`q_d,\dot{q}_d,\ddot{q}_d` – desired joint trajectory - :math:`M(q)` – mass-inertia matrix - :math:`C(q,\dot{q})` – Coriolis/centrifugal matrix - :math:`G(q)` – gravity vector - :math:`K_p,K_i,K_d` – proportional, integral, derivative gains PID Control ----------- A simpler joint-space PID law (no dynamics compensation): .. math:: \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: .. math:: \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: .. math:: \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} Where :math:`\hat{\theta}` is the estimated parameter vector and :math:`\gamma` the adaptation gain. Robust Control -------------- Adds a disturbance-rejection term to a computed-torque core: .. math:: \tau = M(q)\,\ddot{q} + C(q,\dot{q})\,\dot{q} + G(q) + J(q)^T\,F_{ext} + \alpha\,\hat{d} With :math:`\hat{d}` the estimated disturbance and :math:`\alpha` its gain. Kalman-Filter State Estimation ------------------------------- Predict-update loop for joint state :math:`x = [q;\dot{q}]`: 1. **Prediction** .. math:: \hat{x}^- = F\,\hat{x} + B\,u, \quad P^- = F\,P\,F^T + Q 2. **Update** .. math:: 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^- Where :math:`Q,R` are process/measurement covariances. These formulas correspond exactly to the methods in :class:`~ManipulaPy.control.ManipulatorController`. Installation and Setup ======================= Prerequisites ------------- .. code-block:: bash pip install cupy-cuda11x # or cupy-cuda12x for CUDA 12 pip install numpy matplotlib Basic Import ------------ .. code-block:: python import numpy as np import cupy as cp from ManipulaPy.control import ManipulatorController from ManipulaPy.dynamics import ManipulatorDynamics Quick Start =========== Initialize Controller --------------------- .. code-block:: python # 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. .. automethod:: ManipulaPy.control.ManipulatorController.pid_control :no-index: .. note:: Both ``pid_control`` and ``computed_torque_control`` accept an optional ``i_clamp`` keyword argument (default: ``None``, behavior identical to v1.3.1) that caps the integral error term to prevent windup during sustained tracking error. When provided, it must be a **positive scalar** — every joint's integral term is independently clamped to ``[-i_clamp, +i_clamp]``. Passing an array raises ``ValueError`` ("i_clamp must be a scalar"), as do negative, zero, ``nan``, or otherwise non-finite values. **Example**: .. code-block:: python # 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. .. automethod:: ManipulaPy.control.ManipulatorController.pd_control :no-index: **Example**: .. code-block:: python 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. .. automethod:: ManipulaPy.control.ManipulatorController.computed_torque_control :no-index: **Example**: .. code-block:: python # 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. .. automethod:: ManipulaPy.control.ManipulatorController.feedforward_control :no-index: **Example**: .. code-block:: python 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. .. automethod:: ManipulaPy.control.ManipulatorController.pd_feedforward_control :no-index: **Example**: .. code-block:: python 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. .. automethod:: ManipulaPy.control.ManipulatorController.robust_control :no-index: **Example**: .. code-block:: python 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. .. automethod:: ManipulaPy.control.ManipulatorController.adaptive_control :no-index: **Example**: .. code-block:: python 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 ----------------------------------- .. automethod:: ManipulaPy.control.ManipulatorController.enforce_limits :no-index: **Example**: .. code-block:: python # 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 ------------------------------- .. automethod:: ManipulaPy.control.ManipulatorController.kalman_filter_predict :no-index: .. automethod:: ManipulaPy.control.ManipulatorController.kalman_filter_update :no-index: .. automethod:: ManipulaPy.control.ManipulatorController.kalman_filter_control :no-index: **Example**: .. code-block:: python # 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 ---------------------- .. automethod:: ManipulaPy.control.ManipulatorController.ziegler_nichols_tuning :no-index: .. note:: Since v1.3.2, passing ``Tu=0`` to the PI or PID branches raises ``ValueError`` with a descriptive message instead of silently producing ``inf`` gains. The P-only branch ignores ``Tu``, so ``Tu=0`` remains valid there. **Example**: .. code-block:: python # 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 ------------------------------- .. automethod:: ManipulaPy.control.ManipulatorController.find_ultimate_gain_and_period :no-index: .. automethod:: ManipulaPy.control.ManipulatorController.tune_controller :no-index: **Example**: .. code-block:: python # 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 ------------------------------ .. automethod:: ManipulaPy.control.ManipulatorController.plot_steady_state_response :no-index: **Example**: .. code-block:: python # 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: .. automethod:: ManipulaPy.control.ManipulatorController.calculate_rise_time :no-index: .. automethod:: ManipulaPy.control.ManipulatorController.calculate_percent_overshoot :no-index: .. automethod:: ManipulaPy.control.ManipulatorController.calculate_settling_time :no-index: .. note:: **v1.3.2 semantics:** ``calculate_settling_time`` returns the *first* time the response enters and stays within the tolerance band around the setpoint — using ``abs(set_point) * tolerance`` as the half-width so the band is correct for negative setpoints. The previous implementation used the signed ``set_point * tolerance`` threshold (wrong sign for negative targets) and returned the index of the last in-band sample rather than the first settled time (off-by-one for oscillatory responses). Callers upgrading from v1.3.1 may see different numeric return values for negative setpoints or oscillatory traces. .. automethod:: ManipulaPy.control.ManipulatorController.calculate_steady_state_error :no-index: **Example**: .. code-block:: python # 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 ------------------- .. automethod:: ManipulaPy.control.ManipulatorController.joint_space_control :no-index: Cartesian Space Control ----------------------- .. automethod:: ManipulaPy.control.ManipulatorController.cartesian_space_control :no-index: Examples ======== Complete Control Loop Example ------------------------------ .. code-block:: python 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 ---------------------------- .. code-block:: python 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 -------------- .. code-block:: python 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 ============= .. autoclass:: ManipulaPy.control.ManipulatorController :no-members: :no-index: :undoc-members: :show-inheritance: Additional Resources ==================== - :doc:`Trajectory_Planning` - Motion planning and trajectory generation - :doc:`../api/control` - Complete API reference For issues and questions: - GitHub Issues: `ManipulaPy Issues `_ - Documentation: `ManipulaPy Docs `_