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)
