fl_controller = ADRController(b_est_1, kp_1, kd_1)
sl_controller = ADRController(b_est_2, kp_2, kd_2)
p = 100
l1 = 3*p
l2 = 3*p**2
l3 = p**3
A = np.array([[0., 1., 0.], [0., 0., 1.], [0., 0., 0.]])
B1 = np.array([0., b_est_1, 0.])[:, np.newaxis]
B2 = np.array([0., b_est_2, 0.])[:, np.newaxis]
L1 = np.array([l1, l2, l3])[:, np.newaxis]
L2 = np.array([l1, l2, l3])[:, np.newaxis]
first_link_state_estimator = ESO(A, B1, L1)
second_link_state_estimator = ESO(A, B2, L2)
traj_gen = Poly3(np.array([0., 0.]), np.array([pi/2, pi/2]), end)
ctrl = []
T = []
Q_d = []
def system(x_and_eso, t):
x = x_and_eso[:4]
fl_estimates = x_and_eso[4:7]
sl_estimates = x_and_eso[7:]
T.append(t)
q_d, q_d_dot, q_d_ddot = traj_gen.generate(t)
Q_d.append(q_d)
u1 = fl_controller.calculate_control(x[0], q_d[0], q_d_dot[0], q_d_ddot[0], fl_estimates)
from trajectory_generators.sinusonidal import Sinusoidal
from trajectory_generators.poly3 import Poly3
Tp = 0.01
start = 0
end = 3
t = np.linspace(start, end, int((end - start) / Tp))
manipulator = PlanarManipulator2DOF(Tp)
kp1 = 8.2
kp2 = 6.5
kd1 = 12.5
kd2 = 10.8
fl_controller = PDDecentralizedController(kp1, kd1)
sl_controller = PDDecentralizedController(kp2, kd2)
traj_gen = Poly3(np.array([0., 0.]), np.array([pi / 4, pi / 6]), end)
ctrl = []
T = []
Q_d = []
def system(x, t):
T.append(t)
q_d, q_d_dot, q_d_ddot = traj_gen.generate(t)
Q_d.append(q_d)
print(q_d_ddot)
u1 = fl_controller.calculate_control(x[0], x[2], q_d[0], q_d_dot[0],
q_d_ddot[0])
u2 = sl_controller.calculate_control(x[1], x[3], q_d[1], q_d_dot[1],
q_d_ddot[1])