from pyro.planning import discretizer from pyro.analysis import costfunction from pyro.planning import valueiteration sys = vehicle.HolonomicMobileRobotwithObstacles() # Discrete world grid_sys = discretizer.GridDynamicSystem(sys, (51, 51), (3, 3)) # Cost Function cf = costfunction.QuadraticCostFunction(sys) cf.INF = 1E9 # VI algo vi = valueiteration.ValueIteration_2D(grid_sys, cf) vi.initialize() vi.load_data('holonomic_vi') vi.compute_steps(1) vi.plot_cost2go(40000) vi.assign_interpol_controller() vi.plot_policy(0) vi.plot_policy(1) #vi.save_data('holonomic_vi') # Closed loop cl_sys = vi.ctl + sys # Simulation and animation x0 = [9, 0]
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])