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