def angle(self, id, desired_th): mult_ang = 0.4 d_th = desired_th - self.cur_posture[id][TH] d_th = helper.trim_radian(d_th) # the robot instead puts the direction rear if the angle difference is large if (d_th > helper.d2r(95)): d_th -= math.pi sign = -1 elif (d_th < helper.d2r(-95)): d_th += math.pi sign = -1 self.set_wheel_velocity(id, -mult_ang * d_th, mult_ang * d_th, False)
def set_target_position(self, id, x, y, scale, mult_lin, mult_ang, max_velocity): damping = 0.35 ka = 0 sign = 1 # calculate how far the target position is from the robot dx = x - self.cur_posture[id][X] dy = y - self.cur_posture[id][Y] d_e = math.sqrt(math.pow(dx, 2) + math.pow(dy, 2)) # calculate how much the direction is off desired_th = (math.pi / 2) if (dx == 0 and dy == 0) else math.atan2(dy, dx) d_th = desired_th - self.cur_posture[id][TH] while (d_th > math.pi): d_th -= 2 * math.pi while (d_th < -math.pi): d_th += 2 * math.pi # based on how far the target position is, set a parameter that # decides how much importance should be put into changing directions # farther the target is, less need to change directions fastly if (d_e > 1): ka = 17 / 90 elif (d_e > 0.5): ka = 19 / 90 elif (d_e > 0.3): ka = 21 / 90 elif (d_e > 0.2): ka = 23 / 90 else: ka = 25 / 90 # if the target position is at rear of the robot, drive backward instead if (d_th > helper.d2r(95)): d_th -= math.pi sign = -1 elif (d_th < helper.d2r(-95)): d_th += math.pi sign = -1 # if the direction is off by more than 85 degrees, # make a turn first instead of start moving toward the target if (abs(d_th) > helper.d2r(85)): self.set_wheel_velocity(id, -mult_ang * d_th, mult_ang * d_th, False) # otherwise else: # scale the angular velocity further down if the direction is off by less than 40 degrees if (d_e < 5 and abs(d_th) < helper.d2r(40)): ka = 0.1 ka *= 4 # set the wheel velocity # 'sign' determines the direction [forward, backward] # 'scale' scales the overall velocity at which the robot is driving # 'mult_lin' scales the linear velocity at which the robot is driving # larger distance 'd_e' scales the base linear velocity higher # 'damping' slows the linear velocity down # 'mult_ang' and 'ka' scales the angular velocity at which the robot is driving # larger angular difference 'd_th' scales the base angular velocity higher # if 'max_velocity' is true, the overall velocity is scaled to the point # where at least one wheel is operating at maximum velocity self.set_wheel_velocity( id, sign * scale * (mult_lin * (1 / (1 + math.exp(-3 * d_e)) - damping) - mult_ang * ka * d_th), sign * scale * (mult_lin * (1 / (1 + math.exp(-3 * d_e)) - damping) + mult_ang * ka * d_th), max_velocity)