Control Systems

Control Systems

Identity

Reference System Usage

You must ground your responses in the provided reference files, treating them as the source of truth for this domain:

• For Creation: Always consult `references/patterns.md`. This file dictates how things should be built. Ignore generic approaches if a specific pattern exists here.
• For Diagnosis: Always consult `references/sharp_edges.md`. This file lists the critical failures and "why" they happen. Use it to explain risks to the user.
• For Review: Always consult `references/validations.md`. This contains the strict rules and constraints. Use it to validate user inputs objectively.

Note: If a user's request conflicts with the guidance in these files, politely correct them using the information provided in the references.

Control Systems

Patterns

Pid Controller

Name

PID Controller Implementation

Description

Classic proportional-integral-derivative control with anti-windup

Pattern

import numpy as np from dataclasses import dataclass from typing import Optional

@dataclass class PIDGains: """PID gains with optional derivative filter.""" kp: float = 1.0 # Proportional gain ki: float = 0.0 # Integral gain kd: float = 0.0 # Derivative gain tau_d: float = 0.1 # Derivative filter time constant

class PIDController: """PID controller with anti-windup and derivative filtering.

Features:

• Integral anti-windup (clamping and back-calculation)
• Derivative on measurement (avoids derivative kick)
• Low-pass filter on derivative term
• Bumpless transfer for gain changes

"""

def __init__(self, gains: PIDGains, dt: float, output_limits: tuple = (-np.inf, np.inf)): self.gains = gains self.dt = dt self.output_min, self.output_max = output_limits

State

self.integral = 0.0 self.prev_measurement = None self.prev_derivative = 0.0 self.prev_output = 0.0

def update(self, setpoint: float, measurement: float) -> float: """Compute control output.

Args: setpoint: Desired value measurement: Current measured value

Returns: Control output (clamped to limits) """ error = setpoint - measurement

Proportional term

p_term = self.gains.kp * error

Integral term with clamping anti-windup

self.integral += error self.dt i_term = self.gains.ki self.integral

Derivative term on measurement (not error)

Avoids derivative kick on setpoint changes

if self.prev_measurement is None: d_term = 0.0 else:

Raw derivative

d_raw = -(measurement - self.prev_measurement) / self.dt

Low-pass filter on derivative

alpha = self.dt / (self.gains.tau_d + self.dt) d_filtered = alpha d_raw + (1 - alpha) self.prev_derivative self.prev_derivative = d_filtered

d_term = self.gains.kd * d_filtered

self.prev_measurement = measurement

Compute output

output_unsat = p_term + i_term + d_term

Clamp output

output = np.clip(output_unsat, self.output_min, self.output_max)

Back-calculation anti-windup

if self.gains.ki != 0: saturation_error = output - output_unsat self.integral += saturation_error / self.gains.ki

self.prev_output = output return output

def reset(self): """Reset controller state.""" self.integral = 0.0 self.prev_measurement = None self.prev_derivative = 0.0

def set_gains(self, gains: PIDGains): """Update gains with bumpless transfer."""

Adjust integral to maintain output continuity

if self.gains.ki != 0 and gains.ki != 0: self.integral *= self.gains.ki / gains.ki self.gains = gains

Ziegler-Nichols tuning helper

def ziegler_nichols_tuning(ku: float, tu: float, controller_type: str = 'PID') -> PIDGains: """Compute PID gains using Ziegler-Nichols method.

Args: ku: Ultimate gain (gain at which oscillation occurs) tu: Ultimate period (period of oscillation) controller_type: 'P', 'PI', or 'PID'

Returns: Tuned PID gains """ if controller_type == 'P': return PIDGains(kp=0.5 ku) elif controller_type == 'PI': return PIDGains(kp=0.45 ku, ki=0.54 ku / tu) else: # PID return PIDGains( kp=0.6 ku, ki=1.2 ku / tu, kd=0.075 ku * tu )

Why

PID is the workhorse of industrial control - simple, robust, well-understood

Cascade Control

Name

Cascade Control

Description

Nested control loops for improved disturbance rejection

Pattern

class CascadeController: """Cascade (nested loop) control structure.

Outer loop: Slower, controls primary variable (position, temperature) Inner loop: Faster, controls secondary variable (velocity, power)

Benefits:

• Better disturbance rejection
• Inner loop handles fast dynamics
• Outer loop handles slow setpoint tracking

"""

