114 lines
3.9 KiB
Python
114 lines
3.9 KiB
Python
"""Minimal PID controller + angle helper.
|
|
|
|
Adapted from ``Robot/go2_dog_behavior/simple_controllers.py``: keeps the
|
|
anti-windup integral clamp, the deadband-zeroes-integral-and-derivative
|
|
behaviour, and output saturation. The output-deadband compensation feature of
|
|
the original is intentionally dropped -- the visual servo does its own
|
|
min-forward gating, so it is not needed here.
|
|
|
|
Pure module: only depends on :mod:`math`, so it imports and unit-tests with no
|
|
heavy/optional dependencies.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
from typing import Optional, Tuple
|
|
|
|
|
|
def normalize_angle(angle: float) -> float:
|
|
"""Wrap ``angle`` (radians) into ``[-pi, pi]``.
|
|
|
|
Args:
|
|
angle: Angle in radians, any magnitude.
|
|
|
|
Returns:
|
|
The equivalent angle in ``[-pi, pi]`` via ``atan2(sin, cos)``.
|
|
"""
|
|
return math.atan2(math.sin(angle), math.cos(angle))
|
|
|
|
|
|
class PIDController:
|
|
"""A scalar PID controller with anti-windup, deadband, and output clamp.
|
|
|
|
The controller is stateful (it accumulates an integral and remembers the
|
|
previous error for the derivative term); call :meth:`reset` to clear that
|
|
state, e.g. when re-acquiring a target.
|
|
"""
|
|
|
|
def __init__(
|
|
self,
|
|
kp: float,
|
|
ki: float = 0.0,
|
|
kd: float = 0.0,
|
|
output_limits: Tuple[Optional[float], Optional[float]] = (None, None),
|
|
integral_limit: Optional[float] = None,
|
|
deadband: float = 0.0,
|
|
) -> None:
|
|
"""Configure the gains and limits.
|
|
|
|
Args:
|
|
kp: Proportional gain.
|
|
ki: Integral gain.
|
|
kd: Derivative gain.
|
|
output_limits: ``(min_output, max_output)``; use ``None`` on either
|
|
side to leave that bound unclamped.
|
|
integral_limit: Symmetric clamp on the accumulated integral
|
|
(anti-windup); ``None`` disables it.
|
|
deadband: When ``abs(error)`` is below this, the integral and
|
|
derivative terms are zeroed (the proportional term still acts).
|
|
"""
|
|
self.kp = kp
|
|
self.ki = ki
|
|
self.kd = kd
|
|
self.min_output, self.max_output = output_limits
|
|
self.integral_limit = integral_limit
|
|
self.deadband = deadband
|
|
self.integral: float = 0.0
|
|
self.prev_error: float = 0.0
|
|
|
|
def update(self, error: float, dt: float) -> float:
|
|
"""Advance the controller one step and return the control output.
|
|
|
|
Args:
|
|
error: The current error (setpoint - measurement, or any signed
|
|
error convention the caller prefers).
|
|
dt: Timestep in seconds. Non-positive ``dt`` is guarded: the
|
|
derivative term is taken as zero and the integral is left
|
|
unchanged for that step.
|
|
|
|
Returns:
|
|
The (optionally clamped) PID output.
|
|
"""
|
|
# Integrate (skip when dt is non-positive to avoid bogus accumulation).
|
|
if dt > 0:
|
|
self.integral += error * dt
|
|
if self.integral_limit is not None:
|
|
self.integral = max(
|
|
-self.integral_limit, min(self.integral, self.integral_limit)
|
|
)
|
|
derivative = (error - self.prev_error) / dt
|
|
else:
|
|
derivative = 0.0
|
|
|
|
# Inside the deadband, suppress integral build-up and derivative kick.
|
|
if abs(error) < self.deadband:
|
|
self.integral = 0.0
|
|
derivative = 0.0
|
|
|
|
output = self.kp * error + self.ki * self.integral + self.kd * derivative
|
|
|
|
# Saturate to the configured output limits.
|
|
if self.max_output is not None:
|
|
output = min(self.max_output, output)
|
|
if self.min_output is not None:
|
|
output = max(self.min_output, output)
|
|
|
|
self.prev_error = error
|
|
return output
|
|
|
|
def reset(self) -> None:
|
|
"""Clear the integral accumulator and the remembered previous error."""
|
|
self.integral = 0.0
|
|
self.prev_error = 0.0
|