sys = vehicle.KinematicCarModelwithObstacles() ############################################################################### # Planning # Set domain sys.x_ub = np.array([+35, +3, +3]) sys.x_lb = np.array([-5, -2, -3]) sys.u_ub = np.array([+3, +1]) sys.u_lb = np.array([-3, -1]) # Discrete world grid_sys = discretizer.GridDynamicSystem(sys, (51, 51, 21), (3, 3), 0.1) # Cost Function xbar = np.array( [30, 0, 0] ) cf = costfunction.TimeCostFunction( xbar ) cf.INF = 50 cf.EPS = 0.5 # VI algo vi = valueiteration.ValueIteration_ND( grid_sys , cf ) vi.uselookuptable = True vi.initialize() vi.load_data('car_vi_min_time') #vi.compute_steps(20,False)
############################################################################### sys = longitudinal_vehicule.LongitudinalFrontWheelDriveCarWithWheelSlipInput() ############################################################################### # Planning # Set domain sys.x_ub = np.array([+50, 20,]) sys.x_lb = np.array([0, 0]) sys.u_ub = np.array([0.0]) sys.u_lb = np.array([-0.1]) # Discrete world grid_sys = discretizer.GridDynamicSystem(sys, (101, 101), (11,), 0.1) # Cost Function cf = costfunction.QuadraticCostFunction.from_sys( sys ) cf.xbar = np.array( [0, 0] ) # target cf.INF = 1E3 cf.EPS = 0.00 cf.R[0] = 1 cf.Q[0,0] = 0 cf.Q[1,1] = 0.01 # VI algo vi = valueiteration.ValueIteration_ND( grid_sys , cf ) vi.uselookuptable = True vi.initialize()
############################################################################## import numpy as np ############################################################################## from pyro.dynamic import pendulum from pyro.planning import discretizer from pyro.analysis import costfunction from pyro.planning import valueiteration from pyro.control import controller ############################################################################## sys = pendulum.SinglePendulum() ############################################################################## # Discrete world grid_sys = discretizer.GridDynamicSystem(sys) # Cost Function qcf = costfunction.QuadraticCostFunction.from_sys(sys) qcf.xbar = np.array([-3.14, 0]) # target qcf.INF = 10000 # VI algo vi = valueiteration.ValueIteration_ND(grid_sys, qcf) vi.initialize() #vi.load_data('simple_pendulum_vi') vi.plot_max_J = 10 vi.compute_steps(200, True) vi.assign_interpol_controller()
sys = TwoSpeedLinearActuator() ############################################################################### # Planning # Set domain sys.x_ub = np.array([+3, +3]) sys.x_lb = np.array([-3, -3]) sys.u_ub = np.array([+1, +1]) sys.u_lb = np.array([-0, -1]) # Discrete world grid_sys = discretizer.GridDynamicSystem(sys, (51, 51), (2, 11), 0.1) # Cost Function cf = costfunction.QuadraticCostFunction.from_sys(sys) cf.xbar = np.array([1, 0]) # target cf.INF = 1E9 cf.EPS = 0.2 cf.R = np.array([[0, 0], [0, 1]]) # VI algo vi = valueiteration.ValueIteration_ND(grid_sys, cf) vi.uselookuptable = True vi.initialize() vi.compute_steps(100)
from pyro.dynamic import vehicle from pyro.planning import discretizer from pyro.analysis import costfunction from pyro.planning import valueiteration from pyro.control import controller sys = vehicle.KinematicBicyleModel() sys.x_ub = np.array([+2, +2, +0.5]) sys.x_lb = np.array([-0, -0, -0.5]) sys.u_ub = np.array([+1, +1.0]) sys.u_lb = np.array([-1, -1.0]) # Discrete world grid_sys = discretizer.GridDynamicSystem(sys, (41, 41, 21), (3, 3), 0.05) # Cost Function cf = costfunction.QuadraticCostFunction(q=np.ones(sys.n), r=np.ones(sys.m), v=np.zeros(sys.p)) cf.xbar = np.array([1, 1, 0]) # target cf.INF = 1E4 cf.EPS = 0.2 cf.R = np.array([[0.01, 0], [0, 0]]) # VI algo """ EXPERIMENTAL TEST NOT WORKING """
sys = vehicle.KinematicProtoCarModelwithObstacles() ############################################################################### # Planning # Set domain sys.x_ub = np.array([+3.5, +1, +0.3]) sys.x_lb = np.array([-2, -1, -0.3]) sys.u_ub = np.array([+1, +1]) sys.u_lb = np.array([-1, -1]) # Discrete world grid_sys = discretizer.GridDynamicSystem(sys, (61, 61, 21), (3, 3), 0.025) # Cost Function cf = costfunction.QuadraticCostFunction(sys.n, sys.m, sys.p) cf.xbar = np.array([3, 0, 0]) # target cf.INF = 1E4 cf.EPS = 0.05 cf.R = np.array([[0.01, 0], [0, 0]]) # VI algo vi = valueiteration.ValueIteration_ND(grid_sys, cf) vi.uselookuptable = True vi.initialize()
import matplotlib.pyplot as plt from pyro.planning import discretizer, valueiteration # Continuous dynamic system sys = pendulum.DoublePendulum() sys.x_ub = np.array([0.2, 0.2, 0.2, 0.2]) sys.x_lb = np.array([-0.2, -0.2, -0.2, -0.2]) sys.u_ub = np.array([5, 2]) sys.u_lb = np.array([-5, -2]) # Discrete world grid_sys = discretizer.GridDynamicSystem(sys, (11, 11, 11, 11), (3, 3), 0.05) # Cost Function qcf = sys.cost_function qcf.xbar = np.array([0, 0, 0, 0]) # target qcf.INF = 100 # VI algo vi = valueiteration.ValueIteration_ND(grid_sys, qcf) vi.initialize() #vi.load_data('bbb') vi.compute_steps(2, True) #vi.load_data() vi.assign_interpol_controller()
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 from pyro.planning import discretizer # Value iteration algo vi = valueiteration.ValueIteration_2D(discretizer.GridDynamicSystem(sys), cf) vi.initialize() #vi.compute_steps(200,True). # To compute from sratch instead of loading the solution vi.load_data('simple_pendulum_vi') # /content/pyro/examples/demo/ vi.assign_interpol_controller() vi_ctl = vi.ctl vi_ctl.plot_control_law(sys=sys, n=1000) sys_with_vi = vi_ctl + sys sys_with_vi.plot_trajectory('xuj') sys_with_vi.animate_simulation() print('Trajectory cost: ', sys_with_vi.traj.J[-1])