Python synthesize_lqr_controller示例

示例#1

文件： rocket_landing_with_lqr.py 项目： SherbyRobotics/pyro

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()

示例#2

文件： two_mass_with_lqr.py 项目： SherbyRobotics/pyro

# -*- 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

示例#3

文件： double_pendulum_with_lqr.py 项目： SherbyRobotics/pyro

@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()

示例#4

# -*- 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

示例#5

文件： optimal_control_demo.py 项目： SherbyRobotics/pyro

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