def __init__(self, Tp):
# TODO: Fill the list self.models with 3 models of 2DOF manipulators with different m3 and r3
# Use parameters from manipulators/mm_planar_2dof.py
model1 = ManiuplatorModel(Tp, 0.1, 0.05)
model2 = ManiuplatorModel(Tp, 0.01, 0.01)
model3 = ManiuplatorModel(Tp, 1., 0.3)
self.models = [model1, model2, model3]
self.i = 0
self.Kd = np.diag((1, 1)) * [1, -1]
self.Kp = np.diag((1, 1)) * [1.3, 1.2]
def __init__(self, Tp):
# Use parameters from manipulators/mm_planar_2dof.py
self.models = [
ManiuplatorModel(Tp, 0.1, 0.05),
ManiuplatorModel(Tp, 0.01, 0.01),
ManiuplatorModel(Tp, 1., 0.3)
]
self.i = 0
self.kd = np.diag((1, 1)) * [1.5, -1]
self.kp = np.diag((1, 1)) * [-1, 2]
def __init__(self, Tp):
# TODO: Fill the list self.models with 3 models of 2DOF manipulators with different m3 and r3
# Use parameters from manipulators/mm_planar_2dof.py
self.models = [
ManiuplatorModel(Tp, 0.1, 0.05),
ManiuplatorModel(Tp, 0.01, 0.01),
ManiuplatorModel(Tp, 1., 0.3)
]
self.k_p = [[-3, 0.], [0., -0.7]]
self.k_d = [[-0.7, 0.], [0., -2]]
self.i = 0
def __init__(self, Tp):
# TODO: Fill the list self.models with 3 models of 2DOF manipulators with different m3 and r3
# Use parameters from manipulators/mm_planar_2dof.py
self.models = [
ManiuplatorModel(Tp, 0.1, 0.05),
ManiuplatorModel(Tp, 0.01, 0.01),
ManiuplatorModel(Tp, 1.0, 0.3)
]
self.i = 0
self.kd = np.diag((1, 1)) * [1.5, -1]
self.kp = np.diag((1, 1)) * [-1, 2]
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
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 __init__(self, Tp):
self.model = ManiuplatorModel(Tp)
self.Kd = 0.3
self.Kp = 0.6
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 __init__(self, Tp):
self.model = ManiuplatorModel(Tp)
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]]