# -*- coding: utf-8 -*-
"""
Created on Fri Nov 16 12:05:08 2018
@author: Alexandre
"""
###############################################################################
from pathlib import Path
import numpy as np
###############################################################################
from pyro.dynamic import pendulum
from pyro.control import nonlinear
from pyro.planning import plan
###############################################################################
sys = pendulum.DoublePendulum()
# Load pre-computed trajectory from file
this_file_dir = Path(__file__).parent
traj_file = this_file_dir.joinpath(Path('double_pendulum_rrt.npy'))
traj = plan.load_trajectory(str(traj_file))
# Controller
ctl = nonlinear.ComputedTorqueController(sys, traj)
# goal
ctl.rbar = np.array([0, 0])
# New cl-dynamic
cl_sys = ctl + sys
'''
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)
cldp.plot_phase_plane_trajectory(x0, 10, 1, 3)
cldp.sim.plot('xu')
cldp.animate_simulation()
