def get_double_mechanical(m1=1, k1=1, b1=1, m2=1, k2=0, b2=0):
# TODO: determine good default values for m1, k1, b1, m2?
N = 4
sys = LTISystem(
np.c_[ # A
[0, -k1/m1, 0, k1/m2],
[1, -b1/m1, 0, b1/m2],
[0, k1/m1, 0, -(k1+k2)/m2],
[0, b1/m1, 1, -(b1+b2)/m2]
],
np.r_[0, 1/m1, 0, 0], # B
# np.r_[0, 0, 1, 0], # C
)
sys.initial_condition = np.r_[
0.5/k1 if k1 else 0,
0,
0.5/k2 if k2 else 0,
0
]
def ref_func(*args):
if len(args) == 1:
x = np.zeros(N)
else:
x = args[1]
return np.zeros(N)-x
ref = SystemFromCallable(ref_func, N, N)
return sys, ref
def control_systems(request):
ct_sys, ref = request.param
Ac, Bc, Cc = ct_sys.data
Dc = np.zeros((Cc.shape[0], 1))
Q = np.eye(Ac.shape[0])
R = np.eye(Bc.shape[1] if len(Bc.shape) > 1 else 1)
Sc = linalg.solve_continuous_are(Ac, Bc.reshape(-1, 1), Q, R,)
Kc = linalg.solve(R, Bc.T @ Sc).reshape(1, -1)
ct_ctr = LTISystem(Kc)
evals = np.sort(np.abs(
linalg.eig(Ac, left=False, right=False, check_finite=False)
))
dT = 1/(2*evals[-1])
Tsim = (8/np.min(evals[~np.isclose(evals, 0)])
if np.sum(np.isclose(evals[np.nonzero(evals)], 0)) > 0
else 8
)
dt_data = signal.cont2discrete((Ac, Bc.reshape(-1, 1), Cc, Dc), dT)
Ad, Bd, Cd, Dd = dt_data[:-1]
Sd = linalg.solve_discrete_are(Ad, Bd.reshape(-1, 1), Q, R,)
Kd = linalg.solve(Bd.T @ Sd @ Bd + R, Bd.T @ Sd @ Ad)
dt_sys = LTISystem(Ad, Bd, dt=dT)
dt_sys.initial_condition = ct_sys.initial_condition
dt_ctr = LTISystem(Kd, dt=dT)
yield ct_sys, ct_ctr, dt_sys, dt_ctr, ref, Tsim
def test_abc():
for n in range(1, max_n + 1):
for m in range(1, max_m + 1):
for p in range(1, max_p + 1):
A = elem_min + (elem_max - elem_min) * np.random.rand(n, n)
B = elem_min + (elem_max - elem_min) * np.random.rand(n, m)
C = elem_min + (elem_max - elem_min) * np.random.rand(p, n)
x = elem_min + (elem_max - elem_min) * np.random.rand(n)
u = elem_min + (elem_max - elem_min) * np.random.rand(m)
if p != n:
with pytest.raises(AssertionError):
LTISystem(A, B, np.random.rand(n, p))
sys = LTISystem(A, B, C)
assert sys.dim_state == n
assert sys.dim_output == p
assert sys.dim_input == m
npt.assert_allclose(
sys.state_equation_function(n * max_m + m, x, u),
A @ x + B @ u)
npt.assert_allclose(
sys.output_equation_function(n * max_m + m, x), C @ x)
def test_k():
for m in range(1, max_m + 1):
for p in range(1, max_p + 1):
K = elem_min + (elem_max - elem_min) * np.random.rand(p, m)
u = elem_min + (elem_max - elem_min) * np.random.rand(m)
sys = LTISystem(K)
assert sys.dim_state == 0
assert sys.dim_input == m
assert sys.dim_output == p
npt.assert_allclose(sys.output_equation_function(p * max_m + m, u),
K @ u)
def test_mixed_dts(simulation_results):
"""
A DT equivalent that is sampled twice as fast should match original DT
equivalent exactly at t= k*dT under the same inputs.
"""
results, ct_sys, ct_ctr, dt_sys, dt_ctr, ref, Tsim, tspan, intname = \
simulation_results
Ac, Bc, Cc = ct_sys.data
Dc = np.zeros((Cc.shape[0], 1))
dt_unique_t, dt_unique_t_idx = np.unique(
results[0].t, return_index=True
)
discrete_sel = dt_unique_t_idx[
(dt_unique_t < (Tsim*7/8)) & (dt_unique_t % dt_sys.dt == 0)
]
scale_factor = 1/2
Ad, Bd, Cd, Dd, dT = signal.cont2discrete(
(Ac, Bc.reshape(-1, 1), Cc, Dc),
dt_sys.dt*scale_factor
)
dt_sys2 = LTISystem(Ad, Bd, dt=dT)
dt_sys2.initial_condition = ct_sys.initial_condition
intopts = block_diagram.DEFAULT_INTEGRATOR_OPTIONS.copy()
intopts['name'] = intname
bd = BlockDiagram(dt_sys2, ref, dt_ctr)
bd.connect(dt_sys2, ref)
bd.connect(ref, dt_ctr)
bd.connect(dt_ctr, dt_sys2)
res = bd.simulate(tspan, integrator_options=intopts)
mixed_t_discrete_t_equal_idx = np.where(
np.equal(*np.meshgrid(res.t, results[0].t[discrete_sel]))
)[1]
mixed_unique_t, mixed_unique_t_idx_rev = np.unique(
res.t[mixed_t_discrete_t_equal_idx][::-1], return_index=True
)
mixed_unique_t_idx = (len(mixed_t_discrete_t_equal_idx)
- mixed_unique_t_idx_rev - 1)
mixed_sel = mixed_t_discrete_t_equal_idx[mixed_unique_t_idx]
npt.assert_allclose(
res.x[mixed_sel, :], results[0].x[discrete_sel, :],
atol=TEST_ATOL, rtol=TEST_RTOL
)
def get_cart_pendulum(m=1, M=3, L=0.5, g=9.81, pedant=False):
N = 4
sys = LTISystem(
np.c_[ # A
[0, 0, 0, 0], [1, 0, 0, 0],
[0, m * g / M, 0,
(-1)**(pedant) * (m + M) * g / (M * L)], [0, 0, 1, 0]],
np.r_[0, 1 / M, 0, 1 / (M * L)], # B
# np.r_[1, 0, 1, 0] # C
)
sys.initial_condition = np.r_[0, 0, np.pi / 3, 0]
def ref_func(*args):
if len(args) == 1:
x = np.zeros(N)
else:
x = args[1]
return np.zeros(N) - x
ref = SystemFromCallable(ref_func, N, N)
return sys, ref
def get_electromechanical(b=1, R=1, L=1, K=np.pi / 5, M=1):
# TODO: determine good reference and/or initial_condition
# TODO: determine good default values for b, R, L, M
N = 3
sys = LTISystem(
np.c_[ # A
[0, 0, 0], [1, -b / M, -K / L], [0, K / M, -R / L]],
np.r_[0, 0, 1 / L], # B
# np.r_[1, 0, 0], # C
)
sys.initial_condition = np.ones(N)
def ref_func(*args):
if len(args) == 1:
x = np.zeros(N)
else:
x = args[1]
return np.r_[0, 0, 0] - x
ref = SystemFromCallable(ref_func, N, N)
return sys, ref
def get_double_integrator(m=1000, b=50, d=1):
N = 2
sys = LTISystem(
np.c_[[0, 1], [1, -b/m]], # A
np.r_[0, d/m], # B
# np.r_[0, 1], # C
)
def ref_func(*args):
if len(args) == 1:
x = np.zeros(N)
else:
x = args[1]
return np.r_[d/m, 0]-x
ref = SystemFromCallable(ref_func, N, N)
return sys, ref
def test_feedback_equivalent(simulation_results):
# (A-BK) should be exactly same as system A,B under feedback K
results, ct_sys, ct_ctr, dt_sys, dt_ctr, ref, Tsim, tspan, intname = \
simulation_results
intopts = block_diagram.DEFAULT_INTEGRATOR_OPTIONS.copy()
intopts['name'] = intname
dt_equiv_sys = LTISystem(dt_sys.F + dt_sys.G @ dt_ctr.K,
-dt_sys.G @ dt_ctr.K, dt=dt_sys.dt)
dt_equiv_sys.initial_condition = dt_sys.initial_condition
dt_bd = BlockDiagram(dt_equiv_sys, ref)
dt_bd.connect(ref, dt_equiv_sys)
dt_equiv_res = dt_bd.simulate(tspan, integrator_options=intopts)
mixed_t_discrete_t_equal_idx = np.where(
np.equal(*np.meshgrid(dt_equiv_res.t, results[0].t))
)[1]
npt.assert_allclose(
dt_equiv_res.x, results[0].x,
atol=TEST_ATOL, rtol=TEST_RTOL
)
ct_equiv_sys = LTISystem(ct_sys.F + ct_sys.G @ ct_ctr.K,
-ct_sys.G @ ct_ctr.K)
ct_equiv_sys.initial_condition = ct_sys.initial_condition
ct_bd = BlockDiagram(ct_equiv_sys, ref)
ct_bd.connect(ref, ct_equiv_sys)
ct_equiv_res = ct_bd.simulate(tspan, integrator_options=intopts)
unique_t, unique_t_sel = np.unique(ct_equiv_res.t, return_index=True)
ct_res = callable_from_trajectory(
unique_t,
ct_equiv_res.x[unique_t_sel, :]
)
npt.assert_allclose(
ct_res(results[1].t), results[1].x,
atol=TEST_ATOL, rtol=TEST_RTOL
)
def test_ab():
for n in range(1, max_n + 1):
for m in range(1, max_m + 1):
A = elem_min + (elem_max - elem_min) * np.random.rand(n, n)
B = elem_min + (elem_max - elem_min) * np.random.rand(n, m)
x = elem_min + (elem_max - elem_min) * np.random.rand(n)
u = elem_min + (elem_max - elem_min) * np.random.rand(m)
if n != m:
with pytest.raises(AssertionError):
LTISystem(np.random.rand(n, m), B)
with pytest.raises(AssertionError):
LTISystem(A, np.random.rand(m, n))
sys = LTISystem(A, B)
assert sys.dim_state == n
assert sys.dim_output == n
assert sys.dim_input == m
npt.assert_allclose(
sys.state_equation_function(n * max_m + m, x, u),
A @ x + B @ u)
npt.assert_allclose(sys.output_equation_function(n * max_m + m, x),
x)
# construct second order system state and input matrices
m = 1
d = 1
b = 1
k = 1
Ac = np.c_[[0, -k / m], [1, -b / m]]
Bc = np.r_[0, d / m].reshape(-1, 1)
# augment state and input matrices to add integral error state
A_aug = np.hstack((np.zeros((3, 1)), np.vstack((np.r_[1, 0], Ac))))
B_aug = np.hstack((np.vstack((0, Bc)), -np.eye(3, 1)))
# construct system
aug_sys = LTISystem(
A_aug,
B_aug,
)
# construct PID system
Kc = 1
tau_I = 1
tau_D = 1
K = -np.r_[Kc / tau_I, Kc, Kc * tau_D].reshape((1, 3))
pid = LTISystem(K)
# construct reference
ref = SystemFromCallable(lambda *args: np.ones(1), 0, 1)
# create block diagram
BD = BlockDiagram(aug_sys, pid, ref)
BD.connect(aug_sys, pid) # PID requires feedback
(-1)**(pedant) * (m + M) * g / (M * L)], [0, 0, 1, 0]]
Bc = np.r_[0, 1 / M, 0, 1 / (M * L)].reshape(-1, 1)
Cc = np.eye(4)
Dc = np.zeros((4, 1))
ic = np.r_[0, 0, np.pi / 3, 0]
elif use_model == 1:
m = 1
d = 1
b = 1
Ac = np.c_[[0, 1], [1, -b / m]]
Bc = np.r_[0, d / m].reshape(-1, 1)
Cc = np.eye(2)
Dc = np.zeros((2, 1))
ic = np.r_[1, 0]
ct_sys = LTISystem(Ac, Bc, Cc)
ct_sys.initial_condition = ic
n, m = Bc.shape
evals = np.sort(
np.abs(linalg.eig(Ac, left=False, right=False, check_finite=False)))
dT = 1 / (2 * evals[-1])
Tsim = (8 / np.min(evals[~np.isclose(evals[np.nonzero(evals)], 0)])
if np.sum(np.isclose(evals[np.nonzero(evals)], 0)) > 0 else 8)
Ad, Bd, Cd, Dd, dT = signal.cont2discrete((Ac, Bc, Cc, Dc), dT)
dt_sys = LTISystem(Ad, Bd, Cd, dt=dT)
dt_sys.initial_condition = ic
Q = np.eye(Ac.shape[0])
plt.show()
# Plot G components
plt.figure()
plt.plot(sg_sim_res.t, mat_sg_result(sg_sim_res.t)[:, :, -1])
plt.title('forced component of solution to Riccatti differential equation')
plt.legend(['$g_1$', '$g_2$'])
plt.xlabel('$t$, s')
plt.ylabel('$g_{i}(t)$')
plt.show()
# Construct controller from solution to riccati differential algebraic equation
tracking_controller = SystemFromCallable(
lambda t, xx: -Rn**-1 @ Bn.T @ (mat_sg_result(t)[:, :-1] @ xx +
mat_sg_result(t)[:, -1]), 2, 1)
sys = LTISystem(An, Bn) # plant system
sys.initial_condition = np.zeros((2, 1))
# simulate controller and plant
control_BD = BlockDiagram(sys, tracking_controller)
control_BD.connect(sys, tracking_controller)
control_BD.connect(tracking_controller, sys)
control_res = control_BD.simulate(tF)
# plot states and reference
plt.figure()
plt.plot(control_res.t, control_res.x, control_res.t, 2 * control_res.t)
plt.title('reference and state vs. time')
plt.xlabel('$t$, s')
plt.legend([r'$x_1$', r'$x_2$', r'$r_1$'])
plt.ylabel('$x_{i}(t)$')
x0 = np.zeros((3, 1))
u0 = 0
A = sys.state_jacobian_equation_function(t0, x0, u0)
B = sys.input_jacobian_equation_function(t0, x0, u0)
# LQR gain
Q = np.matlib.eye(3)
R = np.matrix([1])
S = linalg.solve_continuous_are(
A,
B,
Q,
R,
)
K = linalg.solve(R, B.T @ S).reshape(1, -1)
ctr_sys = LTISystem(-K)
# Construct block diagram
BD = BlockDiagram(sys, ctr_sys)
BD.connect(sys, ctr_sys)
BD.connect(ctr_sys, sys)
# case 1 - un-recoverable
sys.initial_condition = np.r_[1, 1, 2.25]
result1 = BD.simulate(tF)
plt.figure()
plt.plot(result1.t, result1.y)
plt.legend(legends)
plt.title('controlled system with unstable initial conditions')
plt.xlabel('$t$, s')
plt.tight_layout()
