if __name__ == "__main__":
""" MAIN TEST """
from pyro.analysis import phaseanalysis
from pyro.dynamic import integrator
from pyro.control import controller
# Double integrator
si = integrator.SimpleIntegrator()
di = integrator.DoubleIntegrator()
ti = integrator.TripleIntegrator()
# Controller
psvc = ProportionnalSingleVariableController()
psvc.gain = 1
# New cl-dynamic
clsi = controller.ClosedLoopSystem(si, psvc)
clsi.plot_phase_plane_trajectory([10], 10, 0, 0)
clsi.sim.plot('xu')
cldi = controller.ClosedLoopSystem(di, psvc)
cldi.plot_phase_plane_trajectory([10, 0], 10, 0, 1)
cldi.sim.plot('xu')
clti = controller.ClosedLoopSystem(ti, psvc)
clti.plot_trajectory([10, 0, 0], 10)
clti.sim.plot('xu')
pp = phaseanalysis.PhasePlot3(clti)
pp.plot()
vi = valueiteration.ValueIteration_ND( grid_sys , cf )
vi.uselookuptable = True
vi.initialize()
vi.load_data('car_vi_min_time')
#vi.compute_steps(20,False)
#vi.save_data('car_vi_min_time')
###############################################################################
# Closed-loop Law
vi.assign_interpol_controller()
vi.plot_policy(0)
vi.plot_policy(1)
cl_sys = controller.ClosedLoopSystem( sys , vi.ctl )
###############################################################################
## Simulation and animation
x0 = np.array([6, 0, 0])
tf = 20
cl_sys.x0 = x0
cl_sys.compute_trajectory(tf, 10001, 'euler')
cl_sys.plot_trajectory('xu')
cl_sys.animate_simulation()
'''
#################################################################
################## Main ########
#################################################################
'''
if __name__ == "__main__":
""" MAIN TEST """
from pyro.dynamic import pendulum
sp = pendulum.SinglePendulum()
c = ComputedTorqueController(sp)
# New cl-dynamic
clsp = controller.ClosedLoopSystem(sp, c)
x0 = np.array([2, 0])
clsp.plot_phase_plane_trajectory(x0)
clsp.sim.plot('xu')
clsp.animate_simulation()
####################################
dp = pendulum.DoublePendulum()
c2 = ComputedTorqueController(dp)
# New cl-dynamic
cldp = controller.ClosedLoopSystem(dp, c2)
x0 = np.array([2, 1, 0, 0])
cldp.plot_phase_plane_trajectory(x0, 10, 0, 2)
