From 49cce00d9b5152e079f8a230bf95c9d5e3b95778 Mon Sep 17 00:00:00 2001 From: Videh Patel Date: Tue, 25 Jan 2022 04:00:51 +0530 Subject: [PATCH] Configured the controllers for all three robots --- Controllers/ComputedTorqueFFController.py | 89 ++++++++++++++++++-- Controllers/FeedForwardController.py | 99 ++++++++++++++++++++--- Controllers/MultivariableController.py | 21 +++-- Controllers/PIDController.py | 74 +++++++++++++---- Robots/PUMA.py | 5 +- Robots/SCARA.py | 3 + Robots/Stanford.py | 5 +- test.py | 70 ++++++++++++++++ 8 files changed, 324 insertions(+), 42 deletions(-) create mode 100644 test.py diff --git a/Controllers/ComputedTorqueFFController.py b/Controllers/ComputedTorqueFFController.py index a071c3d..7c996d2 100644 --- a/Controllers/ComputedTorqueFFController.py +++ b/Controllers/ComputedTorqueFFController.py @@ -3,11 +3,83 @@ import sympy as sp class ComputedTorque_FF_Position_Controller: - def __init__(self, RRMmodel, Kp = (0,0,0), Ki = (0,0,0), Kd = (0,0,0)): + def __init__(self, RRMmodel, Kp = None, Ki = None, Kd = None): self.Manipulator = RRMmodel - self.Kp = np.array(Kp) - self.Ki = np.array(Ki) - self.Kd = np.array(Kd) + ## Kp,Kd Calculations + #For "SCARA": + # Near the given coordinates + # For CRITICAL DAMPING + # For joint 1---- + # Jeff = 58 + # Beff = 2 ===> For omega_n = 5, Kp = 58*5**2 = 1450; Kd = 2*5*58 - 2 = 578 + # For joint 2---- + # Jeff = 17.25 + # Beff = 2 ===> For omega_n = 5, Kp = 17.25*5**2 = 431; Kd = 2*5*17.25 - 2 = 170 + # For joint 3---- + # Jeff = 11 + # Beff = 2 ===> For Kp = 1e4 ===> omega_n = 30; Kd = 2*30*11 - 2 = 658 + + #For "PUMA": + # Near the given coordinates (5,7,0) + # For CRITICAL DAMPING + # For joint 1---- + # Jeff = 1 + 5 = 6 + # Beff = 2 ===> For omega_n = 10, Kp = 6*10**2 = 600; Kd = 2*6*10 - 2 = 118 + # For joint 2---- + # Jeff = 31.16 + 1 = 32.16 + # Beff = 2 ===> For omega_n = 10, Kp = 32.16*10**2 = 3216; Kd = 2*10*32.16 - 2 = 641.2 + # For joint 3---- + # Jeff = 8.33 + 1 = 9.33 + # Beff = 2 ===> For omega_n = 10, Kp = 9.33*10**2 = 933; Kd = 2*10*9.33 - 2 = 186.6 + + #For "Stanford": + # Near the given coordinates (2,3,0) + # For CRITICAL DAMPING + # For joint 1---- + # Jeff = 1 + 4.33 = 5.33 + # Beff = 2 ===> For omega_n = 10, Kp = 5.33*10**2 = 534; Kd = 2*5.33*10 - 2 = 104.6 + # For joint 2---- + # Jeff = 12.67 + 1 = 13.67 + # Beff = 2 ===> For omega_n = 10, Kp = 13.67*10**2 = 1367; Kd = 2*10*13.67 - 2 = 271.4 + # For joint 3---- + # Jeff = 1 + 1 = 2 + # Beff = 2 ===> For omega_n = 10, Kp = 2*10**2 = 200; Kd = 2*10*2 - 2 = 38 + + if Kp is None: + if RRMmodel.type == "SCARA": + self.Kp = np.array([1450, 431, 1e4]) + + elif RRMmodel.type == "PUMA": + self.Kp = np.array([150, 804, 233.25]) + + elif RRMmodel.type == "Stanford": + self.Kp = np.array([534, 1367, 200]) + else: + self.Kp = np.array(Kp) + + if Kd is None: + if RRMmodel.type == "SCARA": + self.Kd = np.array([578, 170, 658]) + + elif RRMmodel.type == "PUMA": + self.Kd = np.array([58, 319.6, 91.3]) + + elif RRMmodel.type == "Stanford": + self.Kd = np.array([104.6, 271.4, 38]) + else: + self.Kd = np.array(Kd) + + if Ki is None: + if RRMmodel.type == "SCARA": + self.Ki = np.array([0,0,0]) + + elif RRMmodel.type == "PUMA": + self.Ki = np.array([0,0,0]) + + elif RRMmodel.type == "Stanford": + self.Ki = np.array([0,0,0]) + else: + self.Ki = np.array(Ki) self.nDOF = int(len(self.Manipulator.state)/2) self.target_state = np.array(3*self.nDOF*[0.]) @@ -59,11 +131,12 @@ def calculate_output_voltages(self): return sp.Matrix(V) def achieve_target_state(self): - err_mat = self.error_matrix - err_mat[:,2] = self.error_matrix[:,2]*0.01 - while (((np.linalg.norm(err_mat[0,:2]) + np.linalg.norm(err_mat[1,:2]) + np.linalg.norm(err_mat[2,:2]))/3) > 5e-2) or ((np.abs(err_mat[0,0]) + np.abs(err_mat[1,0]) + np.abs(err_mat[2,0])/3) > 8e-2) or np.linalg.norm(self.Manipulator.state[3:]) > 1e-2: + err_mat = self.error_matrix.astype(float) + while (np.linalg.norm(err_mat[:,0]) > 4e-2) or (np.linalg.norm(err_mat[:,1]) > 3e-2) or (np.linalg.norm(err_mat[:,2]) > 9e-2): self.Manipulator.apply_voltages(self.calculate_output_voltages()) - + err_mat = self.error_matrix.astype(float) + if (np.linalg.norm((self.Manipulator.state[self.nDOF: 2*self.nDOF]).astype(float)) < 5e-3): + err_mat = np.zeros((3,3)) def follow_trajectory(self, q_mat, qdot_mat, qdot2_mat, t, ee_pos_values = None): # t: Total time for execution of trajectory diff --git a/Controllers/FeedForwardController.py b/Controllers/FeedForwardController.py index 3d2a22c..9d9e522 100644 --- a/Controllers/FeedForwardController.py +++ b/Controllers/FeedForwardController.py @@ -3,11 +3,83 @@ import sympy as sp class FeedForward_Position_Controller: - def __init__(self, RRMmodel, Kp = (0,0,0), Ki = (0,0,0), Kd = (0,0,0)): + def __init__(self, RRMmodel, Kp = None, Ki = None, Kd = None): self.Manipulator = RRMmodel - self.Kp = np.array(Kp) - self.Ki = np.array(Ki) - self.Kd = np.array(Kd) + ## Kp,Kd Calculations + #For "SCARA": + # Near the given coordinates + # For CRITICAL DAMPING + # For joint 1---- + # Jeff = 58 + # Beff = 2 ===> For omega_n = 5, Kp = 58*5**2 = 1450; Kd = 2*5*58 - 2 = 578 + # For joint 2---- + # Jeff = 17.25 + # Beff = 2 ===> For omega_n = 5, Kp = 17.25*5**2 = 431; Kd = 2*5*17.25 - 2 = 170 + # For joint 3---- + # Jeff = 11 + # Beff = 2 ===> For Kp = 1e4 ===> omega_n = 30; Kd = 2*30*11 - 2 = 658 + + #For "PUMA": + # Near the given coordinates (5,7,0) + # For CRITICAL DAMPING + # For joint 1---- + # Jeff = 1 + 5 = 6 + # Beff = 2 ===> For omega_n = 10, Kp = 6*10**2 = 600; Kd = 2*6*10 - 2 = 118 + # For joint 2---- + # Jeff = 31.16 + 1 = 32.16 + # Beff = 2 ===> For omega_n = 10, Kp = 32.16*10**2 = 3216; Kd = 2*10*32.16 - 2 = 641.2 + # For joint 3---- + # Jeff = 8.33 + 1 = 9.33 + # Beff = 2 ===> For omega_n = 10, Kp = 9.33*10**2 = 933; Kd = 2*10*9.33 - 2 = 186.6 + + #For "Stanford": + # Near the given coordinates (2,3,0) + # For CRITICAL DAMPING + # For joint 1---- + # Jeff = 1 + 4.33 = 5.33 + # Beff = 2 ===> For omega_n = 10, Kp = 5.33*10**2 = 534; Kd = 2*5.33*10 - 2 = 104.6 + # For joint 2---- + # Jeff = 12.67 + 1 = 13.67 + # Beff = 2 ===> For omega_n = 10, Kp = 13.67*10**2 = 1367; Kd = 2*10*13.67 - 2 = 271.4 + # For joint 3---- + # Jeff = 1 + 1 = 2 + # Beff = 2 ===> For omega_n = 10, Kp = 2*10**2 = 200; Kd = 2*10*2 - 2 = 38 + + if Kp is None: + if RRMmodel.type == "SCARA": + self.Kp = np.array([1450, 431, 1e4]) + + elif RRMmodel.type == "PUMA": + self.Kp = np.array([150, 804, 233.25]) + + elif RRMmodel.type == "Stanford": + self.Kp = np.array([534, 1367, 200]) + else: + self.Kp = np.array(Kp) + + if Kd is None: + if RRMmodel.type == "SCARA": + self.Kd = np.array([578, 170, 658]) + + elif RRMmodel.type == "PUMA": + self.Kd = np.array([58, 319.6, 91.3]) + + elif RRMmodel.type == "Stanford": + self.Kd = np.array([104.6, 271.4, 38]) + else: + self.Kd = np.array(Kd) + + if Ki is None: + if RRMmodel.type == "SCARA": + self.Ki = np.array([0,0,0]) + + elif RRMmodel.type == "PUMA": + self.Ki = np.array([0,0,0]) + + elif RRMmodel.type == "Stanford": + self.Ki = np.array([0,0,0]) + else: + self.Ki = np.array(Ki) self.nDOF = int(len(self.Manipulator.state)/2) self.target_state = np.array(3*self.nDOF*[0.]) @@ -49,17 +121,24 @@ def calculate_output_voltages(self): self.error_matrix[1,:] = self.error_matrix[2,:] self.error_matrix[2,:] = self.previous_error.T - dii_thetadot2_mat = 1/self.Manipulator.r*np.matrix([self.target_state[2*self.nDOF+i]*self.Manipulator.D.subs([(sp.symbols('q1'), curr_state[0]), (sp.symbols('q2'), curr_state[1]), (sp.symbols('q3'), curr_state[2])]).evalf()[i,i] for i in range(3)]).T + dii_thetadot2_mat = 1/self.Manipulator.r*np.matrix([self.target_state[2*self.nDOF+i]*self.Manipulator.D.subs([(sp.symbols('q1'), curr_state[0]), (sp.symbols('q2'), curr_state[1]), (sp.symbols('q3'), curr_state[2])]).evalf()[i,i] for i in range(self.nDOF)]).T V = np.matrix((self.Kp*error + self.Ki*self.integral_error + self.Kd*diff_error)).T + self.Manipulator.R/self.Manipulator.Km*(self.Manipulator.Jm*np.matrix(self.target_state[2*self.nDOF:]).T/self.Manipulator.r + self.Manipulator.r**2*dii_thetadot2_mat + (self.Manipulator.Bm + self.Manipulator.Kb*self.Manipulator.Km/self.Manipulator.R)/self.Manipulator.r*np.matrix(self.target_state[self.nDOF:2*self.nDOF]).T) + # print(np.matrix((self.Kp*error + self.Ki*self.integral_error + self.Kd*diff_error)).T) + # print(self.Manipulator.R/self.Manipulator.Km*(self.Manipulator.Jm*np.matrix(self.target_state[2*self.nDOF:]).T/self.Manipulator.r)) + # print(self.Manipulator.r**2*dii_thetadot2_mat) + # print((self.Manipulator.Bm + self.Manipulator.Kb*self.Manipulator.Km/self.Manipulator.R)/self.Manipulator.r*np.matrix(self.target_state[self.nDOF:2*self.nDOF]).T) + # print("---------------------------") return sp.Matrix(V) def achieve_target_state(self): - err_mat = self.error_matrix - err_mat[:,2] = self.error_matrix[:,2]*0.01 - while (((np.linalg.norm(err_mat[0,:2]) + np.linalg.norm(err_mat[1,:2]) + np.linalg.norm(err_mat[2,:2]))/3) > 5e-2) or ((np.abs(err_mat[0,0]) + np.abs(err_mat[1,0]) + np.abs(err_mat[2,0])/3) > 8e-2) or np.linalg.norm(self.Manipulator.state[3:]) > 1e-2: + err_mat = self.error_matrix.astype(float) + while (np.linalg.norm(err_mat[:,0]) > 4e-2) or (np.linalg.norm(err_mat[:,1]) > 3e-2) or (np.linalg.norm(err_mat[:,2]) > 9e-2): self.Manipulator.apply_voltages(self.calculate_output_voltages()) - - + err_mat = self.error_matrix.astype(float) + # print(np.linalg.norm(err_mat[:,0]) > 4e-2, np.linalg.norm(err_mat[:,1]) > 3e-2, np.linalg.norm(err_mat[:,2]) > 9e-2) + if (np.linalg.norm((self.Manipulator.state[self.nDOF: 2*self.nDOF]).astype(float)) < 5e-3): + err_mat = np.zeros((3,3)) + def follow_trajectory(self, q_mat, qdot_mat, qdot2_mat, t, ee_pos_values = None): # t: Total time for execution of trajectory # q_mat: array of q's diff --git a/Controllers/MultivariableController.py b/Controllers/MultivariableController.py index 9804369..9b8075f 100644 --- a/Controllers/MultivariableController.py +++ b/Controllers/MultivariableController.py @@ -52,15 +52,26 @@ def calculate_output_voltages(self): v = -self.K0*q - self.K1*qdot + r u = (D + self.Jeff_mat)@v + h - V = u*self.Manipulator.Km/self.Manipulator.R/self.Manipulator.r + V = u*self.Manipulator.Km/self.Manipulator.R/self.Manipulator.r + + curr_state = self.Manipulator.get_state()[:self.nDOF] + error = np.array(self.target_state[:self.nDOF] - curr_state) + self.previous_error = error + + self.error_matrix[0,:] = self.error_matrix[1,:] + self.error_matrix[1,:] = self.error_matrix[2,:] + self.error_matrix[2,:] = self.previous_error.T return V def achieve_target_state(self): - err_mat = self.error_matrix - err_mat[:,2] = self.error_matrix[:,2]*0.01 - while (((np.linalg.norm(err_mat[0,:2]) + np.linalg.norm(err_mat[1,:2]) + np.linalg.norm(err_mat[2,:2]))/3) > 5e-2) or ((np.abs(err_mat[0,0]) + np.abs(err_mat[1,0]) + np.abs(err_mat[2,0])/3) > 8e-2) or np.linalg.norm(self.Manipulator.state[3:]) > 1e-2: + err_mat = self.error_matrix.astype(float) + while (np.linalg.norm(err_mat[:,0]) > 5e-2) or (np.linalg.norm(err_mat[:,1]) > 3e-2) or (np.linalg.norm(err_mat[:,2]) > 9e-2): self.Manipulator.apply_voltages(self.calculate_output_voltages()) - + # print(np.linalg.norm(err_mat[:,0]) , np.linalg.norm(err_mat[:,1]) , np.linalg.norm(err_mat[:,2])) + # print(np.linalg.norm(err_mat[:,0]) > 4e-2, np.linalg.norm(err_mat[:,1]) > 3e-2, np.linalg.norm(err_mat[:,2]) > 9e-2) + err_mat = self.error_matrix.astype(float) + if (np.linalg.norm((self.Manipulator.state[self.nDOF: 2*self.nDOF]).astype(float)) < 5e-3): + err_mat = np.zeros((3,3)) def follow_trajectory(self, q_mat, qdot_mat, qdot2_mat, t, ee_pos_values = None): # t: Total time for execution of trajectory diff --git a/Controllers/PIDController.py b/Controllers/PIDController.py index f29c653..e3cf30e 100644 --- a/Controllers/PIDController.py +++ b/Controllers/PIDController.py @@ -4,10 +4,11 @@ class PID_Position_Controller: # Simply applies individual joint PID control to achieve the target position - def __init__(self, RRMmodel, Kp = [1450, 431, 1e4], Ki = [0,0,0], Kd = [578, 170, 658]): + def __init__(self, RRMmodel, Kp = None, Ki = None, Kd = None): self.Manipulator = RRMmodel - if RRMmodel.type == "SCARA": + ## Kp,Kd Calculations + #For "SCARA": # Near the given coordinates # For CRITICAL DAMPING # For joint 1---- @@ -19,11 +20,8 @@ def __init__(self, RRMmodel, Kp = [1450, 431, 1e4], Ki = [0,0,0], Kd = [578, 170 # For joint 3---- # Jeff = 11 # Beff = 2 ===> For Kp = 1e4 ===> omega_n = 30; Kd = 2*30*11 - 2 = 658 - self.Kp = np.array([1450, 431, 1e4]) - self.Ki = np.array([0,0,0]) - self.Kd = np.array([578, 170, 658]) - elif RRMmodel.type == "PUMA": + #For "PUMA": # Near the given coordinates (5,7,0) # For CRITICAL DAMPING # For joint 1---- @@ -35,11 +33,8 @@ def __init__(self, RRMmodel, Kp = [1450, 431, 1e4], Ki = [0,0,0], Kd = [578, 170 # For joint 3---- # Jeff = 8.33 + 1 = 9.33 # Beff = 2 ===> For omega_n = 10, Kp = 9.33*10**2 = 933; Kd = 2*10*9.33 - 2 = 186.6 - self.Kp = np.array([150, 804, 233.25]) - self.Ki = np.array([0,0,0]) - self.Kd = np.array([58, 319.6, 91.3]) - - elif RRMmodel.type == "Stanford": + + #For "Stanford": # Near the given coordinates (2,3,0) # For CRITICAL DAMPING # For joint 1---- @@ -51,10 +46,42 @@ def __init__(self, RRMmodel, Kp = [1450, 431, 1e4], Ki = [0,0,0], Kd = [578, 170 # For joint 3---- # Jeff = 1 + 1 = 2 # Beff = 2 ===> For omega_n = 10, Kp = 2*10**2 = 200; Kd = 2*10*2 - 2 = 38 - self.Kp = np.array([534, 1367, 200]) - self.Ki = np.array([0,0,0]) - self.Kd = np.array([104.6, 271.4, 38]) + + if Kp is None: + if RRMmodel.type == "SCARA": + self.Kp = np.array([1450, 431, 1e4]) + + elif RRMmodel.type == "PUMA": + self.Kp = np.array([150, 804, 233.25]) + + elif RRMmodel.type == "Stanford": + self.Kp = np.array([534, 1367, 200]) + else: + self.Kp = np.array(Kp) + + if Kd is None: + if RRMmodel.type == "SCARA": + self.Kd = np.array([578, 170, 658]) + + elif RRMmodel.type == "PUMA": + self.Kd = np.array([58, 319.6, 91.3]) + + elif RRMmodel.type == "Stanford": + self.Kd = np.array([104.6, 271.4, 38]) + else: + self.Kd = np.array(Kd) + + if Ki is None: + if RRMmodel.type == "SCARA": + self.Ki = np.array([0,0,0]) + elif RRMmodel.type == "PUMA": + self.Ki = np.array([0,0,0]) + + elif RRMmodel.type == "Stanford": + self.Ki = np.array([0,0,0]) + else: + self.Ki = np.array(Ki) self.nDOF = int(len(self.Manipulator.state)/2) self.target_state = np.array(2*self.nDOF*[0.]) @@ -86,6 +113,7 @@ def set_target_state(self, state_tup): def calculate_output_voltages(self): curr_state = self.Manipulator.get_state()[:self.nDOF] error = np.array(self.target_state[:self.nDOF] - curr_state) + # print(error) diff_error = self.target_state[self.nDOF:] - self.Manipulator.get_state()[self.nDOF:] self.integral_error = self.integral_error + (error + self.previous_error)*self.Manipulator.dt/2 @@ -95,15 +123,27 @@ def calculate_output_voltages(self): self.error_matrix[0,:] = self.error_matrix[1,:] self.error_matrix[1,:] = self.error_matrix[2,:] self.error_matrix[2,:] = self.previous_error.T + # print(self.error_matrix) V = ((self.Kp*error + self.Ki*self.integral_error + self.Kd*diff_error)) return sp.Matrix(V) def achieve_target_state(self): - err_mat = self.error_matrix - err_mat[:,2] = self.error_matrix[:,2]*0.01 - while (((np.linalg.norm(err_mat[0,:2]) + np.linalg.norm(err_mat[1,:2]) + np.linalg.norm(err_mat[2,:2]))/3) > 5e-2) or ((np.abs(err_mat[0,0]) + np.abs(err_mat[1,0]) + np.abs(err_mat[2,0])/3) > 8e-2) or np.linalg.norm(self.Manipulator.state[3:]) > 1e-2: + err_mat = self.error_matrix.astype(float) + # err_mat[:,2] = (self.error_matrix[:,2]).astype(float)*0.01 + while ((np.linalg.norm(err_mat[:3,0]) > 2e-2) or (np.linalg.norm(err_mat[:3,1]) > 3e-2) or (np.linalg.norm(err_mat[:3,2]) > 10e-2)): + # print(np.linalg.norm(err_mat[0,:2]) + np.linalg.norm(err_mat[1,:2]) + np.linalg.norm(err_mat[2,:2])/3) + # print((np.abs(err_mat[0,0]) + np.abs(err_mat[1,0]) + np.abs(err_mat[2,0])/3)) + # print(np.linalg.norm(self.Manipulator.state[3:].astype(float))) + # print(err_mat) + # print((err_mat[:3,2])) + # print(np.linalg.norm(err_mat[:3,2])) + # print(np.linalg.norm(err_mat[:3,0]) > 2e-2, np.linalg.norm(err_mat[:3,1]) > 3e-2, np.linalg.norm(err_mat[:3,2]) > 10e-2) self.Manipulator.apply_voltages(self.calculate_output_voltages()) + err_mat = self.error_matrix.astype(float) + if (np.linalg.norm((self.Manipulator.state[self.nDOF: 2*self.nDOF]).astype(float)) < 5e-3): + err_mat = np.zeros((3,3)) + def follow_trajectory(self, q_mat, qdot_mat, t, ee_pos_values = None): diff --git a/Robots/PUMA.py b/Robots/PUMA.py index a91951f..c521fe0 100644 --- a/Robots/PUMA.py +++ b/Robots/PUMA.py @@ -72,7 +72,7 @@ def generate_dynamic_equations(self): [self.q_symbols[2], 0, self.l3, 0, self.m3, I3]]) - print('Starting to calculate dynamic parameters for Stanford Manipulator') + print('Starting to calculate dynamic parameters for PUMA Manipulator') print('_________________________________________________________') self.D, self.C, self.g_ = Dynamic_Equations_from_Dhmi(Dhmi) @@ -149,6 +149,9 @@ def run(self): self.anim_init() self.fig.canvas.manager.set_window_title('Your own PUMA Manipulator: ARKO') + def stop(self): + plt.close(self.fig) + def anim_init(self): q1, q2, q3, *_ = self.state diff --git a/Robots/SCARA.py b/Robots/SCARA.py index 5a7ded9..f88d993 100644 --- a/Robots/SCARA.py +++ b/Robots/SCARA.py @@ -135,6 +135,9 @@ def run(self): self.anim_init() self.fig.canvas.manager.set_window_title('Your own SCARA Manipulator: ARKO') + + def stop(self): + plt.close(self.fig) def anim_init(self): q1, q2, d3, *_ = self.state diff --git a/Robots/Stanford.py b/Robots/Stanford.py index d5522d4..d83045a 100644 --- a/Robots/Stanford.py +++ b/Robots/Stanford.py @@ -127,6 +127,9 @@ def run(self): self.fig.canvas.manager.set_window_title('Your own Stanford Manipulator: ARKO') self.anim_init() + + def stop(self): + plt.close(self.fig) def anim_init(self): q1, q2, d3, *_ = self.state @@ -187,7 +190,7 @@ def set_state(self, state_array): state_array[2] = np.float64(max(min(self.d3_max, state_array[2]), 0)) state_array[5] = (state_array[2] - self.state[2])/self.dt if (abs(state_array[5]) > 5): - print("Clipped the speed of prismatic joint") + # print("Clipped the speed of prismatic joint") state_array[5] = np.float64(max(min(5, state_array[5]), -5)) self.state = state_array diff --git a/test.py b/test.py new file mode 100644 index 0000000..929ede7 --- /dev/null +++ b/test.py @@ -0,0 +1,70 @@ +from load import* + +############################################# +print("Stanford Manipulator") +arko_Stanford,arkoCon_PID = load(StanfordManipulator, PID_Position_Controller) +print(" - PID Controller") +arkoCon_PID.Achieve_EE_Position((2,3,0)) + +print(" - FeedForward Controller") +arko_Stanford.reset() +arkoCon_FF = FeedForward_Position_Controller(arko_Stanford) +arkoCon_FF.Achieve_EE_Position((2,3,0)) + +print(" - Computed Torques Controller") +arko_Stanford.reset() +arkoCon_CT_FF = ComputedTorque_FF_Position_Controller(arko_Stanford) +arkoCon_CT_FF.Achieve_EE_Position((2,3,0)) + +print(" - Multivariable Controller") +arko_Stanford.reset() +arkoCon_MVC = Multivariable_Position_Controller(arko_Stanford) +arkoCon_MVC.Achieve_EE_Position((2,3,0)) + +arko_Stanford.stop() + +############################################# +print("PUMA Manipulator") +arko_PUMA,arkoCon_PID = load(PUMAManipulator, PID_Position_Controller) +print(" - PID Controller") +arkoCon_PID.Achieve_EE_Position((0,5,7)) + +print(" - FeedForward Controller") +arko_PUMA.reset() +arkoCon_FF = FeedForward_Position_Controller(arko_PUMA) +arkoCon_FF.Achieve_EE_Position((0,5,7)) + +print(" - Computed Torques Controller") +arko_PUMA.reset() +arkoCon_CT_FF = ComputedTorque_FF_Position_Controller(arko_PUMA) +arkoCon_CT_FF.Achieve_EE_Position((0,5,7)) + +print(" - Multivariable Controller") +arko_PUMA.reset() +arkoCon_MVC = Multivariable_Position_Controller(arko_PUMA) +arkoCon_MVC.Achieve_EE_Position((0,5,7)) + +arko_PUMA.stop() + +############################################# +print("SCARA Manipulator") +arko_SCARA,arkoCon_PID = load(SCARAManipulator, PID_Position_Controller) +print(" - PID Controller") +arkoCon_PID.Achieve_EE_Position((0.4,0.06,-0.3)) + +print(" - FeedForward Controller") +arko_SCARA.reset() +arkoCon_FF = FeedForward_Position_Controller(arko_SCARA) +arkoCon_FF.Achieve_EE_Position((0.4,0.06,-0.3)) + +print(" - Computed Torques Controller") +arko_SCARA.reset() +arkoCon_CT_FF = ComputedTorque_FF_Position_Controller(arko_SCARA) +arkoCon_CT_FF.Achieve_EE_Position((0.4,0.06,-0.3)) + +print(" - Multivariable Controller") +arko_SCARA.reset() +arkoCon_MVC = Multivariable_Position_Controller(arko_SCARA) +arkoCon_MVC.Achieve_EE_Position((0.4,0.06,-0.3)) + +arko_SCARA.stop() \ No newline at end of file