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_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_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
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])
R = np.eye(Bc.shape[1] if len(Bc.shape) > 1 else 1)
# 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)$')
plt.show()
