def __init__(self, actuate_function, localization_function=[]): self.id = None self.actuate = actuate_function self.locate = localization_function self.long_ultimate_gain = 9 self.long_ultimate_period = .55 # should be determined later self.lateral_ultimate_gain = 10 self.lateral_ultimate_period = 999 # should be determined later self.angular_ultimate_gain = 8 self.angular_ultimate_period = .7 # should be determined later self.long_control = Proportional() # Proportional() self.lateral_control = Proportional() self.angular_control = Proportional() self.dynamic_long_control = Proportional() self.dynamic_angular_control = Proportional() self.dynamic_long_control.setGain(self.long_ultimate_gain / 4) self.dynamic_angular_control.setGain(self.lateral_ultimate_gain / 4) self.logger = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]
class PlanarTracker(object): "This class helps ground robots to navigate on different types of reference trajectories/paths, move to a point, or change their facing." def __init__(self, actuate_function, localization_function=[]): self.id = None self.actuate = actuate_function self.locate = localization_function self.long_ultimate_gain = 9 self.long_ultimate_period = .55 # should be determined later self.lateral_ultimate_gain = 10 self.lateral_ultimate_period = 999 # should be determined later self.angular_ultimate_gain = 8 self.angular_ultimate_period = .7 # should be determined later self.long_control = Proportional() # Proportional() self.lateral_control = Proportional() self.angular_control = Proportional() self.dynamic_long_control = Proportional() self.dynamic_angular_control = Proportional() self.dynamic_long_control.setGain(self.long_ultimate_gain / 4) self.dynamic_angular_control.setGain(self.lateral_ultimate_gain / 4) self.logger = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]] def setID(self, agent_id): self.agent_id = agent_id def goToPoint(self, x, y): print 'Going to static point:', x, y bound = .01 self.long_control.setGain(self.long_ultimate_gain / 2) self.lateral_control.setGain(self.lateral_ultimate_gain / 2) # def getError(): # agent_id, x_actual, y_actual,z, theta_actual, pitch, roll = self.locate(self.agent_id) # x_error = x - x_actual # y_error = y - y_actual # long_error, lateral_error = rotate2DimFrame(x_error, y_error, theta_actual) # return long_error, lateral_error pos = self.locate(self.agent_id) agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = pos x_error = x - x_actual y_error = y - y_actual long_error, lateral_error = rotate2DimFrame(x_error, y_error, theta_actual) # long_error, lateral_error = getError() while (abs(long_error) > bound or abs(lateral_error) > bound) and pos == pos: pos = self.locate(self.agent_id) agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = pos x_error = x - x_actual y_error = y - y_actual long_error, lateral_error = rotate2DimFrame(x_error, y_error, theta_actual) feedback_linear = self.long_control.controllerOutput(long_error) # print '% 4.2f, % 4.2f' % (long_error, feedback_linear), x,y, x_actual,y_actual,theta_actual feedback_angular = self.lateral_control.controllerOutput(lateral_error) self.actuate(feedback_linear, feedback_angular) # self.logger = append(self.logger, [[long_error, lateral_error, # x, y, # x_actual, y_actual, # theta_actual, time()]], axis=0) print 'Longitutional error:', long_error, 'm | Lateral error:', lateral_error, 'm' def moveTowardsDynamicPoint(self, distance, theta): agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = self.locate(self.agent_id) long_error = distance * cos(theta) angular_error = wrapAnglePi(theta) #print 'long error:', long_error, 'angle error: ', angular_error #print 'long error: ',long_error,'ang error: ',angular_error # self.logger = append(self.logger, [[0, 0, 0, 0, x_actual, y_actual, theta_actual, time(),0]], axis=0) feedback_linear = self.dynamic_long_control.controllerOutput(long_error) feedback_angular = self.dynamic_angular_control.controllerOutput(angular_error) feedback_linear = max(0, feedback_linear) #print 'cmd lin: ', feedback_linear, 'cmd ang: ', feedback_angular self.actuate(feedback_linear, feedback_angular) def saveLog(self): save('/home/administrator/barzin_catkin_ws/src/path_tracking/scripts/experimental_results/tracker_of_agent_'+str(self.agent_id), self.logger) self.logger = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]] def faceDirection(self, theta_desired): theta_desired = wrapAnglePi(theta_desired) #print 'Facing direction.' bound = .02 self.angular_control.setGain(self.angular_ultimate_gain / 4) theta_error = wrapAnglePi(theta_desired - self.locate(self.agent_id)[4]) while abs(theta_error) > bound: feedback_angular = self.angular_control.controllerOutput(theta_error) # print feedback_angular self.actuate(0, feedback_angular) # print theta_desired,self.locate(self.agent_id)[4] agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = self.locate(self.agent_id) theta_error = wrapAnglePi(theta_desired - theta_actual) self.logger = append(self.logger, [[0, 0,theta_error, 0,0, theta_desired, x_actual, y_actual, theta_actual, time()]], axis=0) self.actuate(0, 0) agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = self.locate(self.agent_id) theta_error = wrapAnglePi(theta_desired - theta_actual) #print 'Angular error:', theta_error, 'radians =', theta_error * 180 / pi, 'degrees' def followTrajectory(self, trajectory, x_calibrate=0, y_calibrate=0): print 'Following Trajectory.' self.long_control.setGain(self.long_ultimate_gain / 2) self.lateral_control.setGain(self.lateral_ultimate_gain / 2) # self.angular_control.setGain(self.angular_ultimate_gain / 2) reference_pos = trajectory.getPosition() # print 'ref pos 1',reference_pos while reference_pos == reference_pos: # check to see if x_reference is not NaN agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = self.locate(self.agent_id) # x_actual, y_actual, theta_actual = self.locate() x_reference, y_reference, theta_reference = reference_pos x_error = x_reference + x_calibrate - x_actual y_error = y_reference + y_calibrate - y_actual long_error, lateral_error = rotate2DimFrame(x_error, y_error, theta_actual) angular_error = theta_reference - theta_actual feedback_linear = self.long_control.controllerOutput(long_error) feedback_angular = self.lateral_control.controllerOutput(lateral_error) # feedback_angular = feedback_angular + self.angular_control.controllerOutput(angular_error) self.actuate(feedback_linear, feedback_angular) reference_pos = trajectory.getPosition() # print 'ref pos',reference_pos self.logger = append(self.logger, [[long_error, lateral_error, x_reference + x_calibrate, y_reference + y_calibrate, x_actual, y_actual, theta_actual, time(), 0]], axis=0) save('/home/administrator/barzin_catkin_ws/src/path_tracking/scripts/experimental_results/' + str(datetime.now()) + ' followTrajectory', self.logger) self.logger = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]] print 'Reached end of trajectory.' def followPath(self, path, x_calibrate=0, y_calibrate=0): 'Following path.' self.long_control.setGain(self.long_ultimate_gain / 2) self.lateral_control.setGain(self.lateral_ultimate_gain / 2) # self.angular_control.setGain(self.angular_ultimate_gain / 2) reference_pos = path.getPosition() # print 'ref pos 1',reference_pos while reference_pos == reference_pos: # check to see if x_reference is not NaN agent_id, x_actual, y_actual, z, theta_actual, pitch, roll = self.locate(self.agent_id) # x_actual, y_actual, theta_actual = self.locate() x_reference, y_reference, theta_reference = reference_pos x_error = x_reference + x_calibrate - x_actual y_error = y_reference + y_calibrate - y_actual long_error, lateral_error = rotate2DimFrame(x_error, y_error, theta_actual) angular_error = theta_reference - theta_actual lateral_penalty = abs(lateral_error**2) * (-100) # angular_penalty = abs(pi/2-theta_actual) * -.4*18/pi vel = max(.5 + lateral_penalty, 0.05) print '% 4.2f, % 4.2f, % 4.2f' % (vel, abs(lateral_error), abs(pi / 2 - theta_actual)) path.setVelocity(vel) feedback_linear = self.long_control.controllerOutput(long_error) feedback_lateral = self.lateral_control.controllerOutput(lateral_error) feedback_angular = self.angular_control.controllerOutput(angular_error) # print feedback_angular, feedback_lateral self.actuate(feedback_linear, feedback_lateral) reference_pos = path.getPosition() # print 'ref pos',reference_pos self.logger = append(self.logger, [[long_error, lateral_error, x_reference + x_calibrate, y_reference + y_calibrate, x_actual, y_actual, theta_actual, time(), vel]], axis=0) save('/home/administrator/barzin_catkin_ws/src/path_tracking/scripts/experimental_results/' + str(datetime.now()) + ' followPath', self.logger) self.logger = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]] print 'Reached end of path.' def followLoop(self): self.long_control.setGain(self.long_ultimate_gain / 2) self.lateral_control.setGain(self.lateral_ultimate_gain / 2) pass