# -*- coding: utf-8 -*-
"""
Created on Fri Nov 16 12:05:08 2018
@author: Alexandre
"""
###############################################################################
import numpy as np
###############################################################################
from pyro.dynamic import pendulum
from pyro.control import nonlinear
###############################################################################
sys = pendulum.SinglePendulum()
ctl = nonlinear.ComputedTorqueController(sys)
# Set Point
q_target = np.array([3.14])
ctl.rbar = q_target
# New cl-dynamic
cl_sys = ctl + sys
# Simultation
cl_sys.x0 = np.array([-2, 0])
cl_sys.compute_trajectory()
cl_sys.plot_phase_plane_trajectory()
cl_sys.plot_trajectory('xu')
cl_sys.animate_simulation()
# -*- coding: utf-8 -*-
"""
Created on Jun 2 2021
@author: Alex
"""
from pyro.dynamic import pendulum
from pyro.dynamic.tranferfunction import ss2tf
from pyro.dynamic.statespace import linearize
non_linear_sys = pendulum.SinglePendulum()
non_linear_sys.lc1 = 1
non_linear_sys.m1 = 2
non_linear_sys.I1 = 2
non_linear_sys.d1 = 2
non_linear_sys.gravity = 9.81
linearized_sys = linearize( non_linear_sys )
siso_sys = ss2tf( linearized_sys, 0, 0)
print('Poles',siso_sys.poles)
print('num',siso_sys.num)
print('den',siso_sys.den)
siso_sys.bode_plot()
# Shortcut
non_linear_sys.plot_linearized_bode()
return u
'''
#################################################################
################## 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
if __name__ == "__main__":
""" MAIN TEST """
num = np.array([1])
den = np.array([1, 0, 1])
#TF = signal.TransferFunction( num, den)
sys = TransferFunction(num, den)
#sys.bode_plot()
from pyro.dynamic import pendulum
non_linear_sys = pendulum.SinglePendulum()
non_linear_sys.lc1 = 1
non_linear_sys.m1 = 2
non_linear_sys.I1 = 2
non_linear_sys.d1 = 2
non_linear_sys.gravity = 9.81
linearized_sys = linearize(non_linear_sys)
siso_sys = ss2tf(linearized_sys, 0, 0)
print('Poles', siso_sys.poles)
print('num', siso_sys.num)
print('den', siso_sys.den)
siso_sys.bode_plot()
