def __init__(self, velocity=[0, 0], heading_angle=0, course_angle=0, desired_angle=0, angular_velocity=0, location=[2, 3], rudder=0, sail=0, sample_time=0.1, target=[3, 5], IMU_sample_time=0.1, data_base_sample_time=0.1): #### all the units of angles are rad self.velocity = velocity ###[v,u], where v is the heading angle of the sailboat self.heading_angle = heading_angle self.course_angle = course_angle self.desired_angle = desired_angle self.angular_velocity = angular_velocity ## w self.location = location ###[x,y] self.rudder = rudder ### the positive angle is corresponding to counterclockwise self.sail = sail ### the positive angle is corresponding to counterclockwise self.choosen_angle = 0 self.target = target ###the center of target area[x,y] self.dM=10,which is the radius of the pre-arrived area ,self.dT=5,which is r of target area self.desired_acc = 0 self.frequency = 10 self.dT = 1 ## radius of target area self.dM = 1.8 ## radius of pre arrive self.desired_v = 0 self.maxrudder = math.pi / 4 ##max angle of rudder self.maxsail = math.pi / 12 * 5 self.true_wind = [5, math.pi * 3 / 2] ##[wind speed, direction] self.app_wind = [0, 0] ##[wind speed, direction] self.sample_time = 0.01 ##time for every single update ###need to be adjusted ### self.time = 0 self.rudder_controller = PID() self.sail_speed_controller = PID(0.1, 0.01, 0.05) self.d = 4 self.if_acc = 0 #this part is to add the boundary self.x_value = 1 self.y_value = 1 self.flag = False self.if_keeping = False self.keeping_state = 1 ##1: before turning 2: turning 3:recover 4:go back self.v_list = [0] * 7 self.u_list = [0] * 7 self.w_list = [0] * 7
def __init__(self, velocity=[0, 0], heading_angle=0, course_angle=0, desired_angle=0, angular_velocity=0, location=[2, 3], rudder=0, sail=0, sample_time=0.1, target=[3, 6], IMU_sample_time=0.1, data_base_sample_time=0.1): #### all the units of angles are rad self.velocity = velocity ###[v,u], where v is the heading angle of the sailboat self.heading_angle = heading_angle self.next_desired_angle = 0 self.desired_angle = desired_angle self.angular_velocity = angular_velocity ## w self.location = location ###[x,y] self.rudder = rudder ### the positive angle is corresponding to counterclockwise self.sail = sail ### the positive angle is corresponding to counterclockwise self.last_v = 0 self.roll = 0 self.roll_angular_velocity = 0 self.target = target ###the center of target area[x,y] self.dM=10,which is the radius of the pre-arrived area ,self.dT=5,which is r of target area self.frequency = 10 self.dT = 1.4 ## radius of target area self.dM = 2.8 ## radius of pre arrive self.desired_v = 0 self.maxrudder = math.pi / 4 ##max angle of rudder self.maxsail = math.pi / 12 * 5 self.true_wind = [3, math.pi * 3 / 2] ##[wind speed, direction] self.app_wind = [3, math.pi * 3 / 2] ##[wind speed, direction] ###need to be adjusted ### self.tacking_sign = 0 self.tacking_velocity = 0.5 self.find_sail_sign = 1 self.optimal_sail_adjustment = 0 self.p1 = 0 ##drift coefficient####relative big for the plastic sailboat self.p2 = 4 ##tangential friction self.p3 = 2.5 ##angular friction self.p4 = 1.8 ##sail lift self.p5 = 30 ##rudder lift self.p6 = 0.05 ##distance to sail self.p7 = 0.05 ##distance to mast self.p8 = 0.25 ##distance to rudder self.p9 = 3 ##mass of boat self.p10 = 0.3 ##moment of inertia self.p11 = 0.3 self.p12 = 0.002 #wind effect on w self.p13 = 0.045 #wind effect on v self.time = 0 self.rudder_controller = PID() self.d = 3.65 #this part is to add the boundary self.flag = False self.if_keeping = False self.keeping_state = 1 ##1: before turning 2: turning 3:recover 4:go back self.start_tacking_time = -1 self.init_tacking_sign = 0 self.v_list = [0] * 5 self.u_list = [0] * 5 self.w_list = [0] * 5 self.p_list = [0] * 5 self.tacking_state = 'not tacking' self.if_force_turning = False self.tacking_angle = 0 self.go_to_center_start_time = 0 self.go_to_center_angle = 0.5 self.sleep_time = 0
class sailboat: def __init__(self, velocity=[0, 0], heading_angle=0, course_angle=0, desired_angle=0, angular_velocity=0, location=[2, 3], rudder=0, sail=0, sample_time=0.1, target=[3, 5], IMU_sample_time=0.1, data_base_sample_time=0.1): #### all the units of angles are rad self.velocity = velocity ###[v,u], where v is the heading angle of the sailboat self.heading_angle = heading_angle self.course_angle = course_angle self.desired_angle = desired_angle self.angular_velocity = angular_velocity ## w self.location = location ###[x,y] self.rudder = rudder ### the positive angle is corresponding to counterclockwise self.sail = sail ### the positive angle is corresponding to counterclockwise self.choosen_angle = 0 self.target = target ###the center of target area[x,y] self.dM=10,which is the radius of the pre-arrived area ,self.dT=5,which is r of target area self.desired_acc = 0 self.frequency = 10 self.dT = 1 ## radius of target area self.dM = 1.8 ## radius of pre arrive self.desired_v = 0 self.maxrudder = math.pi / 4 ##max angle of rudder self.maxsail = math.pi / 12 * 5 self.true_wind = [5, math.pi * 3 / 2] ##[wind speed, direction] self.app_wind = [0, 0] ##[wind speed, direction] self.sample_time = 0.01 ##time for every single update ###need to be adjusted ### self.time = 0 self.rudder_controller = PID() self.sail_speed_controller = PID(0.1, 0.01, 0.05) self.d = 4 self.if_acc = 0 #this part is to add the boundary self.x_value = 1 self.y_value = 1 self.flag = False self.if_keeping = False self.keeping_state = 1 ##1: before turning 2: turning 3:recover 4:go back self.v_list = [0] * 7 self.u_list = [0] * 7 self.w_list = [0] * 7 ## predict the state for next moment and make decision def update_state(self): global n self.time += 1 if self.time > 3000: self.flag = True #Stop the program self.rudder_control() self.sail_control() self.get_desired_angle(self.target[0], self.target[1]) if self.time % 10 == 0: print('rudder', self.rudder, 'sail', self.sail, 'desired_angle', self.desired_angle) return self.rudder, self.sail, self.desired_angle def rudder_control(self): self.get_app_wind() self.choose_angle() self.get_desired_acc(self.target[0], self.target[1]) if self.keeping_state != 2: self.choosen_angle = self.regular_angle(self.choosen_angle) self.desired_angle = self.regular_angle(self.desired_angle) if math.cos(self.choosen_angle - self.desired_angle) > 0: if self.choosen_angle - self.desired_angle > math.pi / 2: choosen_angle = self.choosen_angle - math.pi * 2 elif self.choosen_angle - self.desired_angle < -math.pi / 2: choosen_angle = self.choosen_angle + math.pi * 2 else: choosen_angle = self.choosen_angle self.rudder = -self.rudder_controller.update( choosen_angle, self.desired_angle) ## if we desire to turn about, set the rudder to the maximum angle, then the sailboat will choose the better diretion for turning. else: self.rudder = self.maxrudder * self.sign( math.sin(self.choosen_angle - self.desired_angle)) if self.velocity[0] < 0: self.rudder = -self.rudder def sail_control(self): ## due to the wind's effect, the sail may not be able to reach its primary maxima if self.if_keeping: if self.keeping_state == 2: self.sail = self.maxsail elif self.keeping_state == 1: if math.cos(self.true_wind[1] - self.heading_angle) > 0.6: optimal_sail = self.maxsail elif math.cos(self.true_wind[1] - self.heading_angle) < -0.73: optimal_sail = self.maxsail else: optimal_sail = 0.25 + ( abs(self.heading_angle - self.true_wind[1] + math.pi) - math.pi / 4) * 0.75 # optimal_sail=abs(math.pi/4*(math.cos(self.true_wind[1]-self.desired_angle)+1)) if self.desired_v > self.velocity[0]: sail = optimal_sail else: sail = (optimal_sail + self.maxsail) / 2 - ( self.desired_v - self.velocity[0]) * 0.8 self.sail = min(self.maxsail, max(sail, 0)) else: self.sail_regular_control() def sail_regular_control(self): global optimal_sail, limit_sail, obtained_angle ##not that angular accelaration of the sail instead of the boat angular_acc = -50 * self.desired_acc obtained_angle = max(angular_acc / self.frequency + abs(self.sail), 0) ###the angle of sail ### if we want the sailboat to speed up, we choose the optimal angle if angular_acc < 0: self.if_acc = 1 if math.cos(self.true_wind[1] - self.heading_angle) > 0.6: optimal_sail = self.maxsail elif math.cos(self.true_wind[1] - self.heading_angle) < -0.73: optimal_sail = self.maxsail else: optimal_sail = 0.25 + ( abs(self.heading_angle - self.true_wind[1] + math.pi) - math.pi / 4) * 0.7 self.sail = min(self.maxsail, max(obtained_angle, abs(optimal_sail))) else: ### we put the sail to limit to slow down self.if_acc = 0 self.sail = min(self.maxsail, obtained_angle) def get_desired_angle(self, target_x, target_y): last_angle = self.desired_angle ### the statagy for reaching the target area #step1 boat_to_target_angle = math.atan2(target_y - self.location[1], target_x - self.location[0]) distance_st = math.sqrt( pow(target_y - self.location[1], 2) + pow(target_x - self.location[0], 2)) #step2 ##this step is for the downwind case where the sailboat is not able to slow down. # The sailboat will therefore turn in a semicicle to arrive (upwind). goal_angle = self.step2(target_x, target_y, boat_to_target_angle) #step 3 ##this is for the upwind case. # If the goal angle is in the dead zone, the sailboat will nevigate in a proper direction. self.step3(goal_angle, distance_st) ###tacking if the velocity is sufficient. Otherwise, the desired angle will remain unchanged. self.get_tacking_state(last_angle) if distance_st < self.dT: self.if_keeping = True self.keeping() else: self.if_keeping = False ###go back before the program stops if self.time > 2800: self.desired_angle = math.atan2(0 - self.location[1], 3.5 - self.location[0]) def step2(self, target_x, target_y, boat_to_target_angle): distance_st = math.sqrt( pow(target_y - self.location[1], 2) + pow(target_x - self.location[0], 2)) #determine if the sailboat is in the pre-arrive area and if it is convinient for the sailboat to turn round and move upwind to the target area if distance_st < self.dM and distance_st > self.dT and math.cos( boat_to_target_angle - self.true_wind[1]) > 0: #if it's in the area and not convinient to, then change the desired orientation to a perpendicular direction afa = math.atan2(target_y - self.location[1], target_x - self.location[0]) # print(self.velocity[0]) if math.cos(afa - (self.true_wind[1] + math.pi / 2)) > math.cos( afa - (self.true_wind[1] - math.pi / 2)): goal_angle = -math.pi / 2 + boat_to_target_angle else: goal_angle = math.pi / 2 + boat_to_target_angle self.keeping_state = 1 self.if_keeping = False else: goal_angle = boat_to_target_angle return goal_angle def step3(self, goal_angle, distance_st): dead_angle = math.pi / 4.5 if math.cos(self.true_wind[1] - goal_angle) + math.cos( math.pi / 2 - dead_angle) > 0: ##if not exceeding dead angle self.desired_angle = goal_angle else: ##if exceeding the dead angle,change the desired angle if distance_st > self.dM: if self.location[0] < 1.6: self.x_value = 1 elif self.location[0] > 5: self.x_value = -1 if self.location[1] < 2: self.y_value = 1 elif self.location[1] > 6: self.y_value = -1 self.desired_angle = math.atan2(self.y_value * 0.8, self.x_value) def keeping(self): boat_to_target_angle = math.atan2(self.target[1] - self.location[1], self.target[0] - self.location[0]) distance_st = math.sqrt( pow(self.target[1] - self.location[1], 2) + pow(self.target[0] - self.location[0], 2)) if distance_st * math.cos(self.true_wind[1] - boat_to_target_angle) > 0.6 * self.dT: self.keeping_state = 4 if self.keeping_state == 1: self.desired_v = self.d * 0.3 * abs( self.true_wind[1] - math.pi - self.desired_angle) / 2 / math.tan(self.maxrudder) if math.sin(self.true_wind[1] - self.heading_angle) > 0: self.desired_angle = self.regular_angle(self.true_wind[1] - math.pi * 0.7) else: self.desired_angle = self.regular_angle(self.true_wind[1] + math.pi * 0.7) if abs(self.velocity[0] - self.desired_v) < 0.1: self.keeping_state = 2 print('state 2') initial_heading_sign = self.sign( math.sin(self.true_wind[1] - self.heading_angle)) ## 1:lefthand side 2:righthand elif self.keeping_state == 2: self.rudder = self.maxrudder * self.sign( math.sin(self.heading_angle - self.desired_angle)) if self.velocity[0] < 0.05 or abs( self.regular_angle(self.heading_angle + math.pi) - self.true_wind[1]) < 0.05: self.keeping_state = 3 if self.velocity[0] < 0.05 and abs( self.regular_angle(self.heading_angle + math.pi) - self.true_wind[1]) > 0.15: self.d += 0.1 elif abs( self.regular_angle(self.heading_angle + math.pi) - self.true_wind[1]) < 0.05 and self.velocity[0] > 0.15: self.d -= 0.1 print('State 3 parameter D', self.d) elif self.keeping_state == 3: self.desired_angle = self.regular_angle(self.true_wind[1] - math.pi) if self.location[1] < self.target[1]: self.keeping_state = 1 print('state 1') elif self.keeping_state == 4: target = [ self.target[0] + math.cos(self.true_wind[1]) * self.dT, self.target[1] + math.sin(self.true_wind[1]) * self.dT ] self.desired_angle = math.atan2(target[1] - self.location[1], target[0] - self.location[0]) if distance_st * math.cos(self.true_wind[1] - boat_to_target_angle) < 0: self.keeping_state = 1 print('State 1') def updata_pos(self, x, y, heading_angle): last_x = self.location[0] last_y = self.location[1] last_heading = self.heading_angle self.location[0] = x self.location[1] = y self.heading_angle = heading_angle self.get_velocity(x, last_x, y, last_y, heading_angle, last_heading) self.course_angle = math.atan2(self.location[1] - last_y, self.location[0] - last_x) def get_velocity(self, x, last_x, y, last_y, heading_angle, last_heading): del_x = x - last_x del_y = y - last_y v = (del_x * math.cos(self.heading_angle) + del_y * math.sin(self.heading_angle)) * self.frequency u = (-del_x * math.sin(self.heading_angle) + del_y * math.cos(self.heading_angle)) * self.frequency w = (heading_angle - last_heading) * self.frequency ### due to the noise, the velocity we choose is the mean value of the velocities in 0.7 second. self.v_list.pop(0) if abs(v) > 3: v = self.v_list[5] self.v_list.append(v) self.u_list.pop(0) if abs(u) > 1: u = self.u_list[4] self.u_list.append(u) self.w_list.pop(0) if abs(w) > 3.5: w = self.w_list[4] self.w_list.append(w) self.velocity[0] = 0 self.velocity[1] = 0 self.angular_velocity = 0 for i in range(0, 7): self.velocity[0] += self.v_list[i] / 7 self.velocity[1] += self.u_list[i] / 7 self.angular_velocity += self.w_list[i] / 7 def get_app_wind(self): ###this part is different from the paper since there might be something wrong in the paper ###get coordinates of true wind self.true_wind = self.get_true_wind()[0] self.app_wind = [ self.true_wind[0] * math.cos(self.true_wind[1] - self.heading_angle) - self.velocity[0], self.true_wind[0] * math.sin(self.true_wind[1] - self.heading_angle) - self.velocity[1] ] ###convert into polar system angle = math.atan2(self.app_wind[1], self.app_wind[0]) self.app_wind = [ math.sqrt(pow(self.app_wind[1], 2) + pow(self.app_wind[0], 2)), angle ] return self.app_wind ##owing to the course angle may not be equal to the heading angle, if the difference is large, we use the heading angle ## the goal of this part is not so clear and needs some discussion def choose_angle(self): self.course_angle = self.regular_angle(self.course_angle) if abs(self.course_angle - self.heading_angle) < math.pi / 18: self.choosen_angle = self.course_angle else: self.choosen_angle = self.heading_angle def get_tacking_state(self, last_angle): ### tack only if the speed is sufficient if math.cos(last_angle - self.true_wind[1]) + math.cos( self.desired_angle - self.true_wind[1]) < 0 and math.cos( last_angle - self.true_wind[1]) > -0.8: if self.sign(math.sin(self.desired_angle - self.true_wind[1])) != self.sign( math.sin(last_angle - self.true_wind[1]) ) and self.velocity[0] < 0.3: self.desired_angle = self.true_wind[1] * 2 - self.desired_angle self.desired_angle = self.regular_angle(self.desired_angle) def get_true_wind(self): # self.true_wind=[10+5*math.sin(6.28*self.time),math.pi/2+math.pi/12*math.sin(6.28*self.time)] coo_true_wind = [ self.true_wind[0] * math.cos(self.true_wind[1]), self.true_wind[0] * math.sin(self.true_wind[1]) ] return self.true_wind, coo_true_wind ## all angle should be modified into the range [0,2*pi) def regular_angle(self, angle): while angle > math.pi * 2: angle -= math.pi * 2 while angle < 0: angle += math.pi * 2 return angle ##Get desired acceleration def get_desired_acc(self, target_x, target_y): kp = 0.25 ### need adjustment kv = 1.2 ### need adjustment ### the prove of the stability is shown in the last page of the paper target_boat_angle = math.atan2(target_y - self.location[1], target_x - self.location[0]) v0 = max(0, self.velocity[1] * math.tan(self.heading_angle)) ###distance between the sailboat and the center of the target distance_st = math.sqrt( pow(target_y - self.location[1], 2) + pow(target_x - self.location[0], 2)) self.desired_acc = -kv * (self.velocity[0] - v0) + kp * distance_st def sign(self, p): if p > 0: return 1 elif p == 0: return 0 else: return -1
class sailboat: def __init__(self, velocity=[0, 0], heading_angle=0, course_angle=0, desired_angle=0, angular_velocity=0, location=[2, 3], rudder=0, sail=0, sample_time=0.1, target=[3, 6], IMU_sample_time=0.1, data_base_sample_time=0.1): #### all the units of angles are rad self.velocity = velocity ###[v,u], where v is the heading angle of the sailboat self.heading_angle = heading_angle self.next_desired_angle = 0 self.desired_angle = desired_angle self.angular_velocity = angular_velocity ## w self.location = location ###[x,y] self.rudder = rudder ### the positive angle is corresponding to counterclockwise self.sail = sail ### the positive angle is corresponding to counterclockwise self.last_v = 0 self.roll = 0 self.roll_angular_velocity = 0 self.target = target ###the center of target area[x,y] self.dM=10,which is the radius of the pre-arrived area ,self.dT=5,which is r of target area self.frequency = 10 self.dT = 1.4 ## radius of target area self.dM = 2.8 ## radius of pre arrive self.desired_v = 0 self.maxrudder = math.pi / 4 ##max angle of rudder self.maxsail = math.pi / 12 * 5 self.true_wind = [3, math.pi * 3 / 2] ##[wind speed, direction] self.app_wind = [3, math.pi * 3 / 2] ##[wind speed, direction] ###need to be adjusted ### self.tacking_sign = 0 self.tacking_velocity = 0.5 self.find_sail_sign = 1 self.optimal_sail_adjustment = 0 self.p1 = 0 ##drift coefficient####relative big for the plastic sailboat self.p2 = 4 ##tangential friction self.p3 = 2.5 ##angular friction self.p4 = 1.8 ##sail lift self.p5 = 30 ##rudder lift self.p6 = 0.05 ##distance to sail self.p7 = 0.05 ##distance to mast self.p8 = 0.25 ##distance to rudder self.p9 = 3 ##mass of boat self.p10 = 0.3 ##moment of inertia self.p11 = 0.3 self.p12 = 0.002 #wind effect on w self.p13 = 0.045 #wind effect on v self.time = 0 self.rudder_controller = PID() self.d = 3.65 #this part is to add the boundary self.flag = False self.if_keeping = False self.keeping_state = 1 ##1: before turning 2: turning 3:recover 4:go back self.start_tacking_time = -1 self.init_tacking_sign = 0 self.v_list = [0] * 5 self.u_list = [0] * 5 self.w_list = [0] * 5 self.p_list = [0] * 5 self.tacking_state = 'not tacking' self.if_force_turning = False self.tacking_angle = 0 self.go_to_center_start_time = 0 self.go_to_center_angle = 0.5 self.sleep_time = 0 ## predict the state for next moment and make decision def update_state(self): global n self.time += 1 if self.time > 3100: self.flag = True #Stop the program if self.sleeper() != True: self.regular_all_angle() self.rudder_control() self.sail_control() # self.sail,self.rudder=0,0 self.get_desired_angle(self.target[0], self.target[1]) if self.time > 2800: ### it's time to go home self.next_desired_angle = -math.pi / 2 # else: # print('sleep',self.rudder) # self.desired_angle=0.7 # self.rudder=0 # if self.time%10==0: # # print('rudder',self.rudder,'sail',self.sail,'desired_angle',self.desired_angle) # # print('next angle',self.next_desired_angle) # # print('keeping state',self.keeping_state,'d',self.d) # print('adjust sail by',self.optimal_sail_adjustment) # print(self.last_v,self.velocity[0]) return self.rudder, self.sail, self.desired_angle def sleeper(self): if self.sleep_time > 0: self.sleep_time -= 1 return True else: return False def regular_all_angle(self): self.heading_angle = self.regular_angle(self.heading_angle) self.desired_angle = self.regular_angle(self.desired_angle) self.roll = self.regular_angle(self.roll) def rudder_control(self): self.get_app_wind() self.app_wind[1] = self.regular_angle(self.app_wind[1]) if self.keeping_state != 2 and self.keeping_state != 5: self.heading_angle = self.regular_angle(self.heading_angle) self.desired_angle = self.regular_angle(self.desired_angle) if math.cos(self.heading_angle - self.desired_angle) > 0: ##防止坐标在-pi到pi时跳跃 if self.heading_angle - self.desired_angle > math.pi / 2: heading_angle = self.heading_angle - math.pi * 2 elif self.heading_angle - self.desired_angle < -math.pi / 2: heading_angle = self.heading_angle + math.pi * 2 else: heading_angle = self.heading_angle self.rudder = -self.rudder_controller.update( heading_angle, self.desired_angle) self.heading_angle = self.regular_angle(self.heading_angle) ## if we desire to turn about, set the rudder to the maximum angle, then the sailboat will choose the better diretion for turning. else: # print('AAAA') self.rudder = self.maxrudder * self.sign( math.sin(self.heading_angle - self.desired_angle)) if self.velocity[0] < -0.02: self.rudder = 0 # print('opp rudder',self.desired_angle,self.keeping_state) def sail_control(self): self.heading_angle = self.regular_angle(self.heading_angle) # print('if tacking',self.tacking_state) ## due to the wind's effect, the sail may not be able to reach its primary maxima if self.if_keeping: # print('keeping') if self.keeping_state == 2: # print('AAAAAA') self.sail = self.maxsail elif self.keeping_state == 5: if math.cos(self.heading_angle - self.true_wind[1]) < -0.1: self.sail = self.maxsail else: self.sail = 1 elif self.keeping_state == 1: if self.heading_angle < -math.pi / 2: self.heading_angle += math.pi * 2 if abs(self.heading_angle - math.pi / 2) <= math.pi / 4: optimal_sail = 0.2 + (math.pi / 4 - abs( self.heading_angle - math.pi / 2)) * 1.41 else: optimal_sail = 0.2 + (abs(self.heading_angle - math.pi / 2) - math.pi / 4) * 0.47 if self.desired_v > self.velocity[0]: sail = optimal_sail + self.optimal_sail_adjustment self.sail = min(self.maxsail, max(sail, 0)) else: # print('BBBBBB') self.sail = self.maxsail elif self.tacking_state == 'is tacking': self.sail = self.maxsail elif self.if_force_turning == True: self.sail = self.maxsail else: self.sail_regular_control() def finding_optimal_sail(self): # self.optimal_sail_adjustment+=self.find_sail_sign*0.03 self.optimal_sail_adjustment += 0 if self.last_v > self.velocity[0]: self.find_sail_sign *= -1 if self.optimal_sail_adjustment > 0.2: self.optimal_sail_adjustment = 0.2 self.find_sail_sign *= -1 if self.optimal_sail_adjustment < -0.2: self.optimal_sail_adjustment = -0.2 self.find_sail_sign *= -1 def sail_regular_control(self): global optimal_sail, limit_sail, obtained_angle if self.heading_angle < -math.pi / 2: self.heading_angle += math.pi * 2 if abs(self.heading_angle - math.pi / 2) <= math.pi / 4: optimal_sail = 0.25 + ( math.pi / 4 - abs(self.heading_angle - math.pi / 2)) * 1.41 else: optimal_sail = 0.4 + (abs(self.heading_angle - math.pi / 2) - math.pi / 4) * 0.7 optimal_sail += self.optimal_sail_adjustment self.heading_angle = self.regular_angle(self.heading_angle) self.sail = min(self.maxsail, max(optimal_sail, 0)) def get_desired_angle(self, target_x, target_y): boat_to_target_angle = math.atan2(target_y - self.location[1], target_x - self.location[0]) distance_st = math.sqrt( pow(target_y - self.location[1], 2) + pow(target_x - self.location[0], 2)) #step2 ##this step is for the downwind case where the sailboat is not able to slow down. # The sailboat will therefore turn in a semicicle to arrive (upwind). if distance_st > self.dT: self.if_keeping = False self.keeping_state = 1 self.go_to_target_area(boat_to_target_angle, distance_st) elif self.tacking_state == 'not tacking' and self.if_force_turning == False: self.if_keeping = True self.keeping_in_target_area() self.tacking_detector() def go_to_target_area(self, boat_to_target_angle, distance_st): self.get_next_desired_angle(boat_to_target_angle, distance_st) self.tacking_if_I_can() if self.tacking_state == 'is tacking': if self.time - self.start_tacking_time > 0: self.desired_angle = self.tacking_angle # else: # print('aaa',self.sail) else: if self.if_force_turning == True: self.desired_angle = self.regular_angle(self.true_wind[1]) self.rudder = self.maxrudder * self.sign( math.sin(self.heading_angle - self.desired_angle)) if self.velocity[0] < -0.1: self.rudder = -self.rudder if abs( self.regular_angle(self.heading_angle) - self.regular_angle(self.true_wind[1])) < 0.5: self.if_force_turning = 'go back to center' self.go_to_center_start_time = self.time elif self.if_force_turning == 'go back to center': self.desired_angle = self.go_to_center_angle if self.time < self.go_to_center_start_time + 5: self.rudder = self.maxrudder * self.sign( math.sin(-math.pi / 2 - self.go_to_center_angle)) # if self.time<self.go_to_center_start_time+15: if self.time > self.go_to_center_start_time + 50: self.if_force_turning = False else: self.force_to_turn_when_hit_the_boundary(boat_to_target_angle) def tacking_if_I_can(self): if self.tacking_state != 'is tacking': if math.cos(self.heading_angle - self.true_wind[1]) + math.cos( self.next_desired_angle - self.true_wind[1]) < 0: # print(self.next_desired_angle,self.heading_angle) if self.sign( math.sin(self.next_desired_angle - self.true_wind[1])) != self.sign( math.sin(self.heading_angle - self.true_wind[1])): ###Yes, it's a tacking! # print('Yes, it is a tacking!') self.desired_v = 3.5 * 0.3 * abs( math.pi / 2 - self.heading_angle) / 2 / math.tan( self.maxrudder) # print(self.desired_v) if self.velocity[0] > self.desired_v: ## I can tack self.tacking_state = 'is tacking' self.start_tacking_time = self.time self.desired_angle = self.next_desired_angle self.tacking_angle = self.regular_angle( self.next_desired_angle) self.init_tacking_sign = self.sign( math.sin(self.heading_angle - math.pi / 2)) else: self.desired_angle = self.next_desired_angle else: self.desired_angle = self.next_desired_angle self.desired_angle = self.regular_angle(self.desired_angle) def tacking_detector(self): if self.tacking_state == 'is tacking': # print('init sign',self.init_tacking_sign,self.sign(math.sin(self.heading_angle-math.pi/2))) is_success = (self.init_tacking_sign != self.sign( math.sin(self.heading_angle - math.pi / 2))) if self.time - self.start_tacking_time > 30 or self.velocity[ 0] < 0.08 or is_success: if math.cos(self.heading_angle - self.true_wind[1]) > -0.85: self.tacking_state = 'not tacking' if self.init_tacking_sign != self.sign( math.sin(self.heading_angle - math.pi / 2)): print('tacked successfully') self.optimal_sail_adjustment = 0 self.d -= 0.05 else: print('failed tacking') self.if_force_turning = True self.go_to_center_angle = math.pi / 2 - self.sign( math.pi / 2 - self.tacking_angle) self.d += 0.05 def get_next_desired_angle(self, boat_to_target_angle, distance_st): self.next_desired_angle = boat_to_target_angle if distance_st < self.dM and distance_st > self.dT: if math.cos(boat_to_target_angle - self.true_wind[1]) > 0.4: ##not able to slow down if math.cos(boat_to_target_angle - (self.true_wind[1] + math.pi / 2)) > math.cos( boat_to_target_angle - (self.true_wind[1] - math.pi / 2)): self.next_desired_angle = -math.pi / 2 + boat_to_target_angle else: self.next_desired_angle = math.pi / 2 + boat_to_target_angle if math.cos(self.true_wind[1] - self.next_desired_angle) < -0.71: ##exceed dead angle self.next_desired_angle = self.sign( math.sin(self.true_wind[1] - self.next_desired_angle)) * 0.98 + math.pi / 2 # print(self.next_desired_angle,self.true_wind) def force_to_turn_when_hit_the_boundary(self, boat_to_target_angle): if self.location[0] > 1.3 and self.location[0] < 5.6 and self.location[ 1] > -3 and self.location[1] < 7.8: print('', end='') else: if self.location[0] < 2: self.go_to_center_angle = 0.6 elif self.location[0] > 3: self.go_to_center_angle = 2.6 ## turn to downwind direction if math.cos(self.heading_angle - boat_to_target_angle) < 0: self.if_force_turning = True def keeping_in_target_area(self): boat_to_target_angle = math.atan2(self.target[1] - self.location[1], self.target[0] - self.location[0]) distance_st = math.sqrt( pow(self.target[1] - self.location[1], 2) + pow(self.target[0] - self.location[0], 2)) if distance_st * math.cos(self.true_wind[1] - boat_to_target_angle) > 0.8 * self.dT: self.keeping_state = 4 print('state 4') if self.keeping_state == 1: self.state1(boat_to_target_angle) ## 1:lefthand side 2:righthand elif self.keeping_state == 2: self.rudder = self.maxrudder * self.sign( math.sin(self.heading_angle - self.true_wind[1] - math.pi)) self.final_heading_sign = self.sign( math.sin(self.true_wind[1] - math.pi - self.heading_angle)) if self.final_heading_sign != self.initial_heading_sign: if self.initial_heading_sign < 0: if self.heading_angle < math.pi - 0.3: self.keeping_state = 3 else: self.keeping_state = 3 if self.velocity[0] > 0.15: self.d -= 0.1 print('State 3 parameter D', self.d) elif self.velocity[0] < 0.05 and math.cos( self.heading_angle - self.true_wind[1]) > -0.8: self.d += 0.1 self.keeping_state = 5 self.init_tacking_sign = self.sign( math.sin(self.heading_angle - math.pi / 2)) print('Turning fail,State 5', self.d) elif self.keeping_state == 3: ##尝试保持迎风 if self.initial_heading_sign == 1: self.desired_angle = self.regular_angle(self.true_wind[1] - math.pi) else: self.desired_angle = self.regular_angle(self.true_wind[1] - math.pi) - 0.3 if self.location[1] < self.target[1] or abs(self.heading_angle - math.pi / 2) > 0.4: self.keeping_state = 1 print('3-state 1') elif self.keeping_state == 4: target = [ self.target[0] + math.cos(self.true_wind[1]) * self.dT, self.target[1] + math.sin(self.true_wind[1]) * self.dT ] self.desired_angle = math.atan2(target[1] - self.location[1], target[0] - self.location[0]) if distance_st * math.cos(self.true_wind[1] - boat_to_target_angle) < 0: self.keeping_state = 1 print('4-State 1') elif self.keeping_state == 5: self.rudder = -self.maxrudder * self.init_tacking_sign if math.cos(self.heading_angle - (self.true_wind[1] - math.pi - self.init_tacking_sign)) > 0.5: self.keeping_state = 1 print('5-State 1') def state1(self, boat_to_target_angle): self.desired_v = self.d * 0.3 * abs( self.true_wind[1] - math.pi - self.desired_angle)**1.5 / 2 / math.tan(self.maxrudder) if self.desired_v < 0.4: self.desired_v = 0.4 elif self.desired_v > 0.55: self.desired_v = 0.55 if math.sin(self.true_wind[1] - self.heading_angle) > 0: self.desired_angle = self.regular_angle(self.true_wind[1] - math.pi * 0.7) else: self.desired_angle = self.regular_angle(self.true_wind[1] + math.pi * 0.7) if self.desired_v > self.velocity[0]: a = 2 b = 2.4 # print(self.desired_v) total_distance = 1.5 * ( -a * b * math.log(1 - b / a * self.desired_v) - b * self.desired_v) / b**2 remanent_distance = total_distance - 1.5 * ( -a * b * math.log(1 - b / a * self.velocity[0]) - b * self.velocity[0]) / b**2 # print(total_distance,remanent_distance) x = remanent_distance * math.cos( self.desired_angle) + 0.85 * self.sign( math.cos(self.desired_angle)) y = (0.6 * remanent_distance * math.sin(self.desired_angle) + 0.6) print([x, remanent_distance, self.location[0]]) print([self.location[0] + x, self.location[1] + y], (self.location[0] + x - self.target[0])**2 + (self.location[1] + y - self.target[1])**2, self.velocity[0] > self.desired_v) else: x = 0.75 * self.sign(math.cos(self.desired_angle)) y = 0.6 if (self.location[0] + x - self.target[0])**2 + ( self.location[1] + y - self.target[1])**2 > self.dT**2: ##即将出边界 print('out!!', [x, y], self.location) print([self.location[0] + x, self.location[1] + y], (self.location[0] + x - self.target[0])**2 + (self.location[1] + y - self.target[1])**2) print(self.desired_v, self.velocity[0]) if 4.5 < self.location[1] and abs(self.location[0] - 3) < 0.4: self.keeping_state = 5 print('state 1- State 5') self.init_tacking_sign = self.sign( math.sin(self.heading_angle - math.pi / 2)) self.sail = self.maxsail self.sleep_time = 4 else: self.sleep_time = 2 self.keeping_state = 2 self.initial_heading_sign = self.sign( math.sin(self.true_wind[1] - math.pi - self.heading_angle)) else: # print('111') if self.velocity[0] - self.desired_v > -0.05: # print('222') if math.cos(self.heading_angle - boat_to_target_angle) < 0: # print('333') self.sleep_time = self.get_sleeping_time(y, x) if self.sleep_time > 2: self.sleep_time = 2 print('sleep', self.sleep_time) self.sail = self.maxsail self.keeping_state = 2 print('state 2', self.velocity[0], self.desired_v) self.initial_heading_sign = self.sign( math.sin(self.true_wind[1] - math.pi - self.heading_angle)) def get_sleeping_time(self, y, x): c = self.dT C = self.regular_angle(self.heading_angle) - math.atan2( self.target[1] - self.location[1] - y, self.target[0] - self.location[0] - x) a = math.sqrt((self.target[1] - self.location[1] - y)**2 + (self.target[0] - self.location[0] - x)**2) b1 = max( abs(a * math.cos(C) + math.sqrt((a * math.cos(C))**2 + c**2 - a**2)) - 0.4, 0) b2 = max( abs(a * math.cos(C) - math.sqrt((a * math.cos(C))**2 + c**2 - a**2)) - 0.4, 0) b = min(b1, b2) print(a, b, c, C, a * math.cos(C)**2, c**2 - a**2) return int(b * 5) def updata_pos(self, x, y, heading_angle, roll): last_x = self.location[0] last_y = self.location[1] last_heading = self.heading_angle last_roll = self.roll self.location[0] = x self.location[1] = y self.heading_angle = heading_angle self.roll = roll self.get_velocity(x, last_x, y, last_y, heading_angle, last_heading, last_roll) self.course_angle = math.atan2(self.location[1] - last_y, self.location[0] - last_x) self.regular_all_angle() def get_velocity(self, x, last_x, y, last_y, heading_angle, last_heading, last_roll): del_x = x - last_x del_y = y - last_y if self.time % 8 == 0: self.last_v = self.velocity[0] elif self.time % 16 == 7: self.finding_optimal_sail() v = (del_x * math.cos(self.heading_angle) + del_y * math.sin(self.heading_angle)) * self.frequency u = (-del_x * math.sin(self.heading_angle) + del_y * math.cos(self.heading_angle)) * self.frequency w = (heading_angle - last_heading) * self.frequency p = (self.roll - last_roll) * self.frequency ### due to the noise, the velocity we choose is the mean value of the velocities in 0.7 second. self.v_list.pop(0) if abs(v) > 3: v = self.v_list[3] self.v_list.append(v) self.u_list.pop(0) if abs(u) > 1: u = self.u_list[3] self.u_list.append(u) self.w_list.pop(0) if abs(w) > 3.5: w = self.w_list[3] self.w_list.append(w) self.p_list.pop(0) if abs(p) > 2: p = self.p_list[3] self.p_list.append(p) self.velocity[0] = 0 self.velocity[1] = 0 self.angular_velocity = 0 self.roll_angular_velocity = 0 for i in range(0, 5): self.velocity[0] += self.v_list[i] / 5 self.velocity[1] += self.u_list[i] / 5 self.angular_velocity += self.w_list[i] / 5 self.roll_angular_velocity += self.p_list[i] / 5 def get_app_wind(self): ###this part is different from the paper since there might be something wrong in the paper ###get coordinates of true wind self.app_wind = [ self.true_wind[0] * math.cos(self.true_wind[1] - self.heading_angle) - self.velocity[0], self.true_wind[0] * math.sin(self.true_wind[1] - self.heading_angle) - self.velocity[1] ] ###convert into polar system angle = math.atan2(self.app_wind[1], self.app_wind[0]) self.app_wind = [ math.sqrt(pow(self.app_wind[1], 2) + pow(self.app_wind[0], 2)), angle ] return self.app_wind ## all angle should be modified into the range [0,2*pi) def regular_angle(self, angle): while angle > math.pi: angle -= math.pi * 2 while angle < -math.pi: angle += math.pi * 2 return angle def sign(self, p): if p > 0: return 1 elif p == 0: return 0 else: return -1
class sailboat: def __init__(self, velocity=[2, 0], heading_angle=0, course_angle=0, desired_angle=0, angular_velocity=0, location=[2, 3], rudder=0, sail=0, sample_time=0.1, target=[3, 5.5], IMU_sample_time=0.1, data_base_sample_time=0.1): #### all the units of angles are rad self.velocity = velocity ###[v,u], where v is the heading angle of the sailboat self.heading_angle = heading_angle self.next_desired_angle = 0 self.desired_angle = desired_angle self.angular_velocity = angular_velocity ## w self.location = location ###[x,y] self.rudder = rudder ### the positive angle is corresponding to counterclockwise self.sail = sail ### the positive angle is corresponding to counterclockwise self.last_v = 0 self.target = target ###the center of target area[x,y] self.dM=10,which is the radius of the pre-arrived area ,self.dT=5,which is r of target area self.frequency = 10 self.dT = 1.5 ## radius of target area self.dM = 2.5 ## radius of pre arrive self.desired_v = 0 self.maxrudder = math.pi / 4 ##max angle of rudder self.maxsail = math.pi / 12 * 5 self.true_wind = [3, math.pi * 3 / 2] ##[wind speed, direction] self.app_wind = [3, math.pi * 3 / 2] ##[wind speed, direction] ###need to be adjusted ### self.tacking_sign = 0 self.tacking_velocity = 0.5 self.find_sail_sign = 1 self.optimal_sail_adjustment = 0 self.p1 = 0 ##drift coefficient####relative big for the plastic sailboat self.p2 = 4 ##tangential friction self.p3 = 2.5 ##angular friction self.p4 = 1.8 ##sail lift self.p5 = 30 ##rudder lift self.p6 = 0.05 ##distance to sail self.p7 = 0.05 ##distance to mast self.p8 = 0.25 ##distance to rudder self.p9 = 3 ##mass of boat self.p10 = 0.3 ##moment of inertia self.p11 = 0.3 self.p12 = 0.002 #wind effect on w self.p13 = 0.045 #wind effect on v self.time = 0 self.rudder_controller = PID() self.d = 2.3 #this part is to add the boundary self.flag = False self.if_keeping = False self.keeping_state = 1 ##1: before turning 2: turning 3:recover 4:go back self.start_tacking_time = -1 self.init_tacking_sign = 0 self.v_list = [0] * 5 self.u_list = [0] * 5 self.w_list = [0] * 5 self.tacking_state = 'not tacking' self.if_force_turning = False self.tacking_angle = 0 self.go_to_center_start_time = 0 self.go_to_center_angle = 0.3 ## predict the state for next moment and make decision def update_state(self): global n self.time += 1 if self.time > 3100: self.flag = True #Stop the program self.regular_all_angle() self.rudder_control() self.sail_control() # self.sail,self.rudder=0,0 self.get_desired_angle(self.target[0], self.target[1]) # self.desired_angle=0.7 # self.rudder=0 if self.time % 10 == 0: # # print('rudder',self.rudder,'sail',self.sail,'desired_angle',self.desired_angle) # # print('next angle',self.next_desired_angle) # # print('keeping state',self.keeping_state,'d',self.d) print('adjust sail by', self.optimal_sail_adjustment) print(self.last_v, self.velocity[0]) return self.rudder, self.sail, self.desired_angle def regular_all_angle(self): self.heading_angle = self.regular_angle(self.heading_angle) self.desired_angle = self.regular_angle(self.desired_angle) def rudder_control(self): self.get_app_wind() self.app_wind[1] = self.regular_angle(self.app_wind[1]) if self.keeping_state != 2: self.heading_angle = self.regular_angle(self.heading_angle) self.desired_angle = self.regular_angle(self.desired_angle) if math.cos(self.heading_angle - self.desired_angle) > 0: ##防止坐标在-pi到pi时跳跃 if self.heading_angle - self.desired_angle > math.pi / 2: heading_angle = self.heading_angle - math.pi * 2 elif self.heading_angle - self.desired_angle < -math.pi / 2: heading_angle = self.heading_angle + math.pi * 2 else: heading_angle = self.heading_angle self.rudder = -self.rudder_controller.update( heading_angle, self.desired_angle) self.heading_angle = self.regular_angle(self.heading_angle) ## if we desire to turn about, set the rudder to the maximum angle, then the sailboat will choose the better diretion for turning. else: self.rudder = self.maxrudder * self.sign( math.sin(self.heading_angle - self.desired_angle)) if self.velocity[0] < -0.1: self.rudder = -self.rudder def sail_control(self): self.heading_angle = self.regular_angle(self.heading_angle) ## due to the wind's effect, the sail may not be able to reach its primary maxima if self.if_keeping: if self.keeping_state == 2: self.sail = self.maxsail elif self.keeping_state == 1: if self.heading_angle < -math.pi / 2: self.heading_angle += math.pi * 2 if abs(self.heading_angle - math.pi / 2) <= math.pi / 4: optimal_sail = 0.2 + (math.pi / 4 - abs( self.heading_angle - math.pi / 2)) * 1.41 else: optimal_sail = 0.2 + (abs(self.heading_angle - math.pi / 2) - math.pi / 4) * 0.47 if self.desired_v > self.velocity[0]: sail = optimal_sail + self.optimal_sail_adjustment self.sail = min(self.maxsail, max(sail, 0)) else: self.sail = self.maxsail elif self.tacking_state == 'is tacking': self.sail = self.maxsail elif self.if_force_turning: self.sail = self.maxsail else: self.sail_regular_control() def finding_optimal_sail(self): self.optimal_sail_adjustment += self.find_sail_sign * 0.03 if self.last_v > self.velocity[0]: self.find_sail_sign *= -1 if self.optimal_sail_adjustment > 0.2: self.optimal_sail_adjustment = 0.2 self.find_sail_sign *= -1 if self.optimal_sail_adjustment < -0.2: self.optimal_sail_adjustment = -0.2 self.find_sail_sign *= -1 def sail_regular_control(self): global optimal_sail, limit_sail, obtained_angle if self.heading_angle < -math.pi / 2: self.heading_angle += math.pi * 2 if abs(self.heading_angle - math.pi / 2) <= math.pi / 4: optimal_sail = 0.25 + ( math.pi / 4 - abs(self.heading_angle - math.pi / 2)) * 1.41 else: optimal_sail = 0.4 + (abs(self.heading_angle - math.pi / 2) - math.pi / 4) * 0.7 optimal_sail += self.optimal_sail_adjustment self.heading_angle = self.regular_angle(self.heading_angle) self.sail = min(self.maxsail, max(optimal_sail, 0)) def get_desired_angle(self, target_x, target_y): boat_to_target_angle = math.atan2(target_y - self.location[1], target_x - self.location[0]) distance_st = math.sqrt( pow(target_y - self.location[1], 2) + pow(target_x - self.location[0], 2)) #step2 ##this step is for the downwind case where the sailboat is not able to slow down. # The sailboat will therefore turn in a semicicle to arrive (upwind). if distance_st > self.dT: self.if_keeping = False self.keeping_state = 1 self.go_to_target_area(boat_to_target_angle, distance_st) elif self.tacking_state == 'not tacking' and self.if_force_turning == False: self.if_keeping = True self.keeping_in_target_area() if self.time > 2800: ### it's time to go home self.next_desired_angle = math.atan2(0 - self.location[1], 3.5 - self.location[0]) def go_to_target_area(self, boat_to_target_angle, distance_st): self.get_next_desired_angle(boat_to_target_angle, distance_st) self.tacking_if_I_can() self.tacking_detector() if self.tacking_state == 'is tacking': if time.time() - self.start_tacking_time > 0.8: self.desired_angle = self.tacking_angle else: if self.if_force_turning == True: self.desired_angle = self.regular_angle(self.true_wind[1]) self.rudder = self.maxrudder * self.sign( math.sin(self.heading_angle - self.desired_angle)) if self.velocity[0] < -0.1: self.rudder = -self.rudder if abs( self.regular_angle(self.heading_angle) - self.regular_angle(self.true_wind[1])) < 0.5: self.if_force_turning = 'go back to center' self.go_to_center_start_time = self.time elif self.if_force_turning == 'go back to center': self.desired_angle = self.go_to_center_angle if self.time < self.go_to_center_start_time + 5: self.rudder = self.maxrudder * self.sign( math.sin(-math.pi / 2 - self.go_to_center_angle)) # if self.time<self.go_to_center_start_time+15: if self.location[0] > 0.3 and self.location[ 0] < 5.6 and self.location[1] > -3 and self.location[ 1] < 7.3: self.if_force_turning = False else: self.force_to_turn_when_hit_the_boundary() def tacking_if_I_can(self): if self.tacking_state != 'is tacking': if math.cos(self.heading_angle - self.true_wind[1]) + math.cos( self.next_desired_angle - self.true_wind[1]) < 0: # print(self.next_desired_angle,self.heading_angle) if self.sign( math.sin(self.next_desired_angle - self.true_wind[1])) != self.sign( math.sin(self.heading_angle - self.true_wind[1])): ###Yes, it's a tacking! # print('Yes, it is a tacking!') self.desired_v = 2.7 * 0.3 * abs( math.pi / 2 - self.heading_angle) / 2 / math.tan( self.maxrudder) # print(self.desired_v) if self.velocity[0] > self.desired_v: ## I can tack self.tacking_state = 'is tacking' self.start_tacking_time = self.time self.desired_angle = self.next_desired_angle self.tacking_angle = self.regular_angle( self.next_desired_angle) self.init_tacking_sign = self.sign( math.sin(self.heading_angle - math.pi / 2)) else: self.desired_angle = self.next_desired_angle else: self.desired_angle = self.next_desired_angle self.desired_angle = self.regular_angle(self.desired_angle) def tacking_detector(self): if self.tacking_state == 'is tacking': if self.time - self.start_tacking_time > 30 or self.velocity[ 0] < 0.08: if math.cos(self.heading_angle - self.true_wind[1]) > -0.85: self.tacking_state = 'not tacking' if self.init_tacking_sign != self.sign( math.sin(self.heading_angle - math.pi / 2)): print('tacked successfully') self.optimal_sail_adjustment = 0 # self.d-=0.05 else: print('failed tacking') self.if_force_turning = True self.go_to_center_angle = math.pi / 2 - self.sign( math.pi / 2 - self.tacking_angle) # self.d+=0.05 def get_next_desired_angle(self, boat_to_target_angle, distance_st): self.next_desired_angle = boat_to_target_angle if distance_st < self.dM and distance_st > self.dT: if math.cos(boat_to_target_angle - self.true_wind[1]) > 0.4: ##not able to slow down if math.cos(boat_to_target_angle - (self.true_wind[1] + math.pi / 2)) > math.cos( boat_to_target_angle - (self.true_wind[1] - math.pi / 2)): self.next_desired_angle = -math.pi / 2 + boat_to_target_angle else: self.next_desired_angle = math.pi / 2 + boat_to_target_angle if math.cos(self.true_wind[1] - self.next_desired_angle) < -0.71: ##exceed dead angle self.next_desired_angle = self.sign( math.sin(self.true_wind[1] - self.next_desired_angle)) * 0.98 + math.pi / 2 def force_to_turn_when_hit_the_boundary(self): if self.location[0] > 0.3 and self.location[0] < 5.6 and self.location[ 1] > -3 and self.location[1] < 7.3: print('', end='') else: if self.location[0] < 2: self.go_to_center_angle = 0.6 elif self.location[0] > 3: self.go_to_center_angle = 2.6 ## turn to downwind direction self.if_force_turning = True # start_accelerate_x=0 # accelerate_x_distance=0 # start_accelerate_y=0 # accelerate_y_distance=0 # start_accelerate_x=0 # accelerate_x_distance=0 # start_accelerate_y=0 # accelerate_y_distance=0 # def distance_recorder(self): # global tacking_distance # def if_distance_enough(self): def keeping_in_target_area(self): boat_to_target_angle = math.atan2(self.target[1] - self.location[1], self.target[0] - self.location[0]) distance_st = math.sqrt( pow(self.target[1] - self.location[1], 2) + pow(self.target[0] - self.location[0], 2)) if distance_st * math.cos(self.true_wind[1] - boat_to_target_angle) > 0.6 * self.dT: self.keeping_state = 4 if self.keeping_state == 1: self.desired_v = self.d * 0.3 * abs( self.true_wind[1] - math.pi - self.desired_angle) / 2 / math.tan(self.maxrudder) if math.sin(self.true_wind[1] - self.heading_angle) > 0: self.desired_angle = self.regular_angle(self.true_wind[1] - math.pi * 0.7) else: self.desired_angle = self.regular_angle(self.true_wind[1] + math.pi * 0.7) if self.velocity[0] - self.desired_v > 0 and self.velocity[ 0] - self.desired_v < 0.2: # self.if_distance_enough(): self.keeping_state = 2 print('state 2') self.initial_heading_sign = self.sign( math.sin(self.true_wind[1] - math.pi - self.heading_angle)) ## 1:lefthand side 2:righthand elif self.keeping_state == 2: self.rudder = self.maxrudder * self.sign( math.sin(self.heading_angle - self.true_wind[1] - math.pi)) self.final_heading_sign = self.sign( math.sin(self.true_wind[1] - math.pi - self.heading_angle)) if self.final_heading_sign != self.initial_heading_sign: self.keeping_state = 3 if self.velocity[0] > 0.15: self.d -= 0.1 print('State 3 parameter D', self.d) elif self.velocity[0] < 0.05: self.d += 0.1 self.keeping_state = 4 print('Turning fail,State 4') elif self.keeping_state == 3: self.desired_angle = self.regular_angle(self.true_wind[1] - math.pi) if self.location[1] < self.target[1]: self.keeping_state = 1 print('state 1') elif self.keeping_state == 4: target = [ self.target[0] + math.cos(self.true_wind[1]) * self.dT, self.target[1] + math.sin(self.true_wind[1]) * self.dT ] self.desired_angle = math.atan2(target[1] - self.location[1], target[0] - self.location[0]) if distance_st * math.cos(self.true_wind[1] - boat_to_target_angle) < 0: self.keeping_state = 1 print('State 1') def updata_pos(self, x, y, heading_angle): last_x = self.location[0] last_y = self.location[1] last_heading = self.heading_angle self.location[0] = x self.location[1] = y self.heading_angle = heading_angle self.get_velocity(x, last_x, y, last_y, heading_angle, last_heading) self.course_angle = math.atan2(self.location[1] - last_y, self.location[0] - last_x) def get_velocity(self, x, last_x, y, last_y, heading_angle, last_heading): del_x = x - last_x del_y = y - last_y if self.time % 8 == 0: self.last_v = self.velocity[0] elif self.time % 16 == 7: self.finding_optimal_sail() v = (del_x * math.cos(self.heading_angle) + del_y * math.sin(self.heading_angle)) * self.frequency u = (-del_x * math.sin(self.heading_angle) + del_y * math.cos(self.heading_angle)) * self.frequency w = (heading_angle - last_heading) * self.frequency ### due to the noise, the velocity we choose is the mean value of the velocities in 0.7 second. self.v_list.pop(0) if abs(v) > 3: v = self.v_list[3] self.v_list.append(v) self.u_list.pop(0) if abs(u) > 1: u = self.u_list[3] self.u_list.append(u) self.w_list.pop(0) if abs(w) > 3.5: w = self.w_list[3] self.w_list.append(w) self.velocity[0] = 0 self.velocity[1] = 0 self.angular_velocity = 0 for i in range(0, 5): self.velocity[0] += self.v_list[i] / 5 self.velocity[1] += self.u_list[i] / 5 self.angular_velocity += self.w_list[i] / 5 def get_app_wind(self): ###this part is different from the paper since there might be something wrong in the paper ###get coordinates of true wind self.app_wind = [ self.true_wind[0] * math.cos(self.true_wind[1] - self.heading_angle) - self.velocity[0], self.true_wind[0] * math.sin(self.true_wind[1] - self.heading_angle) - self.velocity[1] ] ###convert into polar system angle = math.atan2(self.app_wind[1], self.app_wind[0]) self.app_wind = [ math.sqrt(pow(self.app_wind[1], 2) + pow(self.app_wind[0], 2)), angle ] return self.app_wind ## all angle should be modified into the range [0,2*pi) def regular_angle(self, angle): while angle > math.pi: angle -= math.pi * 2 while angle < -math.pi: angle += math.pi * 2 return angle def sign(self, p): if p > 0: return 1 elif p == 0: return 0 else: return -1
class sailboat: def __init__(self, velocity=[0, 0], choosen_angle=0, heading_angle=0, course_angle=0, desired_angle=0, angular_velocity=0, location=[0, 0], rudder=0, sail=0, jibsail=0, sample_time=0.1, acc_v=0, acc_u=0, acc_w=0, desired_acc=0): #### all the units of angles are rad self.velocity = velocity ###[v,u], where v is the heading angle of the sailboat self.heading_angle = heading_angle self.course_angle = course_angle self.desired_angle = desired_angle self.angular_velocity = angular_velocity ## w self.location = location ###[x,y] self.rudder = rudder ### the positive angle is corresponding to counterclockwise self.sail = sail ### the positive angle is corresponding to counterclockwise self.choosen_angle = choosen_angle #self.jibsail=jibsail ### to be continued self.acc_v = acc_v self.acc_u = acc_u self.acc_w = acc_w self.target = [ 3, 6 ] ###the center of target area[x,y] self.dM=10,which is the radius of the pre-arrived area ,self.dT=5,which is r of target area self.desired_acc = desired_acc self.p1 = 0.01 ##drift coefficient####relative big for the plastic sailboat self.p2 = 2.5 ##tangential friction self.p3 = 0.8 ##angular friction self.p4 = 1.5 ##sail lift self.p5 = 30 ##rudder lift self.p6 = 0.05 ##distance to sail self.p7 = 0.05 ##distance to mast self.p8 = 0.25 ##distance to rudder self.p9 = 3 ##mass of boat self.p10 = 0.3 ##moment of inertia self.p11 = 0.3 ##rudder break coefficient self.dT = 0.7 self.dM = 1.4 self.boat_size = 0.1 ##which equals that the length of the boat is 0.3m self.maxrudder = math.pi / 4 ##max angle of rudder self.maxsail = math.pi / 12 * 5 self.true_wind = [5, math.pi * 3 / 2] ##[wind speed, direction] self.app_wind = [0, 0] ##[wind speed, direction] self.sample_time = 0.01 ##time for every single update self.ks = 50 ###need to be adjusted ### self.aligned_p = 0.1 ####need to be adjusted ##about 15 degrees of the mini sailboat self.q = -1 ##the parameter for tacking which is 1 or -1 self.time = 0 self.rudder_controller = PID() self.n = 0.1 self.if_acc = 0 # self.if_tacking=0 #this part is to add the boundary target_boat_angle = math.atan2(self.target[1] - self.location[1], self.target[0] - self.location[0]) self.x_value = self.sign(math.cos(target_boat_angle)) self.y_value = self.sign(math.sin(target_boat_angle)) self.ser = serial.Serial('COM5', 57600) self.b = 0 # self.th_send=threading.Thread(target=self.send) # self.th_send.setDaemon(False) # self.th_updata=threading.Thread(target=self.to_next_moment) # self.th_updata.setDaemon(False)#守护线程 def get_tacking_state(self, last_angle): if math.cos(last_angle - self.true_wind[1]) + math.cos( self.desired_angle - self.true_wind[1]) < 0 and math.cos( last_angle - self.true_wind[1]) > -0.8: if self.sign(math.sin(self.desired_angle - self.true_wind[1])) != self.sign( math.sin(last_angle - self.true_wind[1]) ) and self.velocity[0] < 0.3: self.desired_angle = self.true_wind[1] * 2 - self.desired_angle self.desired_angle = self.regular_angle(self.desired_angle) def get_true_wind(self): # self.true_wind=[10+5*math.sin(6.28*self.time),math.pi/2+math.pi/12*math.sin(6.28*self.time)] coo_true_wind = [ self.true_wind[0] * math.cos(self.true_wind[1]), self.true_wind[0] * math.sin(self.true_wind[1]) ] return self.true_wind, coo_true_wind def regular_angle(self, angle): while angle > math.pi * 2: angle -= math.pi * 2 while angle < 0: angle += math.pi * 2 return angle def get_app_wind(self): ###this part is different from the paper since there might be something wrong in the paper ###get coordinates of true wind self.true_wind = self.get_true_wind()[0] self.app_wind = [ self.true_wind[0] * math.cos(self.true_wind[1] - self.heading_angle) - self.velocity[0], self.true_wind[0] * math.sin(self.true_wind[1] - self.heading_angle) - self.velocity[1] ] ###convert into polar system angle = math.atan2(self.app_wind[1], self.app_wind[0]) self.app_wind = [ math.sqrt(pow(self.app_wind[1], 2) + pow(self.app_wind[0], 2)), angle ] return self.app_wind def choose_angle(self): ##owing to the course angle may not be equal to the heading angle, if the difference is large, we use the heading angle ## the goal of this part is not so clear and needs some discussion self.course_angle = self.regular_angle(self.course_angle) if abs(self.course_angle - self.heading_angle) < math.pi / 18: self.choosen_angle = self.course_angle else: self.choosen_angle = self.heading_angle def rudder_control(self): self.choosen_angle = self.regular_angle(self.choosen_angle) self.desired_angle = self.regular_angle(self.desired_angle) if math.cos(self.choosen_angle - self.desired_angle) > 0: if self.choosen_angle - self.desired_angle > math.pi / 2: choosen_angle = self.choosen_angle - math.pi * 2 elif self.choosen_angle - self.desired_angle < -math.pi / 2: choosen_angle = self.choosen_angle + math.pi * 2 else: choosen_angle = self.choosen_angle # print([choosen_angle,self.desired_angle]) self.rudder = -self.rudder_controller.update( choosen_angle, self.desired_angle) # print('pid',self.choosen_angle,self.desired_angle) ## if we desire to turn about, set the rudder to the maximum angle ## this part garantee the sailboat will choose the better diretion for turning. else: self.rudder = self.maxrudder * self.sign( math.sin(self.choosen_angle - self.desired_angle)) # print('normal',self.rudder,self.choosen_angle) if self.velocity[0] < 0: self.rudder = -self.rudder def sign(self, p): if p > 0: return 1 elif p == 0: return 0 else: return -1 def sail_control(self): global optimal_sail, limit_sail, obtained_angle ## due to the wind's effect, the sail may not be able to reach its primary maxima # print(self.app_wind) pratical_sail_max = min(abs(math.pi - abs(self.app_wind[1])), self.maxsail) ##not that angular accelaration of the sail instead of the boat angular_acc = -self.ks * (self.desired_acc - self.acc_v) obtained_angle = max(angular_acc * self.sample_time + abs(self.sail), 0) ###the angle of sail limit_sail = max(pratical_sail_max - self.aligned_p, 0) ### if we want the sailboat to speed up, we choose the optimal angle if angular_acc < 0: self.if_acc = 1 optimal_sail = math.pi / 4 * ( math.cos(self.true_wind[1] - self.desired_angle) + 1) self.sail = -self.sign(self.app_wind[1]) * min( self.maxsail, max(obtained_angle, min(abs(optimal_sail), limit_sail))) # print('acc',obtained_angle,abs(optimal_sail),limit_sail,self.acc_v) else: ### we put the sail to limit to slow down self.if_acc = 0 self.sail = -self.sign(self.app_wind[1]) * min( pratical_sail_max, obtained_angle) # print('deacc',obtained_angle,pratical_sail_max,self.app_wind[1]) def get_desired_acc(self, target_x, target_y): kp = 0.25 ### need adjustment kv = 1.2 ### need adjustment ### the prove of the stability is shown in the last page of the paper target_boat_angle = math.atan2(target_y - self.location[1], target_x - self.location[0]) v0 = max(0, self.velocity[1] * math.tan(self.heading_angle)) ###distance between the sailboat and the center of the target distance_st = math.sqrt( pow(target_y - self.location[1], 2) + pow(target_x - self.location[0], 2)) self.desired_acc = -kv * (self.velocity[0] - v0) + kp * distance_st def arrive(self, target_x, target_y): last_angle = self.desired_angle ### the statagy for reaching the target area #step1 boat_to_target_angle = math.atan2(target_y - self.location[1], target_x - self.location[0]) #step2 ##this case is for downwind distance_st = math.sqrt( pow(target_y - self.location[1], 2) + pow(target_x - self.location[0], 2)) #determine if the sailboat is in the pre-arrive area and if it is convinient for the sailboat to turn round and move upwind to the target area if distance_st < self.dM and distance_st > self.dT and math.cos( boat_to_target_angle - self.true_wind[1]) > 0: #if it's in the area and not convinient to, then change the desired orientation to a perpendicular direction afa = math.atan2(target_y - self.location[1], target_x - self.location[0]) # print(self.velocity[0]) if math.cos(afa - (self.true_wind[1] + math.pi / 2)) > math.cos( afa - (self.true_wind[1] - math.pi / 2)): goal_angle = -math.pi / 2 + boat_to_target_angle else: goal_angle = math.pi / 2 + boat_to_target_angle else: goal_angle = boat_to_target_angle #step 3 ##this is for the upwind case dead_angle = math.pi / 5 ##if not exceeding dead angle if math.cos(self.true_wind[1] - goal_angle) + math.cos(math.pi / 2 - dead_angle) > 0: self.desired_angle = goal_angle ###prepare for reaching the dead angle, determine which way is more convinient to tack if math.cos(goal_angle - (self.true_wind[1] + math.pi - dead_angle)) > math.cos( goal_angle - (self.true_wind[1] + math.pi + dead_angle)): self.q = 1 else: self.q = -1 else: if distance_st > self.dM: if self.location[0] < 0.8: self.x_value = 1 elif self.location[0] > 5.5: self.x_value = -1 if self.location[1] < 2: self.y_value = 1 elif self.location[1] > 8: self.y_value = -1 ##if exceeding the dead angle,change the desired angle # self.desired_angle=self.true_wind[1]+math.pi+self.q*(math.pi/2-dead_angle) self.desired_angle = math.atan2(self.y_value * 0.8, self.x_value) #step 4 ##upwind strategy for inside the target area. aim to keep the heading angle being oppisite of the wind direction # print(self.app_wind[1]) ###however, in the practical case, the boat cannot stay in the target area forever, therefore, a plan for re-arriving the target area is proposed ###tacking if the velocity is sufficient # self.desired_angle=self.regular_angle(self.desired_angle) self.get_tacking_state(last_angle) if distance_st < self.dT: self.keeping() # if self.velocity[0]>0: # self.desired_angle=self.true_wind[1]+math.pi # self.sail=-self.sign(self.app_wind[1])*min(self.maxsail,abs(math.pi-abs(self.app_wind[1]))) # elif self.velocity[0]<=0: # self.desired_angle=self.true_wind[1]+math.pi-dead_angle*self.sign(self.velocity[1]) # self.sail=-self.sign(self.app_wind[1])*min(abs(optimal_sail),limit_sail) def keeping(self): boat_to_target_angle = math.atan2( self.target[1] + 0.2 - self.location[1], self.target[0] - self.location[0]) if math.cos(boat_to_target_angle - self.true_wind[1]) < 0: self.desired_angle = boat_to_target_angle self.regular_angle(self.desired_angle) if self.desired_angle < 0.5: self.desired_angle = 1.57 elif self.desired_angle > 2.65: self.desired_angle = 1.57 elif self.desired_angle < 1.57 and self.desired_angle > 0.8: self.desired_angle = 0.8 elif self.desired_angle > 1.57 and self.desired_angle < 2.3: self.desired_angle = 2.3 if self.velocity[0] < 0.1: self.sail = -self.sign(self.app_wind[1]) * min( self.maxsail, abs(optimal_sail), limit_sail) else: self.desired_angle = self.true_wind[1] - math.pi self.sail = -self.sign(self.app_wind[1]) * min( self.maxsail, abs(math.pi - abs(self.app_wind[1]))) def updata_pos(self): ### this part is simply use the sailboat dynamic to simulate the position for the next moment. g_rv = self.p5 * self.velocity[0] * abs(self.velocity[0]) * math.sin( self.rudder) g_ru = self.p5 * self.velocity[1] * abs(self.velocity[1]) * math.cos( self.rudder) g_s = self.p4 * self.app_wind[0] * math.sin(self.sail - self.app_wind[1]) self.acc_v = (g_s * math.sin(self.sail) - g_rv * self.p11 * math.sin(self.rudder) - self.p2 * self.velocity[0] * abs(self.velocity[0]) + self.p1 * pow(self.app_wind[0], 2) * math.cos(self.app_wind[1])) / self.p9 # if self.acc_v>0: # print(g_s*math.sin(self.sail),-g_rv*self.p11*math.sin(self.rudder),-self.p2*self.velocity[0]*abs(self.velocity[0]),self.p1*pow(self.app_wind[0],2)*math.cos(self.app_wind[1]),self.velocity[0]) self.acc_u = (-g_ru * self.p11 * math.cos(self.rudder) - self.p2 * self.velocity[1] * abs(self.velocity[1]) + self.p1 * pow(self.app_wind[0], 2) * math.sin(self.app_wind[1]) / 6) / self.p9 self.acc_w = (g_s * (self.p6 - self.p7 * math.cos(self.sail)) - g_rv * self.p8 * math.cos(self.rudder) - self.p3 * self.angular_velocity * self.velocity[0] * self.sign(self.velocity[0])) / self.p10 # print(g_s*(self.p6-self.p7*math.cos(self.sail)),-g_rv*self.p8*math.cos(self.rudder),-self.p3*self.angular_velocity*self.velocity[0],self.acc_w,self.angular_velocity) self.velocity[0] += self.sample_time * self.acc_v self.velocity[1] += self.sample_time * self.acc_u self.angular_velocity += self.sample_time * self.acc_w # self.velocity[0]+=random.gauss(0,self.velocity[0]/10) # self.velocity[1]+=random.gauss(0,self.velocity[1]/10) # self.angular_velocity+=random.gauss(0,self.angular_velocity/10) last_x = self.location[0] last_y = self.location[1] self.location[0] += ( self.velocity[0] * math.cos(self.heading_angle) - self.velocity[1] * math.sin(self.heading_angle)) * self.sample_time self.location[1] += ( self.velocity[1] * math.cos(self.heading_angle) + self.velocity[0] * math.sin(self.heading_angle)) * self.sample_time self.heading_angle += self.angular_velocity * self.sample_time # print(g_s*(self.p6-self.p7*math.cos(self.sail)),-g_rv*self.p8*math.cos(self.rudder),self.p3*self.angular_velocity*self.velocity[0]) # print('location',self.location,'heading',self.heading_angle,'velocity',self.velocity) self.course_angle = math.atan2(self.location[1] - last_y, self.location[0] - last_x) def to_next_moment(self): global n self.time += self.sample_time # while True: # updata_time=time.time() # if time.time()-updata_time>0.02: # updata_time=time.time self.get_app_wind() self.updata_pos() self.choose_angle() self.rudder_control() self.get_desired_acc(self.target[0], self.target[1]) self.sail_control() self.arrive(self.target[0], self.target[1]) def send_and_read(self): # last_send_time=time.time() # while True: # if time.time()-last_send_time>0.1: rudder = 90 - self.sign(self.rudder) * pow(self.rudder * 57.32, 2) / 65 sail = -self.sail * 57.32 * 1.4 + 91 last_send_time = time.time() command = 100 * int(rudder) + sail # print(rudder,sail,command) # print('rudder',90+output*33/255,end=' ') command = (',' + str(command)).encode(encoding='utf-8') self.ser.write(command) mess = 0 mess = self.ser.readline() mess = bytes.decode(mess) mess = str(mess) if mess != 0: # print(mess) a = mess.split('\n')[0] a = re.sub('\D', '', a) try: self.b = int(a) except: self.b = self.b if self.ser.inWaiting() > 6: self.ser.flushInput() def start(self): self.th_send.start() self.th_updata.start()
def __init__(self, velocity=[0, 0], choosen_angle=0, heading_angle=0, course_angle=0, desired_angle=0, angular_velocity=0, location=[0, 0], rudder=0, sail=0, jibsail=0, sample_time=0.1, acc_v=0, acc_u=0, acc_w=0, desired_acc=0): #### all the units of angles are rad self.velocity = velocity ###[v,u], where v is the heading angle of the sailboat self.heading_angle = heading_angle self.course_angle = course_angle self.desired_angle = desired_angle self.angular_velocity = angular_velocity ## w self.location = location ###[x,y] self.rudder = rudder ### the positive angle is corresponding to counterclockwise self.sail = sail ### the positive angle is corresponding to counterclockwise self.choosen_angle = choosen_angle #self.jibsail=jibsail ### to be continued self.acc_v = acc_v self.acc_u = acc_u self.acc_w = acc_w self.target = [ 3, 6 ] ###the center of target area[x,y] self.dM=10,which is the radius of the pre-arrived area ,self.dT=5,which is r of target area self.desired_acc = desired_acc self.p1 = 0.01 ##drift coefficient####relative big for the plastic sailboat self.p2 = 2.5 ##tangential friction self.p3 = 0.8 ##angular friction self.p4 = 1.5 ##sail lift self.p5 = 30 ##rudder lift self.p6 = 0.05 ##distance to sail self.p7 = 0.05 ##distance to mast self.p8 = 0.25 ##distance to rudder self.p9 = 3 ##mass of boat self.p10 = 0.3 ##moment of inertia self.p11 = 0.3 ##rudder break coefficient self.dT = 0.7 self.dM = 1.4 self.boat_size = 0.1 ##which equals that the length of the boat is 0.3m self.maxrudder = math.pi / 4 ##max angle of rudder self.maxsail = math.pi / 12 * 5 self.true_wind = [5, math.pi * 3 / 2] ##[wind speed, direction] self.app_wind = [0, 0] ##[wind speed, direction] self.sample_time = 0.01 ##time for every single update self.ks = 50 ###need to be adjusted ### self.aligned_p = 0.1 ####need to be adjusted ##about 15 degrees of the mini sailboat self.q = -1 ##the parameter for tacking which is 1 or -1 self.time = 0 self.rudder_controller = PID() self.n = 0.1 self.if_acc = 0 # self.if_tacking=0 #this part is to add the boundary target_boat_angle = math.atan2(self.target[1] - self.location[1], self.target[0] - self.location[0]) self.x_value = self.sign(math.cos(target_boat_angle)) self.y_value = self.sign(math.sin(target_boat_angle)) self.ser = serial.Serial('COM5', 57600) self.b = 0
def execute_next(self): action = self.actions.pop(0) direction = None if action == "MoveF" or action == "MoveB": current_pose = self.pose quat = (current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) current_yaw = euler[2] if current_yaw > (-math.pi / 4.0) and current_yaw < (math.pi / 4.0): print "Case 1" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x += 0.5 #direction = 'x' #incr y co-ordinate elif current_yaw > (math.pi / 4.0) and current_yaw < (3.0 * math.pi / 4.0): print "Case 2" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y += 0.5 #direction = 'y' #decr x co elif current_yaw > (-3.0 * math.pi / 4.0) and current_yaw < (-math.pi / 4.0): print "Case 3" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y -= 0.5 #direction = '-y' else: print "Case 4" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x -= 0.5 #direction = '-x' PID(target_pose, "linear").publish_velocity() elif action == "TurnCW" or action == "TurnCCW": current_pose = self.pose quat = (current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) yaw = euler[2] if action == "TurnCW": target_yaw = yaw - (math.pi / 2.0) if target_yaw < -math.pi: target_yaw += (math.pi * 2) else: target_yaw = yaw + (math.pi / 2.0) if target_yaw >= (math.pi): target_yaw -= (math.pi * 2) target_pose = Pose() target_pose.position = current_pose.position target_quat = Quaternion(*tf.transformations.quaternion_from_euler( euler[0], euler[1], target_yaw)) target_pose.orientation = target_quat print target_pose.orientation PID(target_pose, "rotational").publish_velocity() else: print "Invalid action" exit(-1) if len(self.actions) == 0: self.status_publisher.publish(self.free)