def __connect_input__(self, input_signal=None, base_system=None, block_diagram=None):
"""
Connects an input signal to a SystemBase object in a new simupy.BlockDiagram or in an existing simupy.BlockDiagram.
"""
if base_system is None:
base_system = self
elif not isinstance(base_system, SystemBase):
error_text = '[SystemBase.__connect_input__] The system should be an SystemBase instance.'
raise TypeError(error_text)
if input_signal is None:
if block_diagram is None:
BD = BlockDiagram(base_system.system)
elif not base_system.system in set(block_diagram.systems):
BD = block_diagram
BD.add_system(base_system.system)
else:
if block_diagram is None:
BD = BlockDiagram(input_signal.system, base_system.system)
else:
BD = block_diagram
BD.add_system(input_signal.system)
if not base_system.system in set(block_diagram.systems):
BD.add_system(base_system.system)
BD.connect(input_signal.system, base_system.system)
return BD
trimmed_KEAS = planet.output_equation_function(
0, trimmed_flight_condition)[planet.V_T_idx] * np.sqrt(
planet.output_equation_function(
0, trimmed_flight_condition)[planet.rho_idx] /
rho_0) * knots_per_mps
planet.initial_condition = trimmed_flight_condition
gnc_block = systems.SystemFromCallable(
get_gnc_function(
*opt_ctrl,
keasCmd=trimmed_KEAS,
), dim_feedback, 4)
BD = BlockDiagram(planet, F16_vehicle, gnc_block)
BD.connect(planet, F16_vehicle, inputs=np.arange(planet.dim_output))
BD.connect(F16_vehicle, planet, inputs=np.arange(F16_vehicle.dim_output))
BD.connect(gnc_block,
F16_vehicle,
inputs=np.arange(planet.dim_output, planet.dim_output + 4))
BD.connect(planet, gnc_block, outputs=planet_output_for_gnc_select)
with benchmark() as b:
res = BD.simulate(180, integrator_options=int_opts)
b.tfinal = res.t[-1]
plot_nesc_comparisons(res, '15')
plot_F16_controls(res, '15', y_idx_offset=0)
def xy_for_north_pole_ground_track(lat, long):
def create_block_diagram(self, forward_systems:list=None, backward_systems:list=None):
"""
Create a closed loop block diagram with negative feedback. The loop contains a list of SystemBase objects in the forward path and ControllerBase objects in the backward path.
Parameters
-----------
forward_systems : list, optional (at least one system should be present in the loop)
A list of SystemBase objects. All input and output dimensions should match.
backward_systems: list, optional (at least one system should be present in the loop)
A list of ControllerBase objects. All input and output dimensions should match.
Returns
--------
BD : a simupy's BlockDiagram object
contains the configuration of the closed-loop.
indices : dict
information on the ranges of the states and outputs in the output vectors of a simulation dataset.
"""
if (forward_systems is None):
if (self.system is None):
error_text = "[ClosedLoop.create_block_diagram] Both the forward_systems argument and the ClosedLoop.system variable are empty. Please provide a forward_system."
assert AssertionError(error_text)
else:
forward_systems = [self.system]
if (backward_systems is None):
if (self.system is None):
error_text = "[ClosedLoop.create_block_diagram] Both the backward_systems argument and the ClosedLoop.controller variable are empty. Please provide a backward_system."
assert AssertionError(error_text)
else:
backward_systems = [self.controller]
BD = BlockDiagram()
# Order of adding systems is important. The negative feedback_block needs to be added before the backward systems. This can be seen from simupy.block_diagram.BlockDiagram.output_equation_function(). Possibly only for stateless systems important. #TODO: check whether there is a problem with list of backward systems.
output_startidx_process = 0
output_endidx_process = -1
state_startidx_process = 0
state_endidx_process = -1
if (len(forward_systems) is not 0):
for forward_system in forward_systems:
forward_sys = forward_system.system
BD.add_system(forward_sys)
output_endidx_process += forward_sys.dim_output
state_endidx_process += forward_sys.dim_state
output_endidx_process += 1
state_endidx_process += 1
output_startidx_controller = output_endidx_process
output_endidx_controller = output_startidx_controller
state_startidx_controller = state_endidx_process
state_endidx_controller = state_startidx_controller
if (len(backward_systems) is not 0):
negative_feedback = gain_block(-1, backward_systems[-1].system.dim_output)
BD.add_system(negative_feedback)
output_startidx_controller += negative_feedback.dim_output
output_endidx_controller = output_startidx_controller
for backward_system in backward_systems:
backward_sys = backward_system.system
BD.add_system(backward_sys)
output_endidx_controller += backward_sys.dim_output
state_endidx_controller += backward_sys.dim_state
else:
negative_feedback = gain_block(-1, forward_systems[-1].system.dim_output)
BD.add_system(negative_feedback)
for i in range(len(forward_systems)):
if (i == len(forward_systems) - 1):
BD.connect(forward_systems[i].system, backward_systems[0].system)
else:
BD.connect(forward_systems[i].system, forward_systems[i + 1].system)
if (len(backward_systems) == 0):
BD.add_system(negative_feedback)
BD.connect(forward_systems[-1].system, negative_feedback)
BD.connect(negative_feedback, forward_systems[0].system)
else:
for j in range(len(backward_systems)):
if (j == len(backward_systems) - 1):
BD.connect(backward_systems[j].system, negative_feedback)
BD.connect(negative_feedback, forward_systems[0].system)
else:
BD.connect(backward_systems[j].system, backward_systems[j + 1].system)
indices = {
'process': {
'output': [output_endidx_process] if output_startidx_process == output_endidx_process\
else [output_startidx_process, output_endidx_process],
'state': None if state_endidx_process is 0\
else[state_endidx_process] if state_startidx_process == state_endidx_process\
else [state_startidx_process, state_endidx_process]
},
'controller': {
'output': [output_endidx_controller] if output_startidx_controller == output_endidx_controller\
else [output_startidx_controller, output_endidx_controller],
'state': None if state_endidx_controller == state_endidx_process\
else[state_endidx_controller] if state_startidx_controller == state_endidx_controller\
else [state_startidx_controller, state_endidx_controller]
}
}
return BD, indices
def latOffsetOutputEquation(t, x):
return x * ft_per_m
latOffsetBlock = systems.DynamicalSystem(
state_equation_function=latOffsetStateEquation,
output_equation_function=latOffsetOutputEquation,
dim_state=1,
dim_input=3,
dim_output=1)
baseChiCmdOutput = np.array([spec_ic_args['psi'] * 180 / np.pi])
baseChiCmdBlock = systems.SystemFromCallable(lambda *args: baseChiCmdOutput, 0,
1)
BD = BlockDiagram(planet, F16_vehicle, controller_block, keasCmdBlock,
altCmdBlock, latOffsetBlock, baseChiCmdBlock)
BD.connect(planet, F16_vehicle, inputs=np.arange(planet.dim_output))
BD.connect(F16_vehicle, planet, inputs=np.arange(F16_vehicle.dim_output))
BD.connect(controller_block,
F16_vehicle,
inputs=np.arange(planet.dim_output, planet.dim_output + 4))
BD.connect(planet,
controller_block,
outputs=controller_feedback_indices,
inputs=np.arange(dim_feedback))
BD.connect(keasCmdBlock, controller_block, inputs=[dim_feedback + 0])
BD.connect(altCmdBlock, controller_block, inputs=[dim_feedback + 1])
BD.connect(latOffsetBlock, controller_block, inputs=[dim_feedback + 2])
BD.connect(baseChiCmdBlock, controller_block, inputs=[dim_feedback + 3])
Iyz = 0.0*kg_per_slug/(ft_per_m**2) #slug-ft2 Izx = 0.0*kg_per_slug/(ft_per_m**2) #slug-ft2 m = 0.155404754*kg_per_slug #slug x = 0. y = 0. z = 0. S_A = 0.22222/(ft_per_m**2) b_l = 1/(3*ft_per_m) c_l = 2/(3*ft_per_m) a_l = b_l aero_model = simupy_flight.get_constant_aero(Cp_b=-1.0, Cq_b=-1.0, Cr_b=-1.0) vehicle = simupy_flight.Vehicle(base_aero_coeffs=aero_model, m=m, I_xx=Ixx, I_yy=Iyy, I_zz=Izz, I_xy=Ixy, I_yz=Iyz, I_xz=Izx, x_com=x, y_com=y, z_com=z, x_mrc=x, y_mrc=y, z_mrc=z, S_A=S_A, a_l=a_l, b_l=b_l, c_l=c_l, d_l=0.,) BD = BlockDiagram(planet, vehicle) BD.connect(planet, vehicle, inputs=np.arange(planet.dim_output)) BD.connect(vehicle, planet, inputs=np.arange(vehicle.dim_output)) lat_ic = 0.*np.pi/180 long_ic = 0.*np.pi/180 h_ic = 30_000/ft_per_m V_N_ic = 0. V_E_ic = 0. V_D_ic = 0. psi_ic = 0.*np.pi/180 theta_ic = 0.*np.pi/180 phi_ic = 0.*np.pi/180 omega_X_ic = 10.*np.pi/180 omega_Y_ic = 20.*np.pi/180 omega_Z_ic = 30.*np.pi/180
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
BD.connect(pid, aug_sys, inputs=[0]) # PID output to system control input
BD.connect(ref, aug_sys,
inputs=[1]) # reference output to system command input
res = BD.simulate(10) # simulate
# plot
plt.figure()
plt.plot(res.t, res.y[:, 0], label=r'$\int x$')
plt.plot(res.t, res.y[:, 1], label='$x$')
plt.plot(res.t, res.y[:, 2], label=r'$\dot{x}$')
plt.plot(res.t, res.y[:, 3], label='$u$')
plt.plot(res.t, res.y[:, 4], label='$x_c$')
plt.legend()
plt.ylabel('position, degrees')
## define systems
x, v, u = dynamicsymbols('x v u')
l, m = sp.symbols('l m')
parameters = {l: 1, m: 1}
inertia = DynamicalSystem(state_equation=r_[v, u / (m * l**2)],
state=r_[x, v],
input_=u,
constants_values=parameters)
g = sp.symbols('g')
parameters[g] = 9.8
gravity = DynamicalSystem(output_equation=-g * m * l * sp.sin(x),
input_=x,
constants_values=parameters)
## put them together
BD = BlockDiagram(inertia, gravity)
BD.connect(gravity, inertia)
BD.connect(inertia, gravity, outputs=[0])
plt.figure()
plot_x(BD.simulate(8), 'first simulation!')
shape_figure()
plt.savefig("first_sim.pdf")
# run_trimmer(get_ic_args_from_spec(), throttle_ic=13.9, elevator_ic=-3.241, allow_roll=True, rudder_ic=None, aileron_ic=None)
# run_trimmer(get_ic_from_baseline(5), throttle_ic=13.9, elevator_ic=-3.241, allow_roll=True, rudder_ic=0., aileron_ic=0.0)
# run_trimmer(get_ic_from_baseline(5), throttle_ic=13.9, elevator_ic=-3.241, allow_roll=True, rudder_ic=None, aileron_ic=None)
# ESD_cond = update_flight_condition(get_ic_from_spec(), psi=0., theta=0., phi=0.,)
##
int_opts['nsteps'] = 5_000
# int_opts['max_step'] = 2**-5
controller_block = systems.SystemFromCallable(
get_controller_function(opt_ctrl[-1] / 100., opt_ctrl[0] / -25.),
dim_feedback, 4)
flight_condition = planet.ic_from_planetodetic(**opt_args)
planet.initial_condition = flight_condition
BD = BlockDiagram(planet, F16_vehicle, controller_block)
BD.connect(planet, F16_vehicle, inputs=np.arange(planet.dim_output))
BD.connect(F16_vehicle, planet, inputs=np.arange(F16_vehicle.dim_output))
BD.connect(controller_block,
F16_vehicle,
inputs=np.arange(planet.dim_output, planet.dim_output + 4))
with benchmark() as b:
res = BD.simulate(180, integrator_options=int_opts)
b.tfinal = res.t[-1]
plot_nesc_comparisons(res, '11')
Kc = linalg.solve(R, Bc.T @ Sc).reshape(1, -1)
ct_ctr = LTISystem(-Kc)
Sd = linalg.solve_discrete_are(
Ad,
Bd,
Q,
R,
)
Kd = linalg.solve(Bd.T @ Sd @ Bd + R, Bd.T @ Sd @ Ad)
dt_ctr = LTISystem(-Kd, dt=dT)
# Equality of discrete-time equivalent and original continuous-time
# system
dtct_bd = BlockDiagram(ct_sys, dt_ctr)
dtct_bd.connect(ct_sys, dt_ctr)
dtct_bd.connect(dt_ctr, ct_sys)
dtct_res = dtct_bd.simulate(Tsim)
dtdt_bd = BlockDiagram(dt_sys, dt_ctr)
dtdt_bd.connect(dt_sys, dt_ctr)
dtdt_bd.connect(dt_ctr, dt_sys)
dtdt_res = dtdt_bd.simulate(Tsim)
plt.figure()
for st in range(n):
plt.subplot(n + m, 1, st + 1)
plt.plot(dtct_res.t, dtct_res.y[:, st], '+-')
plt.stem(dtdt_res.t,
dtdt_res.y[:, st],
SG = Ss.row_join(Gs)
SG_subs = dict(matrix_subs((As, An), (Bs, Bn), (Qs, Qn), (Rs, Rn)))
# construct systems from matrix differential equations and reference
SG_sys = system_from_matrix_DE(SGdot, SG, rxs, SG_subs)
def ref_input_ctr(t, *args):
return np.r_[2 * (tF - t), 0]
ref_input_ctr_sys = SystemFromCallable(ref_input_ctr, 0, 2)
# simulate matrix differential equation with reference input
tF = 20
RiccatiBD = BlockDiagram(SG_sys, ref_input_ctr_sys)
RiccatiBD.connect(ref_input_ctr_sys, SG_sys)
sg_sim_res = RiccatiBD.simulate(tF)
# create callable to interpolate simulation results
sim_data_unique_t, sim_data_unique_t_idx = np.unique(sg_sim_res.t,
return_index=True)
mat_sg_result = array_callable_from_vector_trajectory(
np.flipud(tF - sg_sim_res.t[sim_data_unique_t_idx]),
np.flipud(sg_sim_res.x[sim_data_unique_t_idx]), SG_sys.state, SG)
vec_sg_result = array_callable_from_vector_trajectory(
np.flipud(tF - sg_sim_res.t[sim_data_unique_t_idx]),
np.flipud(sg_sim_res.x[sim_data_unique_t_idx]), SG_sys.state, SG_sys.state)
# Plot S components
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()
plt.show()
# case 2 - recoverable
import numpy as np
import os
import glob
from nesc_testcase_helper import plot_nesc_comparisons, nesc_options, int_opts, ft_per_m
planet = simupy_flight.Planet(
gravity=simupy_flight.earth_J2_gravity,
winds=simupy_flight.get_constant_winds(),
atmosphere=simupy_flight.get_constant_atmosphere(),
planetodetics=simupy_flight.Planetodetic(
a=simupy_flight.earth_equitorial_radius,
omega_p=simupy_flight.earth_rotation_rate,
f=simupy_flight.earth_f),
)
BD = BlockDiagram(planet)
lat_ic = 0. * np.pi / 180
long_ic = 0. * np.pi / 180
h_ic = 30_000 / ft_per_m
V_N_ic = 0.
V_E_ic = 0.
V_D_ic = 0.
psi_ic = 0. * np.pi / 180
theta_ic = 0. * np.pi / 180
phi_ic = 0. * np.pi / 180
omega_X_ic = 0.
omega_Y_ic = 0.
omega_Z_ic = 0.
planet.initial_condition = planet.ic_from_planetodetic(long_ic, lat_ic, h_ic,
x = x1, x2 = Array(dynamicsymbols('x1:3'))
mu = sp.symbols('mu')
state_equation = r_[x2, -x1 + mu * (1 - x1**2) * x2]
output_equation = r_[x1**2 + x2**2, sp.atan2(x2, x1)]
sys = DynamicalSystem(state_equation,
x,
output_equation=output_equation,
constants_values={mu: 5})
sys.initial_condition = np.array([1, 1]).T
BD = BlockDiagram(sys)
res = BD.simulate(30)
plt.figure()
plt.plot(res.t, res.x)
plt.legend([sp.latex(s, mode='inline') for s in sys.state])
plt.ylabel('$x_i(t)$')
plt.xlabel('$t$, s')
plt.title('system state vs time')
plt.tight_layout()
plt.show()
plt.figure()
plt.plot(*res.x.T)
plt.xlabel('$x_1(t)$')
plt.ylabel('$x_2(t)$')
from simupy.block_diagram import BlockDiagram
from simupy.array import Array, r_
from simupy.discontinuities import SwitchedOutput
plt.ion()
# This example shows how to implement a simple saturation block
llim = -0.75
ulim = 0.75
x = Array([dynamicsymbols('x')])
tvar = dynamicsymbols._t
sin = DynamicalSystem(Array([sp.cos(tvar)]), x)
sin_bd = BlockDiagram(sin)
sin_res = sin_bd.simulate(2 * np.pi)
plt.figure()
plt.plot(sin_res.t, sin_res.x)
limit = r_[llim, ulim]
saturation_output = r_['0,2', llim, x[0], ulim]
sat = SwitchedOutput(x[0], limit, output_equations=saturation_output, input_=x)
sat_bd = BlockDiagram(sin, sat)
sat_bd.connect(sin, sat)
sat_res = sat_bd.simulate(2 * np.pi)
plt.plot(sat_res.t, sat_res.y[:, -1])
plt.xlabel('$t$, s')
