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)
ctl.ubar
# Simulation Closed-Loop Non-linear with LQR controller
cl_sys = ctl + sys
cl_sys.x0 = np.array([1, 10, -0.8, 0, 0, 0])
cl_sys.compute_trajectory(10)
cl_sys.plot_trajectory('xu')
cl_sys.animate_simulation()
# -*- coding: utf-8 -*- """ Created on Jun 2 2021 @author: Alex """ import numpy as np from pyro.dynamic import massspringdamper from pyro.analysis.costfunction import QuadraticCostFunction from pyro.control.lqr import synthesize_lqr_controller # Plant sys = massspringdamper.TwoMass() sys.m1 = 2 sys.m2 = 3 sys.k1 = 5 sys.k2 = 2 sys.b1 = 0.1 sys.b2 = 0.2 sys.compute_ABCD() #Full state feedback (default of class is x2 output only) sys.C = np.diag([1, 1, 1, 1]) sys.p = 4 # dim of output vector sys.output_label = sys.state_label sys.output_units = sys.state_units # Cost function cf = QuadraticCostFunction.from_sys(sys) cf.Q[0, 0] = 5 cf.Q[1, 1] = 10 cf.Q[2, 2] = 5 cf.Q[3, 3] = 2 cf.R[0, 0] = 0.01
@author: alex
"""
import numpy as np
from pyro.dynamic.pendulum import DoublePendulum
from pyro.analysis.costfunction import QuadraticCostFunction
from pyro.dynamic.statespace import linearize
from pyro.control.lqr import synthesize_lqr_controller
# Non-linear model
sys = DoublePendulum()
# Linear model
ss = linearize(sys, 0.01)
# Cost function
cf = QuadraticCostFunction.from_sys(sys)
cf.R[0, 0] = 1000
cf.R[1, 1] = 10000
# LQR controller
ctl = synthesize_lqr_controller(ss, cf)
# Simulation Closed-Loop Non-linear with LQR controller
cl_sys = ctl + sys
cl_sys.x0 = np.array([0.4, 0, 0, 0])
cl_sys.compute_trajectory()
cl_sys.plot_trajectory('xu')
cl_sys.animate_simulation()
# -*- coding: utf-8 -*- """ Created on Jun 2 2021 @author: Alex """ import numpy as np from pyro.dynamic import massspringdamper from pyro.analysis.costfunction import QuadraticCostFunction from pyro.control.lqr import synthesize_lqr_controller # Plant sys = massspringdamper.ThreeMass() sys.m1 = 2 sys.m2 = 3 sys.k1 = 0.01 sys.k2 = 2 sys.b1 = 0.1 sys.b2 = 0.2 sys.compute_ABCD() # Update Matrices based on updated parameters #Full state feedback (default of class is x2 output only) sys.C = np.diag([1, 1, 1, 1, 1, 1]) sys.p = 6 # dim of output vector sys.output_label = sys.state_label sys.output_units = sys.state_units # Cost function cf = QuadraticCostFunction.from_sys(sys) cf.Q[0, 0] = 0 cf.Q[1, 1] = 0 cf.Q[2, 2] = 1 cf.Q[3, 3] = 0 cf.Q[4, 4] = 0
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])
##############################################################################
# VI
##############################################################################
from pyro.planning import valueiteration