def test_sdof_pid():
"""Check PID controller outputs"""
tf = 2 # simulation time
npts = 500 # simulation samples
tau = 1E-2 # time constant of derivative filter
# Create system/controller and run simulation
sys = FirstOrder(1)
ctl = PIDController([[3]], [[2]], [[0.6]], dv_tau=tau)
clsys = ctl + sys
sim = clsys.compute_trajectory(0, r=step(1), tf=tf, n=npts)
# error
e = (sim.r - sim.y)[:, 0]
# integral of error
e_int = integrate.cumtrapz(e, sim.t, initial=0)
# filtered derivative of error
e_fd = filtered_deriv(e, sim.t, tau)
# Check controller output
ctl_ref = (e * ctl.KP + e_int * ctl.KI + e_fd * ctl.KD).reshape((sim.n, ))
assert np.allclose(sim.u[:, 0], ctl_ref, atol=0, rtol=1E-2)
def test_check_and_normalize():
# Check normalization of scalars to 2D array
ctl = PIDController(1, 1, 1)
for arr in [ctl.KP, ctl.KI, ctl.KD]:
assert arr.ndim == 2
assert arr.shape == (1, 1)
assert arr[0, 0] == 1
# Check normalization of 1D array to 2D array
vals = [1, 2, 3]
ctl = PIDController(vals, vals, vals)
for arr in [ctl.KP, ctl.KI, ctl.KD]:
assert arr.ndim == 2
assert arr.shape == (1, 3)
assert np.all(vals == arr)
# Check that 2D arrays should be unchanged
vals = np.ones((3, 4)) * 2.3
ctl = PIDController(vals, vals, vals)
for arr in [ctl.KP, ctl.KI, ctl.KD]:
assert arr.ndim == 2
assert arr.shape == vals.shape
assert np.all(vals == arr)
# Check default zero values for KI and KD
for testval in [1, [1, 2, 3], np.ones((8, 10))]:
ctl = PIDController(testval)
for arr in [ctl.KI, ctl.KD]:
assert arr.shape == ctl.KP.shape
assert np.all(arr == np.zeros(ctl.KP.shape))
with pytest.raises(ValueError):
ctl = PIDController(1, KI=np.ones(2))
with pytest.raises(ValueError):
ctl = PIDController(1, KD=np.ones(2))
with pytest.raises(ValueError):
ctl = PIDController(np.ones((2, 3)), KI=np.ones((3, 2)))
with pytest.raises(ValueError):
ctl = PIDController(np.ones((2, 3)), KD=np.ones((3, 2)))
def test_nd_pid():
tf = 2
npts = 500
tau = 1E-2
class DummyManipulator(TwoLinkManipulator):
"""Manipulator with null response to inputs"""
def __init__(self):
super().__init__()
self.p = 2
def B(self, q):
return np.zeros(self.dof)
def h(self, x, u, t):
"""Read joint positions"""
return x[:2]
sys = DummyManipulator()
ctl = PIDController(KP=np.ones((2, 2)),
KI=np.ones((2, 2)),
KD=np.ones((2, 2)),
dv_tau=tau)
clsys = ctl + sys
x0 = [np.pi - 0.5, np.pi / 2, 0, 0]
sim = clsys.compute_trajectory(x0=x0, tf=tf, n=npts)
errors = sim.r - sim.y
e_int = integrate.cumtrapz(errors, sim.t, axis=0, initial=0)
e_der = np.empty(errors.shape)
for j in range(errors.shape[1]):
e_der[:, j] = filtered_deriv(errors[:, j], sim.t, tau=tau)
ctl_ref = errors.dot(ctl.KP.T) + e_int.dot(ctl.KI.T) + e_der.dot(ctl.KD.T)
assert np.allclose(sim.u, ctl_ref, atol=0.05)
# -*- coding: utf-8 -*-
"""
Created on Jun 2 2021
@author: Alex
"""
import numpy as np
from pyro.dynamic import massspringdamper
from pyro.control.linear import PIDController
# Plant
sys = massspringdamper.TwoMass(output_mass=1)
sys.m1 = 1
sys.m2 = 1
sys.k1 = 1
sys.k2 = 1
sys.b1 = 0.5
sys.b2 = 0.5
sys.compute_ABCD()
# LQR controller
kp = 1
ki = 1
kd = 1
tau = 0.1
ctl = PIDController(kp, ki, kd, tau)
ctl.rbar[0] = 2 # reference
# Simulation Closed-Loop Non-linear with LQR controller
cl_sys = ctl + sys
cl_sys.compute_trajectory(30)
cl_sys.plot_trajectory('y')
cl_sys.animate_simulation()
# -*- coding: utf-8 -*-
"""
Created on Jun 2 2021
@author: Alex
"""
import numpy as np
from pyro.dynamic import massspringdamper
from pyro.control.linear import PIDController
# Plant
sys = massspringdamper.TwoMass()
sys.m1 = 2
sys.m2 = 3
sys.k1 = 5
sys.k2 = 2
sys.b1 = 0.1
sys.b2 = 0.2
sys.compute_ABCD()
# LQR controller
kp = 5
ki = 0
kd = 2
tau = 0.1
ctl = PIDController(kp, ki, kd, tau)
# Simulation Closed-Loop Non-linear with LQR controller
cl_sys = ctl + sys
cl_sys.x0[1] = 1
cl_sys.compute_trajectory()
cl_sys.plot_trajectory('xu')
# -*- coding: utf-8 -*-
"""
Created on Jun 2 2021
@author: Alex
"""
from pyro.dynamic import massspringdamper
from pyro.control.linear import PIDController
# Plant
sys = massspringdamper.FloatingThreeMass(output_mass=1)
sys.m1 = 1
sys.m2 = 1
sys.m3 = 1
sys.k2 = 3
sys.k3 = 3
sys.b2 = 1
sys.compute_ABCD()
# PID controller
kp = 2
ki = 0
kd = 0
tau = 0.01
ctl = PIDController(kp, ki, kd, tau)
ctl.rbar[0] = 2 # reference
# Simulation Closed-Loop Non-linear with LQR controller
cl_sys = ctl + sys
cl_sys.compute_trajectory(30)
cl_sys.plot_trajectory('y')
cl_sys.animate_simulation()