def test_sample_system(self, tsys):
# Make sure we can convert various types of systems
for sysc in (tsys.siso_tf1, tsys.siso_tf1c, tsys.siso_ss1,
tsys.siso_ss1c, tsys.mimo_ss1, tsys.mimo_ss1c):
for method in ("zoh", "bilinear", "euler", "backward_diff"):
sysd = sample_system(sysc, 1, method=method)
assert sysd.dt == 1
# Check "matched", defined only for SISO transfer functions
for sysc in (tsys.siso_tf1, tsys.siso_tf1c):
sysd = sample_system(sysc, 1, method="matched")
assert sysd.dt == 1
def initialize(self, param_value_dict):
self.k1 = param_value_dict["k1"]
self.k2 = param_value_dict["k2"]
self.luenberger_poles = param_value_dict["Luenberger poles"]
AB = np.array([[0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 0]])
BB = np.array([[0, 0],
[0, 0],
[0, 0],
[1, 0],
[0, 0],
[0, 1]])
CB = np.array([[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
L = place(AB.T, CB.T, self.luenberger_poles).T
cont_observer_system = ss(AB - L @ CB, np.hstack((BB, L)), np.zeros((1, 6)), np.zeros((1, 6)))
# TODO: Remove hardcoded sample time
discrete_observer_system = sample_system(cont_observer_system, 1/60)
self.luenberger_Ad, self.luenberger_Bd, _, _ = ssdata(discrete_observer_system)
self.run_once = False
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 __init__(self, plant, dt):
self.plant, self.dt = plant, dt
Ac, Bc = hcu.num_jacobian([0, 0, 0, 0], [0], plant.dyn_cont)
#print Ac
#print Bc
Cc, Dc = np.array([[0, 1, 0, 0]]), np.array([[0]]) # regulate theta
self.sysc = control.ss(Ac, Bc, Cc, Dc)
self.sysd = control.sample_system(self.sysc, dt)
plant_tfd = control.tf(self.sysd)
ctl_tfd = control.tf([-4.945, 8.862, -3.967], [1.000, -1.481, 0.4812],
dt)
gt = control.feedback(ctl_tfd * plant_tfd, 1)
cl_polesd = gt.pole()
cl_polesc = np.log(cl_polesd) / dt
#y, t = control.step(gt, np.arange(0, 5, dt))
#plt.plot(t, y[0])
#plt.show()
#self.il_ctl = self.synth_pid(0.5, 0.01, 0.05)
#pdb.set_trace()
if 1:
#self.il_ctl = DiscTimeFilter([-4.945, 8.862, -3.967], [1.000, -1.481, 0.4812], 1.05)
self.il_ctl = DiscTimeFilter([-4.945, 8.862, -3.967],
[1.000, -1.481, 0.4812], 1.25)
else:
self.il_ctl = self.synth_pid(0.4, 0.04, 0.1)
self.il_ctl.num = [-c for c in self.il_ctl.num]
self.il_ctl.gain = 1.
#self.ol_ctl = DiscTimeFilter([0.18856, -0.37209, 0.18354], [1.00000, -1.86046, 0.86046], 0.9)
self.ol_ctl = DiscTimeFilter([0.18856, -0.37209, 0.18354],
[1.00000, -1.86046, 0.86046], 0.4)
def update_discrete_model(self, dt):
'''
creates the discrete system due to non-constant dt
'''
self.sysd = control.sample_system(self.sysc,
dt) # create a discrete time model
self.F = self.sysd.A
self.G = self.sysd.B
def test_sample_system(self):
# Make sure we can convert various types of systems
for sysc in (self.siso_tf1, self.siso_tf1c, self.siso_ss1,
self.siso_ss1c, self.mimo_ss1, self.mimo_ss1c):
for method in ("zoh", "bilinear", "euler", "backward_diff"):
sysd = sample_system(sysc, 1, method=method)
self.assertEqual(sysd.dt, 1)
# Check "matched", defined only for SISO transfer functions
for sysc in (self.siso_tf1, self.siso_tf1c):
sysd = sample_system(sysc, 1, method="matched")
self.assertEqual(sysd.dt, 1)
# Check errors
self.assertRaises(ValueError, sample_system, self.siso_ss1d, 1)
self.assertRaises(ValueError, sample_system, self.siso_tf1d, 1)
self.assertRaises(ValueError, sample_system, self.siso_ss1, 1,
'unknown')
def test_printing(self):
"""Print SISO"""
sys = ss2tf(rss(4, 1, 1))
assert isinstance(str(sys), str)
assert isinstance(sys._repr_latex_(), str)
# SISO, discrete time
sys = sample_system(sys, 1)
assert isinstance(str(sys), str)
assert isinstance(sys._repr_latex_(), str)
def get_model(self):
Acont = np.array([[-1 / (self.cr * self.R), 1 / (self.cr * self.R)],
[1 / (self.cw * self.R), -2 / (self.cw * self.R)]])
Bcont = np.array([[1 / self.cr, 0], [0, 1 / (self.cw * self.R)]])
C = np.array([[1, 0]])
D = np.array([[0, 0]])
sys = control.ss(Acont, Bcont, C, D)
sysd = control.sample_system(sys, SIMULATION_TIME_STEP)
Adisc, Bdisc, Cdisc, Ddisc = control.ssdata(sysd)
return Adisc, Bdisc[:, 0], Bdisc[:, 1]
def _get_longi_lti_ssr(dm, trim_args, dt=0.01, report=False):
Xe, Ue = dm.trim(trim_args, report=report)
Ac, Bc = dm.get_jacobian(Xe, Ue)
s = p3_fr.SixDOFAeroEuler
A1c = np.array([[Ac[s.sv_theta, s.sv_theta], Ac[s.sv_theta, s.sv_q]],
[Ac[s.sv_q, s.sv_theta], Ac[s.sv_q, s.sv_q]]])
B1c = np.array([[Bc[s.sv_theta, dm.iv_de()]],
[Bc[s.sv_q, dm.iv_de()]]])
ct_ssr = control.ss(A1c, B1c, [[1, 0]], [[0]])
dt_ssr = control.sample_system(ct_ssr, dt, method='zoh') # ‘matched’, ‘tustin’, ‘zoh’
return A1c, B1c, dt_ssr.A, dt_ssr.B
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 test_sample_tf(self, tsys):
# double integrator
sys = TransferFunction(1, [1,0,0])
for h in (0.1, 0.5, 1, 2):
numd_expected = 0.5 * h**2 * np.array([1.,1.])
dend_expected = np.array([1.,-2.,1.])
sysd = sample_system(sys, h, method='zoh')
assert sysd.dt == h
numd = sysd.num[0][0]
dend = sysd.den[0][0]
np.testing.assert_array_almost_equal(numd, numd_expected)
np.testing.assert_array_almost_equal(dend, dend_expected)
def test_output_of_difference_equation_against_discrete_tf_high_order(
self):
"""
This test checks the step response of the DifferenceEquation class vs the discrete transfer function
for a system where the system has high order
"""
sample_time = 0.10 # [seconds]
end_time = 10
coefficient_1 = 0.01
coefficient_2 = 0.001
coefficient_3 = 1
# Construct the Laplace operator
s = ct.tf([1, 0], 1)
step_input = 1
continuous_tf = (coefficient_3 * s) / (coefficient_1 * s**3 +
coefficient_2 * s + 1.5)
discrete_tf = ct.sample_system(continuous_tf, sample_time, "zoh")
difference_equation = de.DifferenceEquation(discrete_tf)
num_points = int(end_time / sample_time) + 1
T = np.linspace(0, end_time, num_points)
T, yout = ct.step_response(discrete_tf, T)
# Set up circular buffers for input/output management
inputs = collections.deque(
maxlen=difference_equation.get_system_order())
outputs = collections.deque(
maxlen=difference_equation.get_system_order())
full_output_history = []
for i in range(0, difference_equation.get_system_order()):
inputs.append(0)
for i in range(0, difference_equation.get_system_order()):
outputs.append(0)
for i in range(0, num_points):
inputs.appendleft(step_input)
output = difference_equation.calculate_output(outputs, inputs)
outputs.appendleft(output)
full_output_history.append(output)
for i in range(0, len(T)):
self.assertAlmostEqual(yout[i], full_output_history[i], 1)
def test_sample_system_errors(self, tsys):
# Check errors
with pytest.raises(ValueError):
sample_system(tsys.siso_ss1d, 1)
with pytest.raises(ValueError):
sample_system(tsys.siso_tf1d, 1)
with pytest.raises(ValueError):
sample_system(tsys.siso_ss1, 1, 'unknown')
def test_sample_ss(self, tsys):
# double integrators, two different ways
sys1 = StateSpace([[0.,1.],[0.,0.]], [[0.],[1.]], [[1.,0.]], 0.)
sys2 = StateSpace([[0.,0.],[1.,0.]], [[1.],[0.]], [[0.,1.]], 0.)
I = np.eye(2)
for sys in (sys1, sys2):
for h in (0.1, 0.5, 1, 2):
Ad = I + h * sys.A
Bd = h * sys.B + 0.5 * h**2 * sys.A @ sys.B
sysd = sample_system(sys, h, method='zoh')
np.testing.assert_array_almost_equal(sysd.A, Ad)
np.testing.assert_array_almost_equal(sysd.B, Bd)
np.testing.assert_array_almost_equal(sysd.C, sys.C)
np.testing.assert_array_almost_equal(sysd.D, sys.D)
assert sysd.dt == h
def test_call_siso(self, dt, omega, resp):
"""Evaluate the frequency response of a SISO system at one frequency."""
sys = TransferFunction([1., 3., 5], [1., 6., 2., -1])
if dt:
sys = sample_system(sys, dt)
s = np.exp(omega * 1j * dt)
else:
s = omega * 1j
# Correct versions of the call
np.testing.assert_allclose(evalfr(sys, s), resp, atol=1e-3)
np.testing.assert_allclose(sys(s), resp, atol=1e-3)
# Deprecated version of the call (should generate exception)
with pytest.raises(AttributeError):
np.testing.assert_allclose(sys.evalfr(omega), resp, atol=1e-3)
def synth2(self, plant, dt=0.01):
Ac, Bc = hcu.num_jacobian([0, 0, 0, 0], [0], plant.dyn_cont)
print('Ac\n{}\nBc\n{}'.format(Ac, Bc))
ct_ss = control.ss(Ac, Bc, [[1, 0, 0, 0]], [[0]])
print(ct_ss)
dt_ss = control.sample_system(
ct_ss, dt, method='zoh') # ‘matched’, ‘tustin’, ‘zoh’
print(dt_ss)
dt_tf = control.tf(dt_ss)
print(dt_tf)
desired_cl_poles_c = np.array([
-18.36312687 + 0.j, -7.08908759 + 0.j, -6.64111168 + 1.52140048j,
-6.64111168 - 1.52140048j
])
desired_cl_poles_d = np.exp(desired_cl_poles_c * dt)
print('desired poles\n{}\n{}'.format(desired_cl_poles_c,
desired_cl_poles_d))
def compute_cl_poles(ctl_num, ctl_den):
ctl_tf = control.tf(ctl_num, ctl_den, dt)
#print(ctl_tf)
cl_tf = control.feedback(dt_tf, ctl_tf, sign=-1)
#print control.damp(cl_tf)
#print(cl_tf)
cl_poles_d = control.pole(cl_tf)
#print(cl_polesd)
cl_poles_c = np.sort_complex(np.log(cl_poles_d) / dt)
print('cl_poles_c\n{}'.format(cl_poles_c))
return cl_poles_d
compute_cl_poles([-4.945, 8.862, -3.967], [1.000, -1.481, 0.4812])
compute_cl_poles([-4.945, 8.862, -3.967], [1.000, -1.2, 0.4812])
def err_fun(p):
ctl_num, ctl_den = p[:3], [1, p[3], p[4]]
err = np.sum(
np.abs(
compute_cl_poles(ctl_num, ctl_den)[:4] -
desired_cl_poles_d))
print ctl_num, ctl_den, err
return err
p0 = [-4.945, 8.862, -3.967, -1.481, 0.4812]
res = scipy.optimize.minimize(err_fun, p0)
pdb.set_trace()
def ss_init(dt, delay=0, method='zoh'):
# Continuous state-space matrices
A_cont = np.array([[0., 1.], [0., 0.]])
B_cont = np.array([[0.], [1.]])
C_cont = np.array([[1., 0.], [0., 1.]])
D_cont = np.array([[0.], [0.]])
numState = A_cont.shape[0]
# Continuous system
SS_cont = ctrl.ss(A_cont, B_cont, C_cont, D_cont)
# Discrete system
SS_disc_nodelay = ctrl.sample_system(SS_cont, dt, method=method)
if delay == 0: return SS_disc_nodelay
# Initialize discrete delayed state-space matrices
dumA = np.zeros((numState * (delay + 1), numState * (delay + 1)))
dumB = np.zeros((numState * (delay + 1), 1))
dumC = np.zeros((SS_disc_nodelay.C.shape[0], numState * (delay + 1)))
dumD = np.zeros((SS_disc_nodelay.D.shape[0], SS_disc_nodelay.B.shape[1]))
dumvec = np.ones((numState * delay))
# Populate ss-matrices such that the input is delayed by delay samples
dumA[0:numState, 0:numState] = SS_disc_nodelay.A
for k in range(numState):
dumA[k, numState + k * delay] = 1.
dumB[numState - 1 + (k + 1) * delay, 0] = SS_disc_nodelay.B[k]
dumvec[(k + 1) * delay - 1] = 0.
dumvec = dumvec[:-1]
dumA[numState:, numState:] = np.diag(dumvec, k=1)
dumC[:SS_disc_nodelay.C.shape[0], :SS_disc_nodelay.C.
shape[1]] = SS_disc_nodelay.C
dumD[:SS_disc_nodelay.D.shape[0], :SS_disc_nodelay.D.
shape[1]] = SS_disc_nodelay.D
# Create delayed discrete state-space system
SS_disc_delay = ctrl.ss(dumA, dumB, dumC, dumD, dt)
return SS_disc_delay
def report_plant_id(plant, Xe, Ue, plant_ann, dt=0.005):
Ac, Bc = plant.get_jacobian(Xe, Ue)
s = p3_fr.SixDOFAeroEuler
A1c = np.array([[Ac[s.sv_theta, s.sv_theta], Ac[s.sv_theta, s.sv_q]],
[Ac[s.sv_q, s.sv_theta], Ac[s.sv_q, s.sv_q]]])
B1c = np.array([[Bc[s.sv_theta, plant.iv_de()]],
[Bc[s.sv_q, plant.iv_de()]]])
ct_ss = control.ss(A1c, B1c, [[1, 0]], [[0]])
dt_ss = control.sample_system(ct_ss, dt, method='zoh') # ‘matched’, ‘tustin’, ‘zoh’
fmt_arg = {'precision':6, 'suppress_small':True}
LOG.info('Real plant jacobian:\n{}\n{}'.format(np.array2string(dt_ss.A, **fmt_arg), np.array2string(dt_ss.B, **fmt_arg)))
LOG.info(' modes: {}'.format(np.linalg.eig(dt_ss.A)[0]))
w_identified = plant_ann.get_layer(name="plant").get_weights()[0]
#print('Identified weights: \n{}'.format(w_identified))
A_id = np.matrix(w_identified[:2,:].T, dtype=np.float64)
B_id = np.matrix(w_identified[2:,:].T, dtype=np.float64)
LOG.info('Identified plant:\n{}\n{}'.format(np.array2string(A_id, **fmt_arg), np.array2string(B_id, **fmt_arg)))
LOG.info(' modes: {}'.format(np.linalg.eig(A_id)[0]))
def test_div(self):
# Make sure that sampling times work correctly
sys1 = TransferFunction([1., 3., 5], [1., 6., 2., -1], None)
sys2 = TransferFunction([[[-1., 3.]]], [[[1., 0., -1.]]], True)
sys3 = sys1 / sys2
assert sys3.dt is True
sys2 = TransferFunction([[[-1., 3.]]], [[[1., 0., -1.]]], 0.5)
sys3 = sys1 / sys2
assert sys3.dt == 0.5
sys1 = TransferFunction([1., 3., 5], [1., 6., 2., -1], 0.1)
with pytest.raises(ValueError):
TransferFunction.__truediv__(sys1, sys2)
sys1 = sample_system(rss(4, 1, 1), 0.5)
sys3 = TransferFunction.__rtruediv__(sys2, sys1)
assert sys3.dt == 0.5
def __init__(self, m=100., b=20., F=50., ts=0.05, tf=50):
# sys parameters
self.m = m
self.b = b
self.F_norm = F
# Time parameters
self.ts = ts
self.t0 = 0
self.t1 = 5
self.t2 = 25
self.t3 = 30
self.tf = tf
# Noise parameters
self.sigma_pos_meas = 0.001 # m^2
self.Q = self.sigma_pos_meas
self.sigma_vel = 0.01 # m^2/s^2
self.sigma_pos = 0.0001 # m^2
self.R = np.diagflat([self.sigma_pos, self.sigma_vel])
self.mu = np.array([[0.], [0.]])
self.SIG = np.eye(2)
# SS parameters
self.F = np.array([[0., 1.], [0., -self.b / self.m]])
self.G = np.array([[0.], [1. / self.m]])
self.H = np.array([1., 0.])
self.J = np.array([0.])
self.sys = mt.ss(self.F, self.G, self.H, self.J)
self.sysd = ctl.sample_system(self.sys, self.ts, 'tustin')
self.A = self.sysd.A
self.B = self.sysd.B
self.C = self.sysd.C
self.D = self.sysd.D
# States
self.X_1 = np.array([[0.], [0.]])
self.X = np.array([[0.], [0.]])
# Measurements
self.z = 0. + np.random.randn() * np.sqrt(self.Q)
def synth(self, plant, dt=0.01):
lqr = planar_control.CtlPlaceFullLQR(plant, dt)
B, C, D = [[0], [1], [0], [0]], [[0, 1, 0, 0]], [[0]]
#H = hcu.get_precommand(lqr.A, lqr.B, C, lqr.K)
#pdb.set_trace()
cl_ss = control.ss(lqr.Acl, B, C, D)
dt_ss = control.sample_system(
cl_ss, dt, method='zoh') # ‘matched’, ‘tustin’, ‘zoh’
print(dt_ss)
dt_tf = control.tf(dt_ss)
print('desired closed loop tf')
print(dt_tf)
def err_fun(p):
ctl_num, ctl_den = p[:3], [1, p[3], p[4]]
ctl_tf = control.tf(ctl_num, ctl_den, dt)
cl_tf = control.feedback(dt_tf, ctl_tf, sign=-1)
pdb.set_trace()
p0 = [-4.945, 8.862, -3.967, -1.481, 0.4812]
err_fun(p0)
def test_sample_system_prewarp(self, tsys, plantname):
"""bilinear approximation with prewarping test"""
wwarp = 50
Ts = 0.025
# test state space version
plant = getattr(tsys, plantname)
plant_fr = plant(wwarp * 1j)
plant_d_warped = plant.sample(Ts, 'bilinear', prewarp_frequency=wwarp)
dt = plant_d_warped.dt
plant_d_fr = plant_d_warped(np.exp(wwarp * 1.j * dt))
np.testing.assert_array_almost_equal(plant_fr, plant_d_fr)
plant_d_warped = sample_system(plant, Ts, 'bilinear',
prewarp_frequency=wwarp)
plant_d_fr = plant_d_warped(np.exp(wwarp * 1.j * dt))
np.testing.assert_array_almost_equal(plant_fr, plant_d_fr)
plant_d_warped = c2d(plant, Ts, 'bilinear', prewarp_frequency=wwarp)
plant_d_fr = plant_d_warped(np.exp(wwarp * 1.j * dt))
np.testing.assert_array_almost_equal(plant_fr, plant_d_fr)
def _get_distance_controller(self, delta_seconds):
'''
Effects: create distance controller
'''
KP_1 = 1.0 #1.0
KI_1 = 1.0 #1.0
num_pi = [-KP_1,
-KI_1] # numerator of the PI transfer function (KP*s + KI)
den_pi = [1.0, 0.01 * KI_1 / KP_1
] # 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.
self.distance_sys = sys
def test_simulator_run_for_ticks(self):
"""
This test checks the step response of the DifferenceEquation class vs the discrete transfer function
for a specified number of ticks
"""
sample_time = 0.01 # [seconds]
end_time = 20
coefficient_1 = 0.01
coefficient_2 = 0.001
coefficient_3 = 1
# Construct the Laplace operator
s = ct.tf([1, 0], 1)
step_input = 1
continuous_tf = (coefficient_3 * s**2 + coefficient_3 * coefficient_2 *
s) / (coefficient_1 * s**2 + coefficient_2 * s + 1.5)
discrete_tf = ct.sample_system(continuous_tf, sample_time, "zoh")
difference_equation = de.DifferenceEquation(discrete_tf)
num_points = int(end_time / sample_time) + 1
T = np.linspace(0, end_time, num_points)
T, yout = ct.step_response(discrete_tf, T)
# Create and run the difference equation simulator for the same number
# of ticks as the control toolbox discrete controller
deSim = de.DifferenceEquationSimulator(difference_equation)
deSim.run_for_ticks(num_points, step_input)
for i in range(0, len(T)):
self.assertAlmostEqual(yout[i], deSim.full_output_history[i], 4)
V_max = 1.25 # Maximum velocity (m/s) theta_max = np.deg2rad(5) # maximum deflection (rad.) # Now, define the state-space form of the equations of motion # For derivation of these, you can see the Jupyter notebook at: # https://git.io/vxDsz A = np.array([[0, 1, 0, 0], [-g / l, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 0]]) B = np.array([[0], [1 / l], [0], [1]]) C = np.eye(4) # Output all states D = np.zeros((4, 1)) sys = control.ss(A, B, C, D) # Convert the system to digital. We need to use the discrete version of the # system for the MPC solution digital_sys = control.sample_system(sys, dt) # Get the number of states and inputs - for use in setting up the optimization # problem num_states = np.shape(A)[0] # Number of states num_inputs = np.shape(B)[1] # Number of inputs # Define the desired states to track. Here, it's just a desired final value # in the each of the states XD = 1.0 # desired position (m) XD_dot = 0.0 # desired velocity (m/s) thetaD = 0.0 # desired cable angle (rad) thetaD_dot = 0.0 # desired cable angular velocity (rad/s) # Define the weights on the system states and input q11 = 100 # The weight on error in angle from desired
# Form the continuous time version of the system
A_cont = np.array([[0, 1],
[-wn**2, -2*zeta*wn]])
B_cont = np.array([[0],
[1/m]])
C_cont = np.array([[1, 0]]) #np.eye(2)
D_cont = np.zeros((np.shape(C_cont)[0],np.shape(B_cont)[1]))
sys = control.ss(A_cont, B_cont, C_cont, D_cont)
# Now, digitize it
digital_sys = control.sample_system(sys, dt)
# And extract the state space components
A = digital_sys.A
B = digital_sys.B
C = digital_sys.C
D = digital_sys.D
# Example arrays from the link in the preamble. Were used to check this code.
# A = np.array([[1, 1],[0, 1]])
# B = np.array([[0.5],[1]])
# C = np.array([[1, 0]])
# D = np.zeros((np.shape(C)[0],np.shape(B)[1]))
# Determine the number of states and inputs from the matrix shapes
num_states = len(A)
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Thu Nov 28 21:47:43 2019
Todos estos códigos requieren comentarios y una limpieza
Limpio=NO
Comentarios=NO
@author: carlos
"""
import control as ct
import sympy as sym
import numpy as np
import matplotlib.pyplot as plt
from scipy import signal
from matplotlib.ticker import (MultipleLocator)
s = sym.symbols('s')
num=[1]
den= ( s*(s+1)*(s+5) )
## pon esto en la terminal para obtener coeficientes sym.Poly(den, s).all_coeffs()
G = ct.tf([1],[1, 6, 5, 0])
Gz = ct.sample_system(G,0.2) #zoh zero order hold es el predeterminado, retenedor de orden cero
print(Gz) ## si pones Gz directamente en el terminal, sale más 'bonito'
bo = compute_block_ouptputs(states) # #### additional code for comparision A, B, C, D = get_linear_ct_model(theStateAdmin, sys_output) # create a control system with the python-control-toolbox, see # https://github.com/python-control/python-control import control cs = control.StateSpace(A, B, C, D) # use default method (zero oder hold (zoh)) cs_dt = control.sample_system(cs, 0.05) # calculate step-response for first input __, yy_dt = control.step_response(cs_dt, T=tt, input=0) # in the time-discrete case the result is shortened by one value. -> repeat the last one. yy_dt = np.column_stack((yy_dt, yy_dt[:, -1])) # __, yy_dt = control.step_response(cs,T=tt, input=0) # #### end of additional code
#%%
dt = 0.25
InputValue = 1.
#%%
# Continuous state-space matrices
A_cont = np.array([[0., 1.], [0., 0]])
B_cont = np.array([[0.], [1.]])
C_cont = np.array([[1., 0.], [0., 1.]])
D_cont = np.array([[0.], [0.]])
# Continuous system
SS_cont = ctrl.ss(A_cont, B_cont, C_cont, D_cont)
# Discrete system
SS_disc_nodelay = ctrl.sample_system(SS_cont, dt, method='tustin')
#%%
T = np.arange(0, 100, dt)
yout, T, xout = ctrl.lsim(SS_disc_nodelay,
U=np.ones(T.shape) * InputValue,
T=T)
#%%
fig = plt.figure()
plt.plot(T, yout[0, :], label='Position')
plt.plot(T, yout[1, :], label='Velocity')
plt.plot(T, yout[1, :] / yout[0, :], label='Divergence')
plt.axhline(y=dt, color='black', linestyle='--', label='dt')
plt.ylim((-100, 100))
plt.legend()
import fem3d_2ss, weld1.filter1
# fem2ss = fem3d_2ss.Fem3d_fenics('data/v5/16x8x2/')
# fem2ss = fem3d_2ss.Fem3d_fenics('data/v5/32x16x4/1500/')
# fem2ss = fem3d_2ss.Fem3d_fenics('data/v5/40x20x5/')
# fem2ss_2 = fem3d_2ss.Fem3d_fenics('data/v5/32x16x4/1500/k30/')
# fem2ss = fem3d_2ss.Fem3d_fenics('data/v5/irregular/')
fem2ss = fem3d_2ss.Fem3d_fenics('data/v5/irregular2/')
fem2ss_2 = fem3d_2ss.Fem3d_fenics('data/v5/irregular2/k30/')
# fem2ss.A.shape
inp = np.array((.02,.02,0))
p1 = (.01,0,0)
p2 = (0,0.005,0)
plantp_d = control.sample_system(fem2ss.get_ss(inp + p1), .001)
plantv_d = control.sample_system(fem2ss.get_ss_v(inp + p2), .001)
# plant_d = plantp_d
T = fem2ss.Teq
control_on = True
perturb_on = False
# Controllers
ctrl1_d = control.sample_system(control.tf(1, (1, 0)), .001)
ctrl1 = weld1.filter1.Filter1((0,ctrl1_d.num[0][0][0]),ctrl1_d.den[0][0])
ctrl2_d = control.sample_system(control.tf(-0.0002, (1, 0)), .001)
ctrl2 = weld1.filter1.Filter1((0,ctrl2_d.num[0][0][0]),ctrl2_d.den[0][0])
# Set points
from matplotlib import pyplot as plt
__author__ = 'javier'
import control
import numpy as np
steps = 500
Ts = 1.84e-3
time = steps * Ts
setpoints = np.concatenate((np.ones(steps / 2), np.ones(steps / 2) * -1))
G = control.tf([52.1], [1.21, 1, 0])
C = control.tf([0.525, 5.022, 4.4], [0.005, 1, 0])
C_discrete = control.sample_system(C, Ts, method='tustin')
G_discrete = control.sample_system(G, Ts, method='zoh')
def factory(ctrl_in, sys):
global fyk1, fyk2, fuk1, fuk2
# Simulate controller's transfer function
den = sys.den[0][0]
num = sys.num[0][0]
# Transfer function parameters
a1 = den[1]
a2 = den[2]
b0 = num[0]
b1 = num[1]
Created on Wed Nov 27 00:49:43 2019
Todos estos códigos requieren comentarios y una limpieza
Limpio=NO
Comentarios=NO
@author: carlos
"""
import control as ct
import sympy as sym
import numpy as np
import matplotlib.pyplot as plt
from scipy import signal
from matplotlib.ticker import (MultipleLocator)
Gp = ct.tf(100, [1, 0, 100])
ts = 0.05
Gz = ct.sample_system(Gp, ts)
sym.pprint(Gz)
t = np.arange(0.0, 50 * ts, ts)
(num, den) = ct.tfdata(Gp) ## por que GP???? lsim no funciona con Gz
num = np.array(num) ##np.asarray hace lo mismo convierte tuple/list a array
den = np.array(den)
num = num.flatten()
den = den.flatten()
do = ct.damp(
Gz, doprint=True
) ##damp me devuelve en vez de un vector, un tuple (parecido a una matriz) que contiene todos los datos
sys = signal.lti(
num, den) ##transformo Gp en lti que es lo que acepta la syntax de lsim
wn = do[
0] ## 0 contiene wn, 1 contiene damping, 2 contiene eigen de los dos polos
