Skip to content

Commit

Permalink
Configured the controllers for all three robots
Browse files Browse the repository at this point in the history
  • Loading branch information
@videh25
videh25 committed Jan 24, 2022
1 parent 3824a1d commit 49cce00
Show file tree
Hide file tree
Showing 8 changed files with 324 additions and 42 deletions.
89 changes: 81 additions & 8 deletions Controllers/ComputedTorqueFFController.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.])
Expand Down Expand Up @@ -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
Expand Down
99 changes: 89 additions & 10 deletions Controllers/FeedForwardController.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.])
Expand Down Expand Up @@ -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
Expand Down
21 changes: 16 additions & 5 deletions Controllers/MultivariableController.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
74 changes: 57 additions & 17 deletions Controllers/PIDController.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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----
Expand All @@ -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----
Expand All @@ -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----
Expand All @@ -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.])
Expand Down Expand Up @@ -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
Expand All @@ -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):
Expand Down
Loading

0 comments on commit 49cce00

Please sign in to comment.