def linearize_and_synthesize_lqr_controller(sys, cf):
"""
Compute the optimal linear controller minimizing the quadratic cost:
J = int ( xQx + uRu ) dt = xSx
with control law:
u = K x
Note:
---------
Controller assume y = x (output is directly the state vector)
Parameters
----------
sys : `ContinuousSystem` instance
cf : "quadratic cost function" instance
Returns
-------
instance of `Proportionnal Controller`
"""
ss = statespace.linearize(sys, 0.01)
ctl = synthesize_lqr_controller(ss, cf, sys.xbar, sys.ubar)
return ctl
def plot_linearized_pz_map(self, u_index=0, y_index=0):
"""
"""
from pyro.dynamic.statespace import linearize
from pyro.dynamic.tranferfunction import ss2tf
linearized_sys = linearize(self)
siso_sys = ss2tf(linearized_sys, u_index, y_index)
siso_sys.pz_map()
def animate_linearized_mode(self, i=0):
"""
Linearize and show eigen mode i
"""
from pyro.dynamic.statespace import linearize
linearized_sys = linearize(self)
linearized_sys.animate_eigen_mode(i, self.is_3d)
return linearized_sys
def plot_linearized_bode(self, u_index=0, y_index=0):
"""
Bode plot of linearized siso
"""
from pyro.dynamic.statespace import linearize
from pyro.dynamic.tranferfunction import ss2tf
linearized_sys = linearize(self)
siso_sys = ss2tf(linearized_sys, u_index, y_index)
siso_sys.bode_plot()
def animate_linearized_modes(self):
"""
Linearize and show eigen modes
"""
from pyro.dynamic.statespace import linearize
linearized_sys = linearize(self)
for i in range(self.n):
linearized_sys.animate_eigen_mode(i, self.is_3d)
return linearized_sys
from pyro.dynamic.rocket import Rocket from pyro.analysis.costfunction import QuadraticCostFunction from pyro.dynamic.statespace import linearize from pyro.control.lqr import synthesize_lqr_controller # Non-linear model sys = Rocket() sys.inertia = 400 sys.xbar = np.array([0, 2.2, 0, 0, 0, 0]) sys.ubar = np.array([1, 0]) * sys.mass * sys.gravity # Nominal trust = weight # Linear model ss = linearize(sys, 0.01) # Cost function cf = QuadraticCostFunction.from_sys(sys) cf.Q[0, 0] = 1 cf.Q[1, 1] = 10000 cf.Q[2, 2] = 0.1 cf.Q[3, 3] = 0 cf.Q[4, 4] = 10000 cf.Q[5, 5] = 0 cf.R[0, 0] = 0.01 cf.R[1, 1] = 10.0 # LQR controller ctl = synthesize_lqr_controller(ss, cf, sys.xbar, sys.ubar)
# -*- coding: utf-8 -*-
"""
Created on Jun 2 2021
@author: Alex
"""
from pyro.dynamic import pendulum
from pyro.dynamic.tranferfunction import ss2tf
from pyro.dynamic.statespace import linearize
non_linear_sys = pendulum.SinglePendulum()
non_linear_sys.lc1 = 1
non_linear_sys.m1 = 2
non_linear_sys.I1 = 2
non_linear_sys.d1 = 2
non_linear_sys.gravity = 9.81
linearized_sys = linearize( non_linear_sys )
siso_sys = ss2tf( linearized_sys, 0, 0)
print('Poles',siso_sys.poles)
print('num',siso_sys.num)
print('den',siso_sys.den)
siso_sys.bode_plot()
# Shortcut
non_linear_sys.plot_linearized_bode()
# -*- coding: utf-8 -*- """ Created on Jun 2 2021 @author: Alex """ import numpy as np from pyro.dynamic import tranferfunction from pyro.dynamic import statespace from pyro.dynamic import massspringdamper from pyro.control import linear # Plant m = 1 k = 1 b = 0 plant = massspringdamper.SingleMass(m,k,b) tf = tranferfunction.ss2tf( plant, 0, 0) tf.bode_plot() tf.pz_map() #Controller kp = 5 ki = 2 kd = 4 tau = 0.1 pid = linear.PIDController(kp,ki,kd,tau) pid.rbar[0] = 1 # set point closed_loop_sys = pid + plant cl_ss = statespace.linearize( closed_loop_sys )
sys_with_pid = pid + sys
sys_with_pid.plot_trajectory('xuj')
sys_with_pid.animate_simulation()
print('Trajectory cost: ', sys_with_pid.traj.J[-1])
##############################################################################
# LQR
##############################################################################
from pyro.dynamic import statespace
from pyro.control import lqr
# Linear model
ss = statespace.linearize(sys)
print('A=\n', ss.A)
print('B=\n', ss.B)
lqr_ctl = lqr.synthesize_lqr_controller(ss, cf, sys.xbar)
print('LQR K=\n', lqr_ctl.K)
lqr_ctl.plot_control_law(sys=sys)
sys_with_lqr = lqr_ctl + sys
sys_with_lqr.plot_trajectory('xuj')
sys_with_lqr.animate_simulation()
print('Trajectory cost: ', sys_with_lqr.traj.J[-1])
'''
#################################################################
################## Main ########
#################################################################
'''
if __name__ == "__main__":
from pyro.dynamic.pendulum import DoublePendulum
from pyro.analysis.costfunction import QuadraticCostFunction
sys = DoublePendulum()
ss = statespace.linearize(sys, 0.01)
cf = QuadraticCostFunction.from_sys(sys)
# Tune cost function here:
cf.R[0, 0] = 1000
cf.R[1, 1] = 10000
ctl = synthesize_lqr_controller(ss, cf)
print('\nLinearized sys matrix A:')
print(ss.A)
print('\nLinearized sys matrix B:')
print(ss.B)
print('\nCost function matrix Q:')