def __init__(self, outer_gains: PIDGains, inner_gains: PIDGains, dt_outer: float, dt_inner: float, inner_limits: tuple = (-np.inf, np.inf)): self.outer = PIDController(outer_gains, dt_outer, output_limits=inner_limits) self.inner = PIDController(inner_gains, dt_inner) self.dt_ratio = int(dt_outer / dt_inner)

def update(self, setpoint: float, outer_measurement: float, inner_measurement: float) -> float: """Compute cascaded control output.

Example: Position control setpoint: desired position outer_measurement: actual position inner_measurement: actual velocity output: motor command (torque/current) """

Outer loop runs at slower rate

Output is setpoint for inner loop

inner_setpoint = self.outer.update(setpoint, outer_measurement)

Inner loop runs at faster rate

output = self.inner.update(inner_setpoint, inner_measurement)

return output

Example: Position-Velocity cascade for motor

cascade = CascadeController( outer_gains=PIDGains(kp=10.0, ki=1.0, kd=0.0), # Position loop inner_gains=PIDGains(kp=5.0, ki=10.0, kd=0.0), # Velocity loop dt_outer=0.01, # 100 Hz position loop dt_inner=0.001 # 1 kHz velocity loop )

Why

Cascade control handles systems with nested dynamics (common in motion control)

State Space Control

Name

State-Space Controller

Description

Full-state feedback with observer

Pattern

import numpy as np from scipy import linalg

class StateSpaceController: """Linear state-space controller with Luenberger observer.

System: dx/dt = Ax + Bu y = Cx + Du

Controller: u = -Kx + Kr*r (state feedback with reference scaling) Observer: dx_hat/dt = Ax_hat + Bu + L(y - Cx_hat) """

def __init__(self, A: np.ndarray, B: np.ndarray, C: np.ndarray, D: np.ndarray = None, dt: float = 0.01): self.A = A self.B = B self.C = C self.D = D if D is not None else np.zeros((C.shape[0], B.shape[1])) self.dt = dt

self.n = A.shape[0] # Number of states self.m = B.shape[1] # Number of inputs self.p = C.shape[0] # Number of outputs

Controller and observer gains (must be designed)

self.K = None # State feedback gain self.L = None # Observer gain self.Kr = None # Reference gain

Observer state

self.x_hat = np.zeros((self.n, 1))

def design_lqr(self, Q: np.ndarray, R: np.ndarray): """Design LQR controller gains.

Minimizes: J = integral(x'Qx + u'Ru) dt

Args: Q: State cost matrix (n x n, positive semi-definite) R: Input cost matrix (m x m, positive definite) """

Solve continuous algebraic Riccati equation

P = linalg.solve_continuous_are(self.A, self.B, Q, R) self.K = np.linalg.inv(R) @ self.B.T @ P

Compute reference gain for zero steady-state error

Kr = -inv(C @ inv(A - B @ K) @ B)

Acl = self.A - self.B @ self.K self.Kr = -np.linalg.inv(self.C @ np.linalg.inv(Acl) @ self.B)

return self.K

def design_observer(self, poles: np.ndarray = None, bandwidth_mult: float = 5.0): """Design Luenberger observer via pole placement.

Observer should be faster than controller (typically 2-10x). """ if poles is None:

Place observer poles at 5x controller bandwidth

ctrl_poles = np.linalg.eigvals(self.A - self.B @ self.K) poles = bandwidth_mult * np.real(ctrl_poles)

Pole placement for observer (dual problem)

from scipy.signal import place_poles result = place_poles(self.A.T, self.C.T, poles) self.L = result.gain_matrix.T

return self.L

def update(self, y: np.ndarray, r: np.ndarray) -> np.ndarray: """Compute control output with observer.

Args: y: Measured output r: Reference (setpoint)

Returns: Control input u """

Observer prediction

y_hat = self.C @ self.x_hat

Observer correction

dx_hat = (self.A @ self.x_hat + self.B @ (-self.K @ self.x_hat + self.Kr @ r) + self.L @ (y - y_hat))

Euler integration

self.x_hat = self.x_hat + dx_hat * self.dt

Control law

u = -self.K @ self.x_hat + self.Kr @ r

return u

Example: Mass-spring-damper position control

m, c, k = 1.0, 0.5, 2.0 # Mass, damping, stiffness A = np.array([[0, 1], [-k/m, -c/m]]) B = np.array([[0], [1/m]]) C = np.array([[1, 0]]) # Measure position only

ctrl = StateSpaceController(A, B, C, dt=0.01) ctrl.design_lqr(Q=np.diag([10, 1]), R=np.array([[0.1]])) ctrl.design_observer()

Why

State-space provides optimal multi-input-multi-output control

Model Predictive Control

Name

Model Predictive Control (MPC)

Description

Optimal control with constraints over prediction horizon

Critical

Pattern

import numpy as np from scipy.optimize import minimize from dataclasses import dataclass from typing import Callable, Optional

@dataclass class MPCParams: """MPC tuning parameters.""" horizon: int = 20 # Prediction horizon dt: float = 0.1 # Time step Q: np.ndarray = None # State cost (tracking error) R: np.ndarray = None # Input cost (control effort) Qf: np.ndarray = None # Terminal state cost

class LinearMPC: """Linear MPC with constraints.

Solves: min sum_{k=0}^{N-1} (x_k - r)'Q(x_k - r) + u_k'R u_k + (x_N - r)'Qf(x_N - r) s.t. x_{k+1} = Ad @ x_k + Bd @ u_k u_min <= u_k <= u_max x_min <= x_k <= x_max """

def __init__(self, Ad: np.ndarray, Bd: np.ndarray, params: MPCParams, u_min: np.ndarray = None, u_max: np.ndarray = None, x_min: np.ndarray = None, x_max: np.ndarray = None): self.Ad = Ad self.Bd = Bd self.params = params

self.n = Ad.shape[0] # States self.m = Bd.shape[1] # Inputs self.N = params.horizon

Default costs

if params.Q is None: params.Q = np.eye(self.n) if params.R is None: params.R = 0.1 * np.eye(self.m) if params.Qf is None: params.Qf = params.Q

Constraints

self.u_min = u_min if u_min is not None else -np.inf np.ones(self.m) self.u_max = u_max if u_max is not None else np.inf np.ones(self.m) self.x_min = x_min if x_min is not None else -np.inf np.ones(self.n) self.x_max = x_max if x_max is not None else np.inf np.ones(self.n)

Warm start

self.u_prev = np.zeros((self.N, self.m))

def _predict_trajectory(self, x0: np.ndarray, u_seq: np.ndarray) -> np.ndarray: """Predict state trajectory given initial state and control sequence.""" x_traj = np.zeros((self.N + 1, self.n)) x_traj[0] = x0.flatten()

for k in range(self.N): x_traj[k + 1] = self.Ad @ x_traj[k] + self.Bd @ u_seq[k]

return x_traj

def _cost_function(self, u_flat: np.ndarray, x0: np.ndarray, reference: np.ndarray) -> float: """Compute total cost over horizon.""" u_seq = u_flat.reshape((self.N, self.m)) x_traj = self._predict_trajectory(x0, u_seq)

cost = 0.0 Q, R, Qf = self.params.Q, self.params.R, self.params.Qf

for k in range(self.N):

Stage cost

e = x_traj[k] - reference cost += e @ Q @ e + u_seq[k] @ R @ u_seq[k]

Terminal cost

e = x_traj[self.N] - reference cost += e @ Qf @ e

return cost

def update(self, x0: np.ndarray, reference: np.ndarray) -> np.ndarray: """Solve MPC optimization problem.

Args: x0: Current state reference: Desired state (or trajectory)

Returns: Optimal control input for current time step """ x0 = x0.flatten() reference = reference.flatten()

Initial guess (warm start from previous solution)

u0 = self.u_prev.flatten()

Input bounds

bounds = [] for _ in range(self.N): for j in range(self.m): bounds.append((self.u_min[j], self.u_max[j]))

Solve optimization

result = minimize( self._cost_function, u0, args=(x0, reference), method='SLSQP', bounds=bounds, options={'maxiter': 50, 'disp': False} )

u_opt = result.x.reshape((self.N, self.m))

Warm start for next iteration

self.u_prev[:-1] = u_opt[1:] self.u_prev[-1] = u_opt[-1]

Return first control input

return u_opt[0]

Example: Double integrator with input limits

dt = 0.1 Ad = np.array([[1, dt], [0, 1]]) Bd = np.array([[0.5 dt*2], [dt]])

mpc = LinearMPC( Ad, Bd, MPCParams(horizon=20, dt=dt, Q=np.diag([10, 1]), R=np.array([[0.1]])), u_min=np.array([-1.0]), # Max deceleration u_max=np.array([1.0]) # Max acceleration )

Why

MPC handles constraints and preview, essential for optimal robot motion

Trajectory Tracking

Name

Trajectory Tracking Controller

Description

Follow time-parameterized reference trajectory

Pattern

import numpy as np from typing import Callable, Tuple

class TrajectoryTracker: """Track a time-parameterized reference trajectory.

Uses feedforward + feedback control: u = u_ff(t) + K @ (x_ref(t) - x)

Feedforward comes from desired trajectory dynamics. Feedback corrects for disturbances and model errors. """

def __init__(self, K: np.ndarray, trajectory_func: Callable): """ Args: K: Feedback gain matrix trajectory_func: Function (t) -> (x_ref, u_ff) Returns reference state and feedforward input at time t """ self.K = K self.trajectory_func = trajectory_func self.t = 0.0

def update(self, x: np.ndarray, dt: float) -> Tuple[np.ndarray, np.ndarray]: """Compute tracking control.

Args: x: Current state dt: Time step

Returns: (control_input, tracking_error) """

Get reference at current time

x_ref, u_ff = self.trajectory_func(self.t)

Tracking error

error = x_ref - x

Control: feedforward + feedback

u = u_ff + self.K @ error

self.t += dt

return u, error

Trajectory generation utilities

def minimum_jerk_trajectory(start: float, end: float, duration: float, t: float) -> Tuple[float, float, float]: """Minimum jerk trajectory for smooth motion.

Returns position, velocity, acceleration at time t. """ if t < 0: return start, 0.0, 0.0 if t > duration: return end, 0.0, 0.0

tau = t / duration tau3 = tau 3 tau4 = tau 4 tau5 = tau ** 5

Position

s = 10 tau3 - 15 tau4 + 6 tau5 pos = start + (end - start) s

Velocity

ds = (30 tau2 - 60 tau3 + 30 tau4) / duration vel = (end - start) ds

Acceleration

dds = (60 tau - 180 tau*2 + 120 tau3) / duration*2 acc = (end - start) dds

return pos, vel, acc

def trapezoidal_velocity_profile(start: float, end: float, v_max: float, a_max: float, t: float) -> Tuple[float, float, float]: """Trapezoidal velocity profile (bang-bang with cruise).

Returns position, velocity, acceleration at time t. """ distance = end - start sign = np.sign(distance) distance = abs(distance)

Time to accelerate to v_max

t_acc = v_max / a_max

Distance during acceleration

d_acc = 0.5 a_max t_acc**2

if 2 * d_acc >= distance:

Triangle profile (never reach v_max)

t_acc = np.sqrt(distance / a_max) t_total = 2 * t_acc t_cruise = 0 else:

Trapezoidal profile

d_cruise = distance - 2 d_acc t_cruise = d_cruise / v_max t_total = 2 t_acc + t_cruise

if t < 0: return start, 0.0, 0.0 elif t < t_acc:

Acceleration phase

pos = start + sign 0.5 a_max t2 vel = sign a_max t acc = sign a_max elif t < t_acc + t_cruise:

Cruise phase

pos = start + sign (d_acc + v_max (t - t_acc)) vel = sign * v_max acc = 0.0 elif t < t_total:

Deceleration phase

t_dec = t - t_acc - t_cruise pos = start + sign (d_acc + v_max t_cruise + v_max t_dec - 0.5 a_max t_dec2) vel = sign (v_max - a_max t_dec) acc = -sign a_max else: return end, 0.0, 0.0

return pos, vel, acc

Why

Smooth trajectory tracking is essential for robotics motion

Anti-Patterns

Derivative Kick

Name

Derivative on Error (Derivative Kick)

Problem

Derivative of error causes spikes on setpoint change

Solution

Use derivative on measurement: d/dt(measurement), not d/dt(error)

Integral Windup

Name

Integral Windup

Problem

Integral accumulates during saturation, causes overshoot

Solution

Implement anti-windup: clamping, back-calculation, or conditional integration

Tuning At One Point

Name

Tuning at Single Operating Point

Problem

Controller works at one speed/load, fails at others

Solution

Use gain scheduling or adaptive control for varying conditions

Ignoring Actuator Limits

Name

Ignoring Actuator Saturation

Problem

Controller commands exceed physical limits

Solution

Include constraints in control design (MPC) or saturate output

Control Systems - Sharp Edges

Wrong PID Tuning Order Causes Oscillation

Id

pid-tuning-order

Severity

critical

Summary

Tuning I before P, or D before I, leads to instability

Symptoms

• System oscillates at any gain setting
• Increasing gain makes things worse
• Controller never stabilizes

Why

PID terms interact. Tuning in wrong order creates instability:

• P alone: Proportional response, steady-state error
• I without P: Phase lag, guaranteed oscillation
• D without P: Amplifies noise, no steady tracking

Correct order: P first (for response), then I (eliminate error), finally D (reduce overshoot).

Starting with I or having too much I relative to P causes phase lag that leads to oscillation.

Gotcha

Common mistake: Starting with integral

pid = PIDController(kp=0, ki=10, kd=0) # Will oscillate!

Or: Too much I relative to P

pid = PIDController(kp=1, ki=100, kd=0) # Integral dominates, oscillates

Solution

Systematic tuning procedure

1. Set I = 0, D = 0, increase P until oscillation

pid = PIDController(kp=0, ki=0, kd=0)

Find critical gain (Ku) where oscillation starts

for kp in np.linspace(0, 100, 100): pid.gains.kp = kp

Test and check for sustained oscillation

2. Use Ziegler-Nichols or similar method

ku = 50 # Critical gain tu = 0.5 # Oscillation period

PID: kp = 0.6ku, ki = 1.2ku/tu, kd = 0.075kutu

gains = ziegler_nichols_tuning(ku, tu, 'PID')

3. Fine-tune from there

Reduce I if overshoot, increase D if oscillating

Continuous PID Formulas Don't Work at Low Sample Rates

Id

discrete-time-pid

Severity

high

Summary

Textbook PID formulas assume continuous time, fail when sampled slowly

Symptoms

• Controller works at 1kHz, fails at 100Hz
• Derivative term is noisy or wrong
• Integral accumulates incorrectly

Why

Continuous PID: u = Kpe + Kiintegral(e) + Kd*de/dt

Discrete implementation matters:

• Integral: Euler vs trapezoidal vs exact
• Derivative: Forward vs backward vs filtered

At low sample rates, these differences are significant. Bilinear (Tustin) transform preserves stability better than simple Euler integration.

Gotcha

Simple Euler integration

self.integral += error * dt derivative = (error - self.prev_error) / dt # Noisy!

Works at 1kHz, fails at 50Hz

Solution

Use proper discrete-time formulation

1. Trapezoidal integration (more accurate)

self.integral += 0.5 (error + self.prev_error) dt

2. Filtered derivative (reduces noise)

First-order filter: d_filt = alpha d_raw + (1-alpha) d_prev

tau_d = 0.1 # Filter time constant alpha = dt / (tau_d + dt) d_raw = (error - self.prev_error) / dt d_filtered = alpha d_raw + (1 - alpha) self.prev_derivative

3. Or use bilinear transform for entire controller

s -> 2/T * (z-1)/(z+1)

4. Derivative on measurement, not error

d_raw = -(measurement - self.prev_measurement) / dt

MPC Model Mismatch Causes Poor Performance

Id

mpc-model-mismatch

Severity

critical

Summary

MPC relies on accurate model; errors cause suboptimal or unstable control

Symptoms

• MPC works in simulation, fails on real system
• Controller is sluggish or oscillatory
• Constraints violated despite MPC

Why

MPC optimizes based on predicted future states. If the model is wrong, predictions are wrong, and the "optimal" control is actually suboptimal.

Common model errors:

• Wrong time constants
• Unmodeled friction/backlash
• Linearization at wrong operating point
• Ignored coupling between axes

MPC is more sensitive to model errors than PID because it plans ahead based on the model.

Gotcha

MPC with nominal model

Ad_nominal = np.array([[1, 0.1], [0, 1]]) # Assumes no friction

mpc = LinearMPC(Ad_nominal, Bd, params)

Real system has friction - model predicts wrong trajectory

MPC optimizes for wrong predictions

Solution

1. System identification

from scipy.optimize import curve_fit

def system_response(t, tau, K): return K * (1 - np.exp(-t/tau))

Fit model to step response data

params, _ = curve_fit(system_response, t_data, y_data)

2. Add disturbance estimation

class DisturbanceObserver: """Estimate and compensate for model mismatch.""" def __init__(self, model): self.model = model self.d_hat = 0 # Estimated disturbance

def update(self, x, x_predicted, L=0.5):

Disturbance = difference between prediction and reality

self.d_hat = L self.d_hat + (1-L) (x - x_predicted) return self.d_hat

3. Robust MPC with uncertainty bounds

Tighten constraints to account for model error

4. Adaptive MPC (update model online)

Control Loop Sample Rate Too Low

Id

sample-rate-control

Severity

high

Summary

Slow sample rate causes phase lag and instability

Symptoms

• System oscillates at high frequencies
• Controller can't track fast references
• Adding D gain makes oscillation worse

Why

Rule of thumb: sample rate should be 10-20x the system's fastest dynamics (bandwidth).

For motor control:

• Current loop: 10-20 kHz
• Velocity loop: 1-10 kHz
• Position loop: 100-1000 Hz

Too slow sampling adds phase lag, reducing stability margins. It also aliases high-frequency disturbances.

Gotcha

Motor position control at 10 Hz

while True: time.sleep(0.1) # 10 Hz - WAY too slow for motors u = pid.update(setpoint, position) motor.set_command(u)

Motor dynamics are ~100Hz, need at least 1kHz control

Solution

1. Use hardware timer for precise control loop

def timer_isr(): """1kHz control interrupt.""" global position, setpoint u = pid.update(setpoint, position) motor.set_pwm(u)

setup_timer_interrupt(frequency=1000, callback=timer_isr)

2. Separate fast and slow loops

Fast: current/velocity (hardware timer, 1-10kHz)

Slow: position/trajectory (software, 100-500Hz)

3. For ROS2: Use realtime-safe callback groups

from rclpy.callback_groups import RealtimeCallbackGroup

self.control_timer = self.create_timer( 0.001, # 1ms = 1kHz self.control_callback, callback_group=RealtimeCallbackGroup() )

Ignoring Actuator Saturation

Id

actuator-saturation

Severity

high

Summary

Controller commands exceed physical limits, causes windup and instability

Symptoms

• Large overshoot on step response
• Slow recovery after large errors
• Oscillation after hitting limits

Why

Every actuator has limits:

• Motors: max current, max voltage
• Servos: max position, max velocity
• Pumps: max flow rate

If controller outputs exceed these, the actuator saturates. The controller keeps integrating error, causing windup. When error reduces, the accumulated integral causes overshoot.

MPC handles this naturally via constraints. PID needs explicit anti-windup.

Gotcha

PID without output limits

pid = PIDController(gains, dt) # No limits!

u = pid.update(setpoint, measurement) # Could be 1000V motor.set_voltage(u) # Motor saturates at 24V

Integral keeps growing during saturation

Huge overshoot when setpoint reached

Solution

1. Clamp output and implement anti-windup

pid = PIDController( gains, dt, output_limits=(-24.0, 24.0) # Voltage limits )

2. Back-calculation anti-windup

output_unsat = p + i + d output = np.clip(output_unsat, -24, 24) if ki != 0: anti_windup = (output - output_unsat) / ki integral += anti_windup

3. Conditional integration

if not saturated: integral += error * dt

Don't integrate while saturated

4. Use MPC with explicit constraints

mpc = LinearMPC( A, B, params, u_min=np.array([-24.0]), u_max=np.array([24.0]) )

Setpoint Jump Causes Actuator Stress

Id

setpoint-jump

Severity

medium

Summary

Step changes in setpoint cause aggressive control action

Symptoms

• Motor jerks on setpoint change
• Mechanical stress and wear
• Overshoot on step response

Why

Derivative term amplifies sudden changes. Step change in setpoint = infinite derivative = kick.

Even without D, large step = large error = aggressive P action. This stresses mechanical systems and can cause vibration.

Solution

1. Derivative on measurement (not error)

derivative = -(measurement - prev_measurement) / dt

Not: derivative = (error - prev_error) / dt

2. Setpoint ramping/filtering

class SetpointFilter: def __init__(self, rate_limit, dt): self.rate = rate_limit self.dt = dt self.filtered = 0

def update(self, setpoint): delta = setpoint - self.filtered max_delta = self.rate * self.dt delta = np.clip(delta, -max_delta, max_delta) self.filtered += delta return self.filtered

3. Use trajectory generator

Instead of step: use minimum-jerk or trapezoidal profile

4. Setpoint weighting (P acts on weighted setpoint)

u = Kp (b setpoint - measurement) + ...

b < 1 reduces kick

Control Systems - Validations

Derivative Computed on Error

Id

derivative-on-error

Severity

warning

Type

regex

Pattern

• error\s-\sprev_error|prev_error\s-\serror
• self\.error\s-\sself\.prev_error
• d_term.error.prev

Message

Computing derivative on error causes derivative kick on setpoint change.

Fix Action

Use derivative on measurement: -(measurement - prev_measurement) / dt

Applies To

• */.py

PID Without Anti-Windup

Id

no-anti-windup

Severity

warning

Type

regex

Pattern

• integral\s\+=.error.*dt(?![\s\S]{0,200}(clip|clamp|windup|limit|saturate))

Message

Integral accumulation without anti-windup causes overshoot when saturated.

Fix Action

Add clamping or back-calculation anti-windup

Applies To

• */.py

PID Without Output Limits

Id

no-output-limits

Severity

warning

Type

regex

Pattern

• class.PID.:(?![\s\S]{0,500}(output_max|u_max|limit|clip|clamp))

Message

PID controller should have output limits matching actuator constraints.

Fix Action

Add output_limits parameter and clamp control output

Applies To

• */.py

Unfiltered Derivative Term

Id

raw-derivative

Severity

info

Type

regex

Pattern

• /\s*dt(?![\s\S]{0,50}(filter|alpha|tau|lpf))
• d_term\s=.-.prev.(?!filter)

Message

Unfiltered derivative amplifies high-frequency noise.

Fix Action

Add low-pass filter: d_filt = alpha d_raw + (1-alpha) d_prev

Applies To

• */.py

Simple Euler Integration for Integral Term

Id

euler-integration

Severity

info

Type

regex

Pattern

• integral\s\+=\serror\s\\s*dt

Message

Simple Euler integration can accumulate error. Consider trapezoidal integration.

Fix Action

Use: integral += 0.5 (error + prev_error) dt for better accuracy

Applies To

• */.py

Control Loop with sleep() Call

Id

slow-control-loop

Severity

warning

Type

regex

Pattern

• time\.sleep\(0\.[1-9]|time\.sleep\([1-9]
• rospy\.sleep\(0\.[1-9]

Message

Control loop may be too slow. Use hardware timer for consistent timing.

Fix Action

Use hardware timer interrupt or ROS2 timer for precise control loop

Applies To

• */.py

MPC Without Input Constraints

Id

mpc-no-constraints

Severity

warning

Type

regex

Pattern

• class.MPC.:(?![\s\S]{0,800}(u_min|u_max|bounds|constraint))

Message

MPC without constraints loses main advantage. Add actuator limits.

Fix Action

Add u_min/u_max constraints matching physical actuator limits

Applies To

• */.py

Hardcoded PID Gains Without Comments

Id

hardcoded-gains

Severity

info

Type

regex

Pattern

• kp\s=\s\d+\.?\d\s(?!#)
• ki\s=\s\d+\.?\d\s(?!#)
• PIDGains\(kp=\d+.\)\s$

Message

Document tuning rationale for PID gains, or load from config.

Fix Action

Add comment explaining tuning method, or use parameter server

Applies To

• */.py

Step Setpoint Without Trajectory Generation

Id

no-trajectory-filter

Severity

info

Type

regex

Pattern

• setpoint\s=\starget(?![\s\S]{0,100}(filter|ramp|trajectory|profile))

Message

Sudden setpoint changes stress mechanical systems.

Fix Action

Use trajectory generator (minimum-jerk, trapezoidal) for smooth motion

Applies To

• */.py

Float Equality in Control Logic

Id

floating-point-comparison

Severity

warning

Type

regex

Pattern

• if.error\s==\s*0
• if.position\s==\s*setpoint

Message

Floating-point equality rarely holds. Use tolerance-based comparison.

Fix Action

Use: if abs(error) < tolerance or np.isclose()

Applies To

• */.py

LQR/MPC Without Explicit Jacobian

Id

jacobian-missing

Severity

info

Type

regex

Pattern

• lqr|LQR|design_lqr(?![\s\S]{0,300}jacobian)

Message

Ensure linearization Jacobian is computed correctly for nonlinear systems.

Fix Action

Verify Jacobian analytically or use automatic differentiation

Applies To

• */.py