def __init__(self, Tp):
self.Tp = Tp
self.planar = PlanarManipulator2DOF(Tp)
self.l1 = 0.5
self.r1 = 0.5 * self.l1
self.m1 = 1.
self.l2 = 0.5
self.r2 = 0.5 * self.l2
self.m2 = 1.
self.I_1 = 1 / 12 * self.m1 * (3 * self.r1**2 + self.l1**2)
self.I_2 = 1 / 12 * self.m2 * (3 * self.r2**2 + self.l2**2)
self.m3 = 0.5
self.r3 = 0.01
self.I_3 = 2. / 5 * self.m3 * self.r3**2
from numpy import pi from scipy.integrate import odeint from controllers.dummy_controller import DummyController from controllers.feedback_linearization_controller import FeedbackLinearizationController from controllers.pd_controller import PDDecentralizedController from manipulators.planar_2dof import PlanarManipulator2DOF from trajectory_generators.constant_torque import ConstantTorque 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 = 5.0 kp2 = 6.7 kd1 = 1.5 kd2 = 1.3 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):