class FeedbackLinearizationController(Controller): def __init__(self, Tp): self.model = ManiuplatorModel(Tp, 4.0, 1.0) self.k_p = [[-3, 0.], [0., -0.7]] self.k_d = [[-0.7, 0.], [0., -2]] def calculate_control(self, x, q_d, q_d_dot, q_d_ddot): """ Please implement the feedback linearization using self.model (which you have to implement also), robot state x and desired control v. """ q_dot = x[2:, np.newaxis] q = x[0:1, np.newaxis] v = q_d_ddot + self.k_d * (q_dot - q_d_dot) + self.k_p * (q - q_d) return self.model.M(x) @ v + self.model.C(x) @ q_dot
class FeedbackLinearizationController(Controller): def __init__(self, Tp): self.model = ManiuplatorModel(Tp, 0.2, 0.05) self.kd = np.diag((1, 1)) * [2, -1] self.kp = np.diag((1, 1)) * [-2, 1.5] def calculate_control(self, x, q_d, q_d_dot, q_dd_dot): q0_dot = x[2:].reshape(-1, 1) q0 = x[:2].reshape(-1, 1) q_d_dot = q_d_dot.reshape(-1, 1) q_d = q_d.reshape(-1, 1) v = q_dd_dot + self.kd.dot(q0_dot - q_d_dot) + self.kp.dot(q0 - q_d) return self.model.M(x).dot(v) + self.model.C(x).dot(x[2:].reshape( -1, 1))
class FeedbackLinearizationController(Controller): def __init__(self, Tp): self.model = ManiuplatorModel(Tp, 0.21, 0.05) self.kd = np.diag((1, 1)) * [2, -1] self.kp = np.diag((1, 1)) * [-2, 1.5] def calculate_control(self, x, q_d, q_d_dot, q_dd_dot): """ Please implement the feedback linearization using self.model (which you have to implement also), robot state x and desired control v. """ q0_dot = x[2:].reshape(-1, 1) q0 = x[:2].reshape(-1, 1) q_d_dot = q_d_dot.reshape(-1, 1) q_d = q_d.reshape(-1, 1) v = q_dd_dot + self.kd.dot(q0_dot - q_d_dot) + self.kp.dot(q0 - q_d) z = self.model.M(x).dot(v) + self.model.C(x).dot(x[2:].reshape(-1, 1)) return z
class FeedbackLinearizationController(Controller): def __init__(self, Tp): self.model = ManiuplatorModel(Tp) self.Kd = 0.3 self.Kp = 0.6 def calculate_control(self, x, v, q_d, q_d_dot, q_d_ddot): """ Please implement the feedback linearization using self.model (which you have to implement also), robot state x and desired control v. """ q1, q2, q1_dot, q2_dot = x M = self.model.M(x) C = self.model.C(x) q = np.array([[q1], [q2]]) q_dot = np.array([[q1_dot], [q2_dot]]) v = q_d_ddot + self.Kd * (q_dot - q_d_dot) + self.Kp * (q - q_d) tau = M.dot(v) + C.dot(q_dot) return tau
class FeedbackLinearizationController(Controller): def __init__(self, Tp): self.model = ManiuplatorModel(Tp, 0.1, 0.01) self.Kd = np.diag((1, 1)) * [1, -1] self.Kp = np.diag((1, 1)) * [1.3, 1.2] def calculate_control(self, x, _q_d_ddot, _q_d_dot, _q_d): """ Please implement the feedback linearization using self.model (which you have to implement also), robot state x and desired control v. """ q1, q2, q1_dot, q2_dot = x q1_d_dot, q2_d_dot = _q_d_ddot # create q** vector from given v q_d_ddot = np.array([[q1_d_dot[0]], [q2_d_dot[0]]]) # create q* vector from given x q_d_dot = np.array([[q1_dot], [q2_dot]]) # create q vector from given x q_d = np.array([[q1], [q2]]) # feedback q_d_ddot = q_d_ddot + np.dot(self.Kd, (q_d_dot - _q_d_dot)) + np.dot( self.Kp, (q_d - _q_d)) # create output vector tau = np.zeros((2, 1), float) # calculate tau from equation no 20 M_v = np.dot(self.model.M(x), q_d_ddot) C_q = np.dot(self.model.C(x), q_d_dot) tau = M_v + C_q return tau