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 __init__(self, plant, dt):
A, B = hcu.num_jacobian([0, 0, 0, 0], [0], plant.dyn_cont)
#poles = [-12+12j, -12-12j, -6.5+0j, -6.5-0j]
poles = [
-18.36312687 + 0.j, -7.08908759 + 0.j, -6.64111168 + 1.52140048j,
-6.64111168 - 1.52140048j
]
pdb.set_trace()
self.K = control.matlab.place(A, B, poles)
self.Ktheta = self.K[0, ms.Plant.s_theta]
self.Kthetad = self.K[0, ms.Plant.s_thetad]
self.Htheta = self.Ktheta
self.Kx = self.K[0, ms.Plant.s_x] / self.Htheta
self.Kxd = self.K[0, ms.Plant.s_xd] / self.Htheta
self.Hx = self.Kx
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 __init__(self, plant, dt):
A, B = hcu.num_jacobian([0, 0, 0, 0], [0], plant.dyn_cont)
At = np.array([[
A[plant.s_theta, plant.s_theta], A[plant.s_theta, plant.s_thetad]
], [
A[plant.s_thetad, plant.s_theta], A[plant.s_thetad, plant.s_thetad]
]])
Bt = np.array([B[plant.s_theta], B[plant.s_thetad]])
# extract non zero cefficients
self.a1 = A[plant.s_xd, plant.s_theta]
self.a2 = A[plant.s_thetad, plant.s_theta]
self.b1 = B[plant.s_xd, 0]
self.b2 = B[plant.s_thetad, 0]
om_il, xi_il = 45, 0.7
il_poles = hcu.get_lambdas(om_il, xi_il)
if 0:
self.Kil = control.matlab.place(
At, Bt, il_poles) # f*****g place is broken
else: # let's do it by hand instead
self.Ktheta, self.Kthetad = (
om_il**2 + self.a2) / self.b2, 2 * xi_il * om_il / self.b2
self.Htheta = (om_il**2) / self.b2
self.Kil = np.array([[self.Ktheta, self.Kthetad]])
Acl = At - np.dot(Bt, self.Kil)
eva, eve = np.linalg.eig(Acl)
print('inner loop wanted:{} obtained{}'.format(il_poles, eva))
om_ol, xi_ol = 1., 0.9
ol_poles = hcu.get_lambdas(om_ol, xi_ol)
if 0:
c = self.a1 - self.b1 * self.Ktheta + self.b1 * self.Htheta
self.Kx, self.Kxd = (om_ol**2) / c, 2 * xi_ol * om_ol / c
self.Hx = (om_ol**2) / c
if 0:
self.Kx, self.Kxd = (om_ol**
2) / self.b1, 2 * xi_ol * om_ol / self.b1
self.Kolt = self.a1 / self.b1 - self.Ktheta
self.Koltd = -self.Kthetad
#self.Kol = np.array([[self.Kx, self.Kxd]])
if 1:
self.Kx, self.Kxd = -0.75, -0.05
self.Hx = self.Kx
pdb.set_trace()
def jac(self, Xe, Ue):
return hcu.num_jacobian(Xe, Ue, self.dyn_cont)
def __init__(self, plant, dt):
A, B = hcu.num_jacobian([0, 0, 0, 0], [0], plant.dyn_cont)
poles = [-12 + 12j, -12 - 12j, -6.5 + 0j, -6.5 - 0j]
self.K = control.matlab.place(A, B, poles)
print('K {}'.format(self.K))
print('cl poles {}'.format(np.linalg.eig(A - np.dot(B, self.K))[0]))