def curve_to_angle(self, desired_angle, actual_angle):
twist = Twist()
diff = get_smallest_signed_angular_difference(desired_angle,
actual_angle)
if diff > 0:
twist.angular = -self.angular_speed
else:
twist.angular = self.angular_speed
#if fabs(diff) < pi/10.0:
twist.linear = self.linear_speed
return twist
def curve_to_angle(self, desired_angle, actual_angle):
diff = get_smallest_signed_angular_difference(actual_angle, desired_angle)
return self.diff_to_twist_bang_bang(diff)
def act(self, robot, sensor_dump, visualize=False):
""" Set wheel speeds according to processed sensor data. """
image = sensor_dump.lower_image
# Check if there are no landmarks in view, but there are wall pixels we
# can use for guidance.
react_to_wall = False
react_to_wall_angle = None
# if self.guidance_vec == None:
closest_wall_angle = None
closest_wall_range = self.wall_react_range
for i in range(image.n_rows):
if image.masks[i] == WALL_MASK:
wall_angle = image.calib_array[i,4]
wall_range = image.calib_array[i,5]
if wall_range < closest_wall_range:
closest_wall_range = wall_range
react_to_wall_angle = wall_angle
react_to_wall = True
# Check for the presence of another robot in the centre of the image.
react_to_robot = False
react_to_robot_angle = None
for i in range(image.n_rows):
if image.masks[i] == ROBOT_MASK:
(xr, yr) = image.calib_array[i,2], image.calib_array[i,3]
bot_a = image.calib_array[i,4]
bot_r = image.calib_array[i,5]
if fabs(bot_a) < pi/4 and bot_r < self.robot_react_range:
react_to_robot = True
react_to_robot_angle = bot_a
# Quantity referenced in a couple of places below:
if self.guidance_vec != None:
dist_to_lmarks = sqrt(self.guidance_vec[0]**2 +
self.guidance_vec[1]**2)
twist = Twist()
action_print = None
if react_to_robot:
# Turn slow in the open direction
twist.linear = self.slow_factor * self.linear_speed
twist.angular = self.angular_speed
action_print = "ROBOT"
if visualize:
draw_ray(robot, react_to_robot_angle, color=(255,0,0))
elif react_to_wall:
action_print = "WALL"
if visualize:
draw_ray(robot, react_to_wall_angle, color=(255,0,255))
#twist = self.curve_to_angle(pi/2, react_to_wall_angle, self.wall_turn_on_spot)
twist.linear = uniform(-0.1, 0.1) * self.linear_speed
twist.angular = -self.angular_speed
elif self.guidance_vec == None:
# No landmarks in view AND no walls in view. Just go straight.
twist.linear = self.linear_speed
action_print = "STRAIGHT (NO LANDMARKS OR WALLS IN VIEW)"
elif self.target_pos != None and dist_to_lmarks > self.puck_dist_threshold:
##twist.linear = self.linear_speed * sign(self.target_pos[0])
##twist.angular = self.angular_speed * sign(self.target_pos[1])
#twist.linear = self.linear_speed * 1.0 * self.target_pos[0]
#twist.angular = self.angular_speed * 20.0 * self.target_pos[1]
#twist = self.cap_twist(twist)
angle = atan2(self.target_pos[1], self.target_pos[0])
twist.linear = self.linear_speed * cos(angle)
twist.angular = self.angular_speed * sin(angle)
action_print = "TARGETING"
if visualize:
draw_segment_wrt_robot(robot, (0, 0),
(self.target_pos[0], self.target_pos[1]),
color=(255, 255, 255), width=3)
else:
# FIRST METHOD: NO CONTROL OVER DISTANCE
#twist = self.curve_to_angle(-pi/2, self.guidance_angle, self.no_target_turn_on_spot)
# SECOND METHOD: BANG-BANG CONTROL OVER DISTANCE BY CURVING IN OR OUT
# BY A FIXED ANGLE
"""
dist = sqrt(self.guidance_vec[0]**2 + self.guidance_vec[1]**2)
gamma = self.curve_gamma
twist = self.curve_to_angle(-pi/2 + gamma, self.guidance_angle)
if dist < self.lmark_ideal_range:
twist = self.curve_to_angle(-pi/2 - gamma, self.guidance_angle)
action_print = "CURVING OUT"
if visualize:
draw_ray(robot, gamma, color=(255,0,0))
else:
twist = self.curve_to_angle(-pi/2 + gamma, self.guidance_angle)
action_print = "CURVING IN"
if visualize:
draw_ray(robot, -gamma, color=(0,255,0))
"""
dist = dist_to_lmarks
ideal_dist = self.lmark_ideal_range
# We want to use proportional control to keep the robot at
# self.lmark_ideal_range away from the landmarks, while facing
# orthogonal to them. So we need an error signal for distance.
# The most simpleminded such signal is this one:
# dist_error = ideal_dist - dist
# But its unknown scale presents a problem. So instead we use the
# following which lies in the range [-1, 1]:
dist_error = (ideal_dist - min(dist, 2*ideal_dist)) / (ideal_dist)
# One consequence of using this error signal is that the response
# at a distance greater than 2*ideal_dist is the same as at
# 2*ideal_dist.
# But we also have to consider the current angle w.r.t. the
# guidance angle. Ideally, the guidance angle should be at -pi/2
# w.r.t. the robot's forward axis when dist_error == 0. If
# dist_error is positive (meaning we are too close to the
# landmarks) then the ideal guidance angle should be in the range
# [-pi, -pi/2]. Otherwise, the ideal guidance angle should be in
# the range [0, -pi/2]. We will use dist_error to scale linearly
# within these ranges as we compute the ideal guidance angle.
pi_2 = pi / 2.0
ideal_guidance_angle = -pi_2 - pi_2 * dist_error
action_print = "PROPORTIONAL CONTROL"
# Now define the angle_error
angle_error = get_smallest_signed_angular_difference(
self.guidance_angle,
ideal_guidance_angle)
twist = self.diff_to_twist_bang_bang(angle_error)
landmark_print = None
if self.closest_pair != None:
landmark_print = "PAIR"
if visualize:
draw_segment_wrt_robot(robot, self.closest_pair[0],
self.closest_pair[1], color=(255,255,255), width=3)
draw_segment_wrt_robot(robot, (0,0), self.guidance_vec,
color=(0,255,255), width=3)
elif self.guidance_vec != None:
landmark_print = "SINGLE"
if visualize:
draw_segment_wrt_robot(robot, (0,0), self.guidance_vec,
color=(255,255,0), width=3)
else:
landmark_print = "NO LANDMARK"
if visualize:
print("{}: {}".format(landmark_print, action_print))
return twist