def testConvertMIMO(self):
"""Test state space to transfer function conversion.
Do a MIMO conversion and make sure that it is processed
correctly both with and without slycot
Example from issue gh-120, jgoppert
"""
# Set up a 1x3 transfer function (should always work)
tsys = tf([[[-235, 1.146e4],
[-235, 1.146E4],
[-235, 1.146E4, 0]]],
[[[1, 48.78, 0],
[1, 48.78, 0, 0],
[0.008, 1.39, 48.78]]])
# Convert to state space and look for an error
if (not slycot_check()):
with pytest.raises(TypeError):
tf2ss(tsys)
else:
ssys = tf2ss(tsys)
assert ssys.B.shape[1] == 3
assert ssys.C.shape[0] == 1
def test_xferfcn_ndarray_precedence(op, tf, arr):
# Apply the operator to the transfer function and array
ss = ct.tf2ss(tf)
result = op(ss, arr)
assert isinstance(result, ct.StateSpace)
# Apply the operator to the array and transfer function
ss = ct.tf2ss(tf)
result = op(arr, ss)
assert isinstance(result, ct.StateSpace)
def ActuatorModel(bw, delay=(0, 1)):
# Inputs: ['cmd']
# Outputs: ['pos']
sysNom = control.tf2ss(control.tf(1, [1 / bw, 1]))
delayPade = control.pade(delay[0], n=delay[1])
sysDelay = control.tf2ss(control.tf(delayPade[0], delayPade[1]))
sys = sysDelay * sysNom
return sys
def test_tf2ss_nonproper(self):
"""Unit tests for non-proper transfer functions"""
# Easy case: input 2 to output 1 is 's'
num = [ [[0], [1, 0]], [[1], [0]] ]
den1 = [ [[1], [1]], [[1,4], [1]] ]
with pytest.raises(ValueError):
tf2ss(tf(num, den1))
# Trickier case (make sure that leading zeros in den are handled)
num = [ [[0], [1, 0]], [[1], [0]] ]
den1 = [ [[1], [0, 1]], [[1,4], [1]] ]
with pytest.raises(ValueError):
tf2ss(tf(num, den1))
def from_TF(tf: _ctrl.TransferFunction) -> "Plant":
"""
Creates an instance of `Plant` from transfer function. Note
that `C2` becomes 0 .
Parameters
----------
tf : control.TransferFunction
Transfer function from input u to output z.
Returns
-------
Plant
Notes
-----
An instance of `Plant` represents a plant
P given by
P : { x(t+1) = A x(t) + B u(t)
{ z(t) = C1 x(t)
{ y(t) = C2 x(t)
But if you use this method, `C2` becomes `0`.
"""
ss = _ctrl.tf2ss(tf)
ret = Plant(
A=matrix(ss.A),
B=matrix(ss.B),
C1=matrix(ss.C),
C2=matrix(zeros(ss.C.shape)),
)
ret.tf1 = tf
ret.tf2 = tf*0
return ret
def make_statespace(self):
jm = self.parameters["DC-Motor"]["Jm"]
bm = self.parameters["DC-Motor"]["Bm"]
kme = self.parameters["DC-Motor"]["Kme"]
kmt = self.parameters["DC-Motor"]["Kmt"]
rm = self.parameters["DC-Motor"]["Rm"]
lm = self.parameters["DC-Motor"]["Lm"]
kdm = self.parameters["DC-Motor"]["Kdm"]
kpm = self.parameters["DC-Motor"]["Kpm"]
kim = self.parameters["DC-Motor"]["Kim"]
nm = self.parameters["DC-Motor"]["Nm"]
dc = control.TransferFunction(
[0, kmt], [jm * lm, bm * lm + jm * rm, bm * rm + kme * kmt])
pidm = control.TransferFunction(
[kpm + kdm * nm, kpm * nm + kim, kim * nm], [1, nm, 0])
ii = control.TransferFunction([1], [1, 0, 0])
agv = ii * control.feedback(dc * pidm, sign=-1)
# Laplace --> Z
agvz = control.sample_system(agv, lib.pt, method='zoh')
# Transferfunction --> StateSpace
ss = control.tf2ss(agvz)
lib.set_statespace(ss)
def cssBlk(pin, pout, sys, X0=[]):
"""
Continous state space block
Call: cssBlk(pin,pout, sys,X0)
Parameters
----------
pin : connected input ports
pout: connected output ports
sys: Discrete system in SS form
X0: Initial conditions
Returns
-------
blk : RCPblk
"""
if isinstance(sys, TransferFunction):
sys = tf2ss(sys)
nin = size(pin)
ni = shape(sys.B)[1]
if (nin != ni):
raise ValueError("Block Robi have %i inputs: received %i input ports" %
(nin, ni))
no = shape(sys.C)[0]
nout = size(pout)
if (no != nout):
raise ValueError("Block have %i outputs: received %i output ports" %
(nout, no))
a = reshape(sys.A, (1, size(sys.A)), 'C')
b = reshape(sys.B, (1, size(sys.B)), 'C')
c = reshape(sys.C, (1, size(sys.C)), 'C')
d = reshape(sys.D, (1, size(sys.D)), 'C')
nx = shape(sys.A)[0]
if (size(X0) == nx):
X0 = reshape(X0, (1, size(X0)), 'C')
else:
X0 = mat(zeros((1, nx)))
indA = 1
indB = indA + nx * nx
indC = indB + nx * ni
indD = indC + nx * no
indX = indD + ni * no
intPar = [nx, ni, no, indA, indB, indC, indD, indX]
realPar = hstack((mat([0.0]), a, b, c, d, X0))
if d.any() == True:
uy = 1
else:
uy = 0
blk = RCPblk('css', pin, pout, [nx, 0], uy, realPar, intPar)
return blk
def sim_setup_theta():
num = num_theta()
den = den_long()
sys = tf2ss(tf(num, den))
n_axis = 1
task = [5, 1, 2]
return sys, n_axis, task
def myfunc(variant: int):
"""Generate test systems. """
A = np.array((0, 0, 0, 1, 0, -0.9215, 0, 1, -0.738)).reshape((3, 3))
B = np.array((1 + 0.1 * variant, 0, 0)).reshape((3, 1))
C = np.array((0, 0.151, -0.6732)).reshape((1, 3))
D = np.zeros((1, 1))
sys1 = StateSpace(A, B, C, D)
sys2 = tf2ss(ss2tf(sys1))
As, Z = schur(A)
Bs = Z.T @ B
Cs = C @ Z
Ds = D
# print(Bs)
sys3 = StateSpace(As, Bs, Cs, Ds)
Ds[0, 0] = 0.3
sys4 = StateSpace(As, Bs, Cs, Ds)
Ds[0, 0] = 0
Ab = np.zeros((4, 4))
Ab[:3, :3] = A
Bb = np.zeros((4, 1))
Bb[:3, :] = B
Cb = np.zeros((1, 4))
Cb[:, :3] = C
sys5 = StateSpace(Ab, Bb, Cb, D)
# sys5.A = Ab
# sys5.B = Bb
# sys5.C = Cb
return locals()
def testTf2ssStaticSiso(self):
"""Regression: tf2ss for SISO static gain"""
gsiso = tf2ss(tf(23, 46))
assert 0 == gsiso.nstates
assert 1 == gsiso.ninputs
assert 1 == gsiso.noutputs
np.testing.assert_allclose([[0.5]], gsiso.D)
def __init__(self, H, dt):
sys = control.tf2ss(control.c2d(H, dt))
self.x = np.zeros((sys.A.shape[0], 1))
self.A = sys.A
self.B = sys.B
self.C = sys.C
self.D = sys.D
self.dt = sys.dt
def LQRControl(plant, Q, R):
plantStateSpace = control.tf2ss(plant)
A, B, C, D = control.ssdata(plantStateSpace)
print(A, B, C, D)
K, P, E = control.lqr(plantStateSpace, Q, R)
output = control.ss(A - B * K, B * K, C, D)
return control.step_response(output, T)
def sim_setup_phi():
num = num_phi()
den = den_lat_dir()
sys = tf2ss(tf(num, den))
n_axis = 0
task = [5, 2, 4]
t_task = [2, 5, 7, 10, 15] # start_1, end_1, start_2, end_2, finish
return sys, n_axis, task, t_task
def sim_setup_theta():
num = num_theta()
den = den_long()
sys = tf2ss(tf(num, den))
n_axis = 1
task = [5, 1, 2]
t_task = [2, 5, 7, 10, 15] # start_1, end_1, start_2, end_2, finish
return sys, n_axis, task, t_task
def test_tf2ss_robustness(self):
"""Unit test to make sure that tf2ss is working correctly. gh-240"""
num = [ [[0], [1]], [[1], [0]] ]
den1 = [ [[1], [1,1]], [[1,4], [1]] ]
sys1tf = tf(num, den1)
sys1ss = tf2ss(sys1tf)
# slight perturbation
den2 = [ [[1], [1e-10, 1, 1]], [[1,4], [1]] ]
sys2tf = tf(num, den2)
sys2ss = tf2ss(sys2tf)
# Make sure that the poles match for StateSpace and TransferFunction
np.testing.assert_array_almost_equal(np.sort(sys1tf.pole()),
np.sort(sys1ss.pole()))
np.testing.assert_array_almost_equal(np.sort(sys2tf.pole()),
np.sort(sys2ss.pole()))
def testTf2ssStaticSiso(self):
"""Regression: tf2ss for SISO static gain"""
import control
gsiso = control.tf2ss(control.tf(23, 46))
self.assertEqual(0, gsiso.states)
self.assertEqual(1, gsiso.inputs)
self.assertEqual(1, gsiso.outputs)
# in all cases ratios are exactly representable, so assert_array_equal is fine
np.testing.assert_array_equal([[0.5]], gsiso.D)
def myfunc3(variant: int):
s = TransferFunction.s
tf = (1 + 0.5 * s) / (s**3 + 3 * s**2 + 2 * s + 17)
sysx = tf2ss(tf)
A = str(sysx.A.tolist())
B = str(sysx.B.tolist())
C = str(sysx.C.tolist())
D = str(sysx.D.tolist())
return locals()
def testTf2ssStaticSiso(self):
"""Regression: tf2ss for SISO static gain"""
import control
gsiso = control.tf2ss(control.tf(23, 46))
self.assertEqual(0, gsiso.states)
self.assertEqual(1, gsiso.inputs)
self.assertEqual(1, gsiso.outputs)
# in all cases ratios are exactly representable, so assert_array_equal is fine
np.testing.assert_array_equal([[0.5]], gsiso.D)
def SensorModel(bw, delay=(0, 1)):
# Inputs: ['meas', 'dist']
# Outputs: ['sens']
sysNom = control.tf2ss(control.tf(1, [1 / bw, 1]))
delayPade = control.pade(delay[0], n=delay[1])
sysDelay = control.tf2ss(control.tf(delayPade[0], delayPade[1]))
sysDist = control.tf2ss(control.tf(1, 1))
sys = control.append(sysDelay * sysNom, sysDist)
sys.C = sys.C[0, :] + sys.C[1, :]
sys.D = sys.D[0, :] + sys.D[1, :]
sys.outputs = 1
return sys
def testTf2ssStaticSiso(self):
"""Regression: tf2ss for SISO static gain"""
gsiso = tf2ss(tf(23, 46))
assert 0 == gsiso.nstates
assert 1 == gsiso.ninputs
assert 1 == gsiso.noutputs
# in all cases ratios are exactly representable, so assert_array_equal
# is fine
np.testing.assert_array_equal([[0.5]], gsiso.D)
def output(self, act):
g = control.tf([0.05, 0], [-0.6, 1])
print(g)
sys = control.tf2ss(g)
print(sys)
time_interval = np.linspace(0, 1, 100)
return control.forced_response(sys, time_interval, act)
def PID2(Kp=1, Ki=0.0, Kd=0, b=1, c=1, Tf=0, dt=None):
# Inputs: ['ref', 'sens']
# Outputs: ['cmd']
sysR = control.tf2ss(
control.tf([Kp * b * Tf + Kd * c, Kp * b + Ki * Tf, Ki], [Tf, 1, 0]))
sysY = control.tf2ss(
control.tf([Kp * Tf + Kd, Kp + Ki * Tf, Ki], [Tf, 1, 0]))
sys = control.append(sysR, sysY)
sys.C = sys.C[0, :] - sys.C[1, :]
sys.D = sys.D[0, :] - sys.D[1, :]
sys.outputs = 1
if dt is not None:
sys = control.c2d(sys, dt)
return sys
def test_issiso_mimo(self):
# MIMO transfer function
sys = tf([[[-1, 41], [1]], [[1, 2], [3, 4]]],
[[[1, 10], [1, 20]], [[1, 30], [1, 40]]])
assert not issiso(sys)
assert not issiso(sys, strict=True)
# MIMO state space system
sys = tf2ss(sys)
assert not issiso(sys)
assert not issiso(sys, strict=True)
def testTf2ssStaticMimo(self):
"""Regression: tf2ss for MIMO static gain"""
import control
# 2x3 TFM
gmimo = control.tf2ss(control.tf([[ [23], [3], [5] ], [ [-1], [0.125], [101.3] ]],
[[ [46], [0.1], [80] ], [ [2], [-0.1], [1] ]]))
self.assertEqual(0, gmimo.states)
self.assertEqual(3, gmimo.inputs)
self.assertEqual(2, gmimo.outputs)
d = np.matrix([[0.5, 30, 0.0625], [-0.5, -1.25, 101.3]])
np.testing.assert_array_equal(d, gmimo.D)
def _construct_rotor_speed_filter(self):
cutoff_frequency = 5 # cut-off frequency
low_pass_transfer_function = ct.tf([0, cutoff_frequency],
[1, cutoff_frequency])
low_pass_state_space = ct.tf2ss(low_pass_transfer_function)
low_pass_state_space_discrete = ct.c2d(low_pass_state_space,
self._sampling_time, 'tustin')
beta_num = 0.0195 # notch parameter
beta_den = 0.125 # notch parameter
notch_frequency = 2 * np.pi * 0.62
notch_transfer_function = ct.tf(
[1, 2 * beta_num * notch_frequency, notch_frequency**2],
[1, 2 * beta_den * notch_frequency, notch_frequency**2])
notch_state_space = ct.tf2ss(notch_transfer_function)
notch_state_space_discrete = ct.c2d(notch_state_space,
self._sampling_time, 'tustin')
combined_filter_transfer_function = low_pass_state_space_discrete * notch_state_space_discrete
return combined_filter_transfer_function
def testTf2ssStaticMimo(self):
"""Regression: tf2ss for MIMO static gain"""
# 2x3 TFM
gmimo = tf2ss(tf(
[[ [23], [3], [5] ], [ [-1], [0.125], [101.3] ]],
[[ [46], [0.1], [80] ], [ [2], [-0.1], [1] ]]))
assert 0 == gmimo.nstates
assert 3 == gmimo.ninputs
assert 2 == gmimo.noutputs
d = np.array([[0.5, 30, 0.0625], [-0.5, -1.25, 101.3]])
np.testing.assert_allclose(d, gmimo.D)
def pid_design(G,
K_guess,
d_tc,
verbose=False,
use_P=True,
use_I=True,
use_D=True):
# type: (control.tf, np.array, float, bool, bool, bool, bool) -> (np.array, control.tf, control.tf)
"""
:param G: transfer function
:param K_guess: gain matrix guess
:param d_tc: time constant for derivative
:param verbose: show debug output
:param use_P: use p gain in design
:param use_I: use i gain in design
:param use_D: use d gain in design
:return: (K, G_comp, Gc_comp)
K: gain matrix
G_comp: open loop compensated plant
Gc_comp: closed loop compensated plant
"""
# compensator transfer function
H = []
if use_P:
H += [control.tf(1, 1)]
if use_I:
H += [control.tf((1), (1, 0))]
if use_D:
H += [control.tf((1, 0), (d_tc, 1))]
H = np.array([H]).T
H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])]
for j in range(H.shape[1])]
H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])]
for j in range(H.shape[1])]
H = control.tf(H_num, H_den)
# print('G', G)
# print('H', H)
ss_open = control.tf2ss(G * H)
if verbose:
print('optimizing controller')
K = lqr_ofb_design(K_guess, ss_open, verbose)
if verbose:
print('done')
# print('K', K)
# print('H', H)
G_comp = control.series(G, H * K)
Gc_comp = control.feedback(G_comp, 1)
return K, G_comp, Gc_comp
def PID2Exc(Kp=1, Ki=0, Kd=0, b=1, c=1, Tf=0, dt=None):
# Inputs: ['ref', 'sens', 'exc']
# Outputs: ['cmd', 'ff', 'fb', 'exc']
sysR = control.tf2ss(
control.tf([Kp * b * Tf + Kd * c, Kp * b + Ki * Tf, Ki], [Tf, 1, 0]))
sysY = control.tf2ss(
control.tf([Kp * Tf + Kd, Kp + Ki * Tf, Ki], [Tf, 1, 0]))
sysX = control.tf2ss(control.tf(1, 1)) # Excitation Input
sys = control.append(sysR, sysY, sysX)
sys.C = np.concatenate((sys.C[0, :] - sys.C[1, :] + sys.C[2, :], sys.C))
sys.D = np.concatenate((sys.D[0, :] - sys.D[1, :] + sys.D[2, :], sys.D))
sys.outputs = 4
if dt is not None:
sys = control.c2d(sys, dt)
return sys
def test_tf2ss_robustness(self):
"""Unit test to make sure that tf2ss is working correctly.
Source: https://github.com/python-control/python-control/issues/240
"""
import control
num = [ [[0], [1]], [[1], [0]] ]
den1 = [ [[1], [1,1]], [[1,4], [1]] ]
sys1tf = control.tf(num, den1)
sys1ss = control.tf2ss(sys1tf)
# slight perturbation
den2 = [ [[1], [1e-10, 1, 1]], [[1,4], [1]] ]
sys2tf = control.tf(num, den2)
sys2ss = control.tf2ss(sys2tf)
# Make sure that the poles match for StateSpace and TransferFunction
np.testing.assert_array_almost_equal(np.sort(sys1tf.pole()),
np.sort(sys1ss.pole()))
np.testing.assert_array_almost_equal(np.sort(sys2tf.pole()),
np.sort(sys2ss.pole()))
def get_T(tf_list, N=200, T=None):
""" Get Time vector """
if T is None:
t_max = 0
for tf in tf_list:
ss = ctl.tf2ss(tf)
T_temp = _default_response_times(ss.A, N)
t_max_temp = T_temp[-1]
if t_max_temp > t_max:
t_max = t_max_temp
T = T_temp
return T
def test_tf2ss_robustness(self):
"""Unit test to make sure that tf2ss is working correctly.
Source: https://github.com/python-control/python-control/issues/240
"""
import control
num = [[[0], [1]], [[1], [0]]]
den1 = [[[1], [1, 1]], [[1, 4], [1]]]
sys1tf = control.tf(num, den1)
sys1ss = control.tf2ss(sys1tf)
# slight perturbation
den2 = [[[1], [1e-10, 1, 1]], [[1, 4], [1]]]
sys2tf = control.tf(num, den2)
sys2ss = control.tf2ss(sys2tf)
# Make sure that the poles match for StateSpace and TransferFunction
np.testing.assert_array_almost_equal(np.sort(sys1tf.pole()),
np.sort(sys1ss.pole()))
np.testing.assert_array_almost_equal(np.sort(sys2tf.pole()),
np.sort(sys2ss.pole()))
def testTf2ssStaticMimo(self):
"""Regression: tf2ss for MIMO static gain"""
import control
# 2x3 TFM
gmimo = control.tf2ss(
control.tf([[[23], [3], [5]], [[-1], [0.125], [101.3]]],
[[[46], [0.1], [80]], [[2], [-0.1], [1]]]))
self.assertEqual(0, gmimo.states)
self.assertEqual(3, gmimo.inputs)
self.assertEqual(2, gmimo.outputs)
d = np.matrix([[0.5, 30, 0.0625], [-0.5, -1.25, 101.3]])
np.testing.assert_array_equal(d, gmimo.D)
def get_PI_controller(delta_seconds):
'''
Effects: create a discrete state-space PI controller
'''
num_pi = [KP, KI] # numerator of the PI transfer function (KP*s + KI)
den_pi = [1.0, 0.01*KI/KP] # denominator of PI transfer function (s + 0.01*KI/KP)
sys = control.tf(num_pi,den_pi) # get transfer function for PI controller (since the denominator has a small term 0.01*KI/KP, it is actually a lag-compensator)
sys = control.sample_system(sys, delta_seconds) # discretize the transfer function (from s-domain which is continuous to z-domain)
#since our simulation is discrete
sys = control.tf2ss(sys) # transform transfer function into state space.
return sys
def pid_design(G, K_guess, d_tc, verbose=False, use_P=True, use_I=True, use_D=True):
"""
:param G: transfer function
:param K_guess: gain matrix guess
:param d_tc: time constant for derivative
:param use_P: use p gain in design
:param use_I: use i gain in design
:param use_D: use d gain in design
:return: (K, G_comp, Gc_comp)
K: gain matrix
G_comp: open loop compensated plant
Gc_comp: closed loop compensated plant
"""
# compensator transfer function
H = []
if use_P:
H += [control.tf(1, 1)]
if use_I:
H += [control.tf((1), (1, 0))]
if use_D:
H += [control.tf((1, 0), (d_tc, 1))]
H = np.array([H]).T
H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])]
H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])]
H = control.tf(H_num, H_den)
# print('G', G)
# print('H', H)
ss_open = control.tf2ss(G*H)
if verbose:
print('optimizing controller')
K = lqr_ofb_design(K_guess, ss_open, verbose)
if verbose:
print('done')
# print('K', K)
# print('H', H)
G_comp = control.series(G, H*K)
Gc_comp = control.feedback(G_comp, 1)
return K, G_comp, Gc_comp
# doubleint.py - double integrator example
# RMM, 10 Nov 2012
#
# This example shows how to compute a trajectory for a very simple double
# integrator system. Mainly useful to show the simplest type of trajectory
# generation computation.
import numpy as np
import matplotlib.pyplot as plt
import control as ctrl # control system toolbox
import control.trajgen as tg # trajectory generation toolbox
# Define a double integrator system
sys1 = ctrl.tf2ss(ctrl.tf([1], [1, 0, 0]))
sysf = tg.LinearFlatSystem(sys1)
# Set the initial and final conditions
x0 = (0, 0);
xf = (1, 3);
# Find a trajectory
systraj = tg.point_to_point(sysf, x0, xf, 1)
# Plot the trajectory
t = np.linspace(0, 1, 100)
xd, ud = systraj.eval(t)
plt.figure(1); plt.clf()
plt.plot(t, xd[:,0], 'b-', t, xd[:,1], 'g-', t, ud[:,0], 'r--')
plt.legend(('x1', 'x2', 'u'))
plt.show()