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