118 lines
4.5 KiB
Python
118 lines
4.5 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 eye
|
|
from numpy.linalg import eig
|
|
from numpy import vstack
|
|
|
|
# Package under test
|
|
from pyrate.act.control import AntiWindupLqr
|
|
|
|
|
|
class TestAntiWindupLqr(TestCase):
|
|
|
|
"""Test for correct runtime behaviour in pyrate.act.control.anti_windup_lqr."""
|
|
|
|
# In this context, we reproduce a common controller notation
|
|
# pylint: disable=invalid-name
|
|
|
|
def setUp(self) -> None:
|
|
"""Setup the LQR specification for testing."""
|
|
|
|
# Model specification
|
|
self.A = array([[0, 1], [0, 0]])
|
|
self.B = array([0, 1])[:, None]
|
|
self.C = array([1, 0])[None, :]
|
|
self.max_control = array([1.0])
|
|
self.dt = 0.5
|
|
|
|
# Time discrete model
|
|
self.Ad = self.dt * eye(2)
|
|
self.Bd = self.B + self.A @ self.B * self.dt
|
|
|
|
# Cost matrix specification
|
|
self.Q = eye(3)
|
|
self.R = array([[1.0]])
|
|
|
|
# State and target
|
|
# Target is already reached in this example
|
|
self.wrong_state = vstack([1.0, 0.0])
|
|
self.state_small_negative = vstack([-0.001, 0.0])
|
|
self.state = vstack([0.0, 0.0])
|
|
self.desired = vstack([0.0])
|
|
self.desired1 = vstack([1.0])
|
|
|
|
def test_lqr_design(self) -> None:
|
|
"""Assert stable controller dynamics"""
|
|
|
|
# Initialize controller
|
|
lqr = AntiWindupLqr(
|
|
self.A, self.B, self.C, self.Q, self.R, self.max_control, self.dt, keep_trace=True
|
|
)
|
|
# check continous time eigen_values
|
|
eigen_values, _ = eig(lqr.A - lqr.B @ lqr.K)
|
|
assert all(ev.real < 0 for ev in eigen_values), "instable controller"
|
|
|
|
def test_anti_windup(self) -> None:
|
|
"""Assert control signal in allowed range, LQR responsive by limited integral part."""
|
|
|
|
# Initialize controller
|
|
lqr = AntiWindupLqr(
|
|
self.A, self.B, self.C, self.Q, self.R, self.max_control, self.dt, keep_trace=True
|
|
)
|
|
|
|
# Execute a few control steps
|
|
for _ in range(10):
|
|
control_signal = lqr.control(desired=self.desired, state=self.wrong_state)
|
|
assert abs(control_signal) <= self.max_control, "control limits are not applied"
|
|
|
|
assert control_signal == -self.max_control, f"control limits not reached {control_signal}"
|
|
|
|
# test stationary summed error
|
|
summed_error = lqr.summed_error
|
|
control_signal = lqr.control(desired=self.desired, state=self.wrong_state)
|
|
assert abs(lqr.summed_error - summed_error) < 1e-6, "summed error changes in saturation"
|
|
|
|
# test reactiveness
|
|
control_signal_back = lqr.control(desired=self.desired, state=self.state_small_negative)
|
|
assert abs(control_signal_back) < self.max_control, "anti wind up is not working"
|
|
|
|
def test_process_tracking(self) -> None:
|
|
"""Assert useful controller behavior and that a pandas.DataFrame with process data is created."""
|
|
|
|
# Initialize controller
|
|
lqr = AntiWindupLqr(
|
|
self.A, self.B, self.C, self.Q, self.R, self.max_control, self.dt, keep_trace=True
|
|
)
|
|
|
|
# Check zero control
|
|
control_signal = lqr.control(desired=self.desired, state=self.state)
|
|
assert control_signal == 0, "Control signal not zero when desired value 0 reached"
|
|
control_signal = lqr.control(desired=self.desired1, state=self.wrong_state)
|
|
assert control_signal == 0, "Control signal not zero when desired value 0 reached"
|
|
lqr.reset()
|
|
|
|
# Execute a few control steps
|
|
state = self.wrong_state.copy()
|
|
initial_error = abs(self.C @ state - self.desired)
|
|
for _ in range(10):
|
|
control_signal = lqr.control(desired=self.desired, state=state)
|
|
state = self.Ad.dot(state) + self.Bd.dot(control_signal)
|
|
error = abs(self.C @ state - self.desired)
|
|
assert error < initial_error or error == 0, "Error exceeds initial - instable controller?"
|
|
|
|
# Assert correct process tracing with LQR controller
|
|
assert lqr.process is not None, "LQR did not keep trace of process"
|
|
assert len(lqr.process.index) == 10, "LQR has not traced enough steps"
|
|
|
|
# Reset controller
|
|
lqr.reset()
|
|
|
|
# Assert correct reset to initial state
|
|
assert lqr.summed_error == 0.0, "Integral did not reset summed error properly"
|
|
assert len(lqr.process.index) == 0, "LQR has not dropped process trace properly"
|