89 lines
3.3 KiB
Python
89 lines
3.3 KiB
Python
"""This module asserts correct runtime behaviour of the pyrate.act.pid module."""
|
|
|
|
# Test environment
|
|
from unittest import TestCase
|
|
|
|
# Mathematics
|
|
from numpy import array
|
|
from numpy import Inf
|
|
from numpy.linalg import norm
|
|
from numpy import vstack
|
|
|
|
# Package under test
|
|
from pyrate.act.control import AntiWindupPid
|
|
|
|
|
|
class TestAntiWindupPid(TestCase):
|
|
|
|
"""Test for correct runtime behaviour in pyrate.act.control.anti_windup_pid."""
|
|
|
|
# In this context, we reproduce a common PID notation
|
|
# pylint: disable=invalid-name
|
|
|
|
def setUp(self) -> None:
|
|
"""Set up the PID specification for testing."""
|
|
|
|
# PID specification
|
|
self.P = array([[1.0]])
|
|
self.I = array([[1.0]]) # noqa: 741
|
|
self.D = array([[1.0]])
|
|
self.max_control = 1.0
|
|
self.dt = 0.5
|
|
|
|
# State and target
|
|
# Target is already reached in this example
|
|
self.state = vstack([0.0])
|
|
self.state_large = vstack([0.5])
|
|
self.state_small_neg = vstack([-0.01])
|
|
self.state_derivative = vstack([0.0])
|
|
self.desired = vstack([0.0])
|
|
|
|
def test_anti_windup(self) -> None:
|
|
"""Assert control signal in allowed range, PID responsive by limited integral part."""
|
|
# Initialize PID controller
|
|
pid = AntiWindupPid(self.P, self.I, self.D, self.max_control, self.dt, keep_trace=True)
|
|
|
|
# Execute a few control steps
|
|
for _ in range(10):
|
|
control_signal = pid.control(
|
|
desired=self.desired, state=self.state_large, state_derivative=self.state_derivative
|
|
)
|
|
assert abs(control_signal) <= self.max_control, "control limits are not applied"
|
|
assert abs(control_signal) == self.max_control, "control limits not reached"
|
|
|
|
# test reactiveness
|
|
control_signal_back = pid.control(
|
|
desired=self.desired, state=self.state_small_neg, state_derivative=self.state_derivative
|
|
)
|
|
assert abs(control_signal_back) < self.max_control + 1e-4, "anti wind up is not working"
|
|
|
|
def test_process_tracking(self) -> None:
|
|
"""Assert that a pandas.DataFrame with process data is created."""
|
|
|
|
# Initialize PID controller
|
|
pid = AntiWindupPid(self.P, self.I, self.D, self.max_control, self.dt, keep_trace=True)
|
|
|
|
# Execute a few control steps
|
|
# Execute a few control steps
|
|
state = self.state.copy()
|
|
previous_error = Inf
|
|
for _ in range(10):
|
|
control_signal = pid.control(
|
|
desired=self.desired, state=self.state, state_derivative=self.state_derivative
|
|
)
|
|
state += control_signal
|
|
error = norm(state - self.desired).item() # Convert from numpy scalar to Python float
|
|
assert error < previous_error or error == 0, "Error did not decrease"
|
|
previous_error = error
|
|
|
|
# Assert correct process tracing with PID controller
|
|
assert pid.process is not None, "PID did not keep trace of process"
|
|
assert len(pid.process.index) == 10, "PID has not traced enough steps"
|
|
|
|
# Reset PID
|
|
pid.reset()
|
|
|
|
# Assert correct reset to initial state
|
|
assert pid.summed_error == 0.0, "PID did not reset summed error properly"
|
|
assert len(pid.process.index) == 0, "PID has not dropped process trace properly"
|