def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
print "x, y, theta: {}, {}, {} ".format(this_robot.body.position.x,
this_robot.body.position.y,
this_robot.body.angle)
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
n = len(rscan.ranges)
target = self.get_target(rscan)
# Convert to an angle
if target == None:
target_angle = None
else:
target_angle = (rscan.ANGLE_MIN + target * (rscan.ANGLE_MAX - rscan.ANGLE_MIN) / float(rscan.NUMBER_POINTS))
if self.modulate and target_angle != None:
compass_angle = normalize_angle_pm_pi(this_robot.body.angle)
if cos(2*compass_angle) < 0:
# Dig in to push the puck more
target_angle = target_angle - self.ingress_angle
else:
# Pull out to skirt the puck
target_angle = target_angle + self.egress_angle
# 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(0, n):
if (rscan.masks[i] == ROBOT_MASK and fabs(rscan.angles[i]) < pi/4 and
rscan.ranges[i] < 2*this_robot.radius):
react_to_robot = True
react_to_robot_angle = (rscan.ANGLE_MIN + i *
(rscan.ANGLE_MAX - rscan.ANGLE_MIN) / float(rscan.NUMBER_POINTS))
if react_to_robot:
# Turn left and slow
twist.linear = self.slow_factor * self.linear_speed
twist.angular = self.angular_speed
if visualize: # Reacting to robot
self.draw_line(this_robot, rscan, react_to_robot_angle,
(255, 0, 0))
elif (target_angle == None or target_angle <= 0):
# Turn right
twist.linear = self.linear_speed
twist.angular = - self.angular_speed
if visualize:
if target_angle != None:
self.draw_line(this_robot, rscan, target_angle, (0, 255, 0))
else:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
self.draw_line(this_robot, rscan, target_angle, (255, 0, 255))
return twist
def react(self, robot, sensor_suite, visualize=False):
range_scan = sensor_suite.range_scan
twist = Twist()
twist.linear = 4
# Find closest distance < max range
closestI = None
closestRange = float('inf')
for i in range(len(range_scan.ranges)):
r = range_scan.ranges[i]
if r < closestRange and r < range_scan.RANGE_MAX:
closestI = i
closestRange = r
if closestI != None:
if range_scan.angles[closestI] != 0:
twist.angular = -1 / range_scan.angles[closestI]
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
pucks = sensor_suite.puck_scan.pucks
robots = sensor_suite.robot_scan.robots
# First find the puck with the smallest absolute angle.
frontal_puck_abs_angle = float('inf')
frontal_puck_range = float('inf')
for puck in pucks:
if puck.kind == 0 and fabs(puck.angle) < frontal_puck_abs_angle:
frontal_puck_abs_angle = fabs(puck.angle)
frontal_puck_range = puck.distance
# Now find the robot with the smallest absolute angle.
frontal_bot_abs_angle = float('inf')
frontal_bot_range = float('inf')
for robot in robots:
if fabs(robot.angle) < frontal_bot_abs_angle:
frontal_bot_abs_angle = fabs(robot.angle)
frontal_bot_range = robot.distance
# Set the two predicates 'react_to_puck' and 'react_to_robot'
react_to_puck = frontal_puck_abs_angle <= FRONT_THRESHOLD
react_to_robot = frontal_bot_abs_angle <= FRONT_THRESHOLD
if react_to_puck and react_to_robot:
# Choose which is closer, then turn off the other.
if frontal_puck_range < frontal_bot_range:
react_to_robot = False
else:
react_to_puck = False
# At this point only one of the two predicates should be true.
assert not (react_to_robot and react_to_puck)
# Now react...
if react_to_puck:
# Turn right
twist.linear = 4
twist.angular = -2.0
elif react_to_robot:
# Turn left and slow
twist.linear = 2
twist.angular = 2.0
else:
# Turn left
twist.linear = 4
twist.angular = 2.0
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
lscan = sensor_suite.landmark_scan
n = len(rscan.ranges)
(left, right) = self.get_closest_landmark_pair_leftmost_indices(lscan)
(left_angle, right_angle) = (self.index_to_angle(lcan, left),
self.index_to_angle(lcan, left))
self.draw_line(this_robot, rscan, left_angle, (255, 0, 0))
self.draw_line(this_robot, rscan, right_angle, (0, 255, 0))
def __init__(self):
self.mass = 1 # 1 kg
# 0.1 meter radius, converted to pixels for display
#self.radius = 0.05 * M_TO_PIXELS
# Bupimo: 0.111 meter radius, converted to pixels for display
self.radius = 0.111 * M_TO_PIXELS
# moment of inertia for disk
rob_I = moment_for_circle(self.mass, 0, self.radius)
self.body = Body(self.mass, rob_I)
self.body.position = 0, 0
self.body.angle = 0
self.body.velocity = 0, 0
self.body.angular_velocity = 0
"""
self.shape = Circle(self.body, self.radius)
self.shape.color = 127, 0, 255 # a pinkish blue
self.shape.filter = ShapeFilter(categories = ROBOT_MASK)
"""
"""
r = self.radius
p = self.radius / 2.0 # Amount the wedge part pokes out.
vertices = [(r+p, r),
(-r/3, r),
(-r, 0),
(-r/3, -r),
(r/3, -r) ]
"""
r = self.radius
d = self.radius * 1.5 # Amount the wedge part pokes out.
vertices = [(0, -r),
(d, 0),
(0, r)]
# Now add the semicircular back part
n = 3
angles = [pi/2 + i*(pi/n) for i in range(1, n)]
for a in angles:
vertices.append((r*cos(a), r*sin(a)))
vertices = vertices[::-1]
self.shape = Poly(self.body, vertices)
self.shape.color = 127, 0, 255 # a pinkish blue
self.shape.filter = ShapeFilter(categories = ROBOT_MASK)
self.command = Twist()
def __init__(self):
self.mass = 1 # 1 kg
# e-puck: 0.037 meter radius, converted to pixels for display
#self.radius = 3.7 * CM_TO_PIXELS
# Bupimo: 9 cm radius, converted to pixels for display
self.radius = 9 * CM_TO_PIXELS
self.circular = False;
if self.circular:
rob_I = moment_for_circle(self.mass, 0, self.radius)
self.body = Body(self.mass, rob_I)
self.shape = Circle(self.body, self.radius)
else:
r = self.radius
d = self.radius * 1.5 # Amount the wedge part pokes out.
vertices = [(0, -r),
(d, 0),
(0, r)]
#vertices = [(0, -r),
# (d/4, 0),
# (d/2, r)]
# Now add the semicircular back part
n = 5
angles = [pi/2 + i*(pi/n) for i in range(1, n)]
for a in angles:
vertices.append((r*cos(a), r*sin(a)))
rob_I = moment_for_poly(self.mass, vertices)
self.body = Body(self.mass, rob_I)
self.shape = Poly(self.body, vertices)
# EXPERIMENTAL: Add bristles to robot
# rest_length = 100
# stiffness = 500
# damping = 10
# self.bristle_body = pymunk.DampedSpring(self.body, body, \
# (0,0), (0,0), rest_length, stiffness, damping)
# self.sim.env.add(self.spring_body)
self.body.position = 0, 0
self.body.angle = 0
self.body.velocity = 0, 0
self.body.angular_velocity = 0
self.shape.color = 0, 255, 0
self.shape.filter = ShapeFilter(categories = ROBOT_MASK)
self.command = Twist()
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
pucks = sensor_suite.puck_scan.pucks
robots = sensor_suite.robot_scan.robots
# First find the puck with the smallest absolute angle.
frontal_puck_abs_angle = float('inf')
frontal_puck_range = float('inf')
for puck in pucks:
if puck.kind == 0 and fabs(puck.angle) < frontal_puck_abs_angle:
frontal_puck_abs_angle = fabs(puck.angle)
frontal_puck_range = puck.distance
# Now find the robot with the smallest absolute angle.
frontal_bot_abs_angle = float('inf')
frontal_bot_range = float('inf')
for robot in robots:
if fabs(robot.angle) < frontal_bot_abs_angle:
frontal_bot_abs_angle = fabs(robot.angle)
frontal_bot_range = robot.distance
# Set the two predicates 'react_to_puck' and 'react_to_robot'
react_to_puck = frontal_puck_abs_angle <= FRONT_THRESHOLD
react_to_robot = frontal_bot_abs_angle <= FRONT_THRESHOLD
if react_to_puck and react_to_robot:
# Choose which is closer, then turn off the other.
if frontal_puck_range < frontal_bot_range:
react_to_robot = False
else:
react_to_puck = False
# At this point only one of the two predicates should be true.
assert not (react_to_robot and react_to_puck)
# Now react...
if react_to_puck:
# Turn right
twist.linear = 4
twist.angular = -2.0
elif react_to_robot:
# Turn left and slow
twist.linear = 2
twist.angular = 2.0
else:
# Turn left
twist.linear = 4
twist.angular = 2.0
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
n = len(rscan.ranges)
target = self.get_leftmost_pixel(rscan)
# Convert to an angle
if target == None:
target_angle = None
else:
target_angle = self.index_to_angle(rscan, target)
# EXPERIMENT TO ACHIEVE A MINIMUM SIZE AGGREGATE.
lscan = sensor_suite.landmark_scan
closest_lmark_distance = float('inf')
for i in range(len(lscan.ranges)):
if (lscan.masks[i] & ANY_LANDMARK_MASK != 0
and lscan.ranges[i] < closest_lmark_distance):
closest_lmark_distance = lscan.ranges[i]
if closest_lmark_distance < self.circle_radius:
target_angle = target_angle + self.egress_angle
if self.modulate and target_angle != None:
compass_angle = normalize_angle_pm_pi(this_robot.body.angle)
if cos(2*compass_angle) < 0:
# Dig in to push the puck more
target_angle = target_angle - self.ingress_angle
else:
# Pull out to skirt the puck
target_angle = target_angle + self.egress_angle
# 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(0, n):
if (rscan.masks[i] == ROBOT_MASK and fabs(rscan.angles[i]) < pi/4 and
rscan.ranges[i] < 2*this_robot.radius):
react_to_robot = True
react_to_robot_angle = (rscan.ANGLE_MIN + i *
(rscan.ANGLE_MAX - rscan.ANGLE_MIN) / float(rscan.NUMBER_POINTS))
if react_to_robot:
# Turn left and slow
twist.linear = self.slow_factor * self.linear_speed
twist.angular = self.angular_speed
if visualize: # Reacting to robot
self.draw_line(this_robot, rscan, react_to_robot_angle,
(255, 0, 0))
elif (target_angle == None or target_angle <= 0):
# Turn right
twist.linear = self.linear_speed
twist.angular = - self.angular_speed
if visualize:
if target_angle != None:
self.draw_line(this_robot, rscan, target_angle, (0, 255, 0))
else:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
self.draw_line(this_robot, rscan, target_angle, (255, 0, 255))
self.summed_angular_speed += twist.angular
#print self.summed_angular_speed
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
# Based on the closest landmark, determine whether we are inside or
# outside the circle
inside = False
lscan = sensor_suite.landmark_scan
closest_lmark_distance = float('inf')
closest_lmark_angle = None
for i in range(len(lscan.ranges)):
if (lscan.masks[i] & ANY_LANDMARK_MASK != 0
and lscan.ranges[i] < closest_lmark_distance):
closest_lmark_distance = lscan.ranges[i]
closest_lmark_angle = lscan.angles[i]
if (closest_lmark_distance == float('inf') or # No landmarks
closest_lmark_distance < self.circle_radius):
inside = True
# We use a state machine to provide some hysterisis. If we transition
# from inside to outside, we go into the HOMING state for a minimum
# period. However, if we go from
if self.state == "PUSHING":
if not inside:
self.state = "HOMING"
self.homing_countdown = self.homing_timeout
else: # In "HOMING"
self.homing_countdown -= 1
if self.homing_countdown == 0:
self.state = "PUSHING"
if self.state == "PUSHING":
# Push out pucks by moving towards the weighted centroid of all
# visible pucks. Nearby pucks get more weight and will be targeted.
cx = 0
cy = 0
pucks_in_view = False
n = len(rscan.ranges)
for i in range(n):
if rscan.masks[i] & self.acceptable_puck_mask != 0:
angle = self.index_to_angle(rscan, i)
if angle >= pi / 2 or angle < -pi / 2:
continue
value = 1.0 / (1.0 + rscan.ranges[i])
#value = 1.0/rscan.ranges[i]
#value = 1.0
cx = cx + value * cos(angle)
cy = cy + value * sin(angle)
pucks_in_view = True
cx /= n
cy /= n
if pucks_in_view:
twist.linear = self.linear_speed
twist.angular = 1000.0 * cy
else:
# We'll move randomly if no pucks are in view.
twist.linear = self.linear_speed
twist.angular = 5 * (random() - 0.5)
self.draw_line(this_robot, lscan, 0, (0, 255, 0))
else:
# Home towards the landmark.
if (closest_lmark_angle <= 0):
# Turn right
twist.linear = self.linear_speed
twist.angular = -self.angular_speed
if visualize:
if closest_lmark_angle != None:
self.draw_line(this_robot, rscan, closest_lmark_angle,
(0, 255, 0))
else:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
self.draw_line(this_robot, rscan, closest_lmark_angle,
(255, 0, 255))
self.last_inside = inside
return twist
def react(self, robot, sensor_suite, visualize=False):
range_scan = sensor_suite.range_scan
#puck_scan = sensor_suite.puck_scan
# We seem to have to create a new simulator object each time because
# otherwise it would contain the obstacles from the last time step.
# If there was a 'removeObstacle' method it would be a bit nicer.
sim = rvo2.PyRVOSimulator(
1 / 60., # Time step
1.5, # neighborDist
5, # maxNeighbors
1.5, # timeHorizon (other agents)
1.5, #2 # timeHorizon (obstacles)
robot.radius, # agent radius
MAX_LINEAR_SPEED) # agent max speed
agent = sim.addAgent((0, 0))
# Add range scan points as obstacles for the RVO simulator
n = len(range_scan.ranges)
points = []
for i in range(0, n):
rho = range_scan.INNER_RADIUS + range_scan.ranges[i]
#if not (rho == float('inf') or isnan(rho)):
theta = range_scan.angles[i]
points.append((rho * cos(theta), rho * sin(theta)))
# Add pucks from the puck scan
#for puck in puck_scan.pucks:
# rho = puck.distance
# theta = puck.angle
# points.append((rho*cos(theta), rho*sin(theta)))
# Add fake points behind the robot to make it think twice about going
# backwards.
#n_fake = 0
#start_angle = range_scan.ANGLE_MAX
#stop_angle = range_scan.ANGLE_MIN + 2*pi
#angle_delta = (stop_angle - start_angle) / (n_fake - 1)
#for i in range(n_fake):
# theta = start_angle + i * angle_delta
# rho = 2 * robot.radius
# points.append((rho*cos(theta), rho*sin(theta)))
# if visualize:
# vx,vy = rho*cos(theta), rho*sin(theta)
# self.draw_line_from_robot(robot, vx, vy, 0, 0, 255, 1)
# The scan points will be treated together as a single "negative"
# obstacle, with vertices specified in CW order. This requires the
# following sort.
points.sort(key=lambda p: -atan2(p[1], p[0]))
sim.addObstacle(points)
sim.processObstacles()
# Get the velocity in the robot reference frame with the clockwise
# rotation matrix
cos_theta = cos(robot.body.angle)
sin_theta = sin(robot.body.angle)
cur_vx = robot.body.velocity.x * cos_theta + \
robot.body.velocity.y * sin_theta
cur_vy = -robot.body.velocity.x * sin_theta + \
robot.body.velocity.y * cos_theta
# To prevent oscillation we will generally just test the preferred
# velocities in the immediate neighbourhood (within the pref_vels list)
# of the preferred velocity chosen last time.
if self.last_mag < 20:
# Last time the magnitude of the chosen velocity was very low.
# Do a full search over the preferred velocities.
start_index = 0
stop_index = self.NUMBER_PREF_VELS - 1
elif self.last_index == 0:
start_index = 0
stop_index = 1
elif self.last_index == len(self.pref_vels) - 1:
start_index = self.NUMBER_PREF_VELS - 2
stop_index = self.NUMBER_PREF_VELS - 1
else:
# This is the general case.
start_index = self.last_index - 1
stop_index = self.last_index + 1
highest_mag = 0
chosen_vel = None
chosen_index = None
for i in range(start_index, stop_index + 1):
pref_vel = self.pref_vels[i]
# Initializing from scratch each time
sim.setAgentPosition(agent, (0, 0))
sim.setAgentVelocity(agent, (cur_vx, cur_vy))
sim.setAgentPrefVelocity(agent, pref_vel)
for j in range(self.SIM_STEPS):
sim.doStep()
(vx, vy) = sim.getAgentVelocity(0)
#print "vel: {}, {}".format(vx, vy)
if visualize:
self.draw_line_from_robot(robot, vx, vy, 255, 255, 255, 3)
mag = sqrt(vx * vx + vy * vy)
if mag > highest_mag:
highest_mag = mag
chosen_vel = (vx, vy)
chosen_index = i
self.last_index = chosen_index
self.last_mag = highest_mag
#print "highest_mag: {}".format(highest_mag)
#chosen_vel = (avg_vx / len(self.pref_vels),
# avg_vy / len(self.pref_vels))
if visualize and chosen_vel != None:
self.draw_line_from_robot(robot, chosen_vel[0], chosen_vel[1], 255,
0, 127, 5)
#print "MAX_LINEAR_SPEED: {}".format(MAX_LINEAR_SPEED)
#print "current_vel: {}, {}".format(cur_vx, cur_vy)
#print "MAG OF current_vel: {}".format(sqrt(cur_vx**2+ cur_vy**2))
#print "chosen_vel: {}, {}".format(chosen_vel[0], chosen_vel[1])
#print "MAG OF chosen_vel: {}".format(sqrt(chosen_vel[0]**2+ chosen_vel[1]**2))
# Now treat (vx, vy) as the goal and apply the simple control law
twist = Twist()
if chosen_vel != None:
twist.linear = 0.1 * chosen_vel[0]
twist.angular = 0.02 * chosen_vel[1]
else:
print "NO AVAILABLE VELOCITY!"
#for r in range_scan.ranges:
# print r
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
scan = sensor_suite.range_scan
# By default we will attend to the centre of the scan
n = len(scan.ranges)
centre_index = n / 2
# ...but we will shift attention based on the compass
# John: Uncomment to play with compass-modulated shifting.
"""
compass_angle = normalize_angle_pm_pi(this_robot.body.angle)
sign_dir = 1
if self.current_puck_type != None and (self.current_puck_type & RED_PUCK_MASK) != 0:
sign_dir = -1
if compass_angle < 0:
centre_index = sign_dir * 2
else:
centre_index = -sign_dir * 2
"""
puck_ahead = (scan.masks[centre_index]
& self.acceptable_puck_mask) != 0
puck_mask = scan.masks[centre_index]
# Set current_puck_type if uninitialized.
if self.current_puck_type == None and puck_ahead:
self.current_puck_type = puck_mask
# With a small random probability, change current_puck_type to match
# the puck ahead.
# John: Uncomment to play with sorting
"""
if puck_ahead and random() < 0.005:
self.current_puck_type = puck_mask
"""
# Set the two predicates 'react_to_puck' and 'react_to_robot'. Only
# one should be true.
react_to_puck = puck_ahead and puck_mask == self.current_puck_type
react_to_robot = scan.masks[centre_index] == ROBOT_MASK
assert not (react_to_puck and react_to_robot)
# Now react...
if react_to_puck:
# Turn right
twist.linear = 4
twist.angular = 2.0
elif react_to_robot:
# Turn left and slow
twist.linear = 0.5
twist.angular = -2.0
else:
# Turn left
twist.linear = 4
twist.angular = -2.0
if visualize:
"""
if self.current_puck_type == RED_PUCK_MASK:
self.draw_circle(this_robot, 255, 0, 0, 1)
elif self.current_puck_type == GREEN_PUCK_MASK:
self.draw_circle(this_robot, 0, 255, 0, 1)
elif self.current_puck_type == BLUE_PUCK_MASK:
self.draw_circle(this_robot, 0, 0, 255, 1)
else:
self.draw_circle(this_robot, 0, 255, 255, 255)
"""
if react_to_puck:
self.draw_circle(this_robot, 255, 0, 0, 1)
elif react_to_robot:
self.draw_circle(this_robot, 0, 0, 255, 1)
else:
self.draw_circle(this_robot, 0, 255, 0, 1)
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
lscan = sensor_suite.landmark_scan
span_angles = self.get_landmark_span_angles(lscan)
sector_widths = self.get_sector_widths(span_angles)
assert len(span_angles) == len(sector_widths)
# If any sector width is greater than the threshold then we will turn
# away from that sector. Otherwise, we will continue forwards. If
# there is more than one such "bad sector" then we choose the one with
# the smallest width.
n = len(sector_widths)
index_of_bad_sector = None
smallest_bad_width = float('inf')
for i in range(0, n):
if (sector_widths[i] > self.threshold
and sector_widths[i] < smallest_bad_width):
index_of_bad_sector = i
smallest_bad_width = sector_widths[i]
#print index_of_bad_sector
if index_of_bad_sector != None:
lmark_a_angle = self.careful_average_angles(
span_angles[(index_of_bad_sector + 1) % n])
lmark_b_angle = self.careful_average_angles(
span_angles[index_of_bad_sector])
#print "LANDMARK ANGLES (in degrees)"
#print "A (yellow): " + str(degrees(lmark_a_angle))
#print "B (white): " + str(degrees(lmark_b_angle))
# Choose direction to react based on the average of the
# corresponding landmark angles.
react_angle = (lmark_a_angle + get_smallest_angular_difference(
lmark_b_angle, lmark_a_angle) / 2.0)
twist.linear = 10.0
twist.angular = 5 * sign(normalize_angle_pm_pi(react_angle))
self.draw_line(this_robot, lscan, lmark_a_angle, (255, 255, 0))
self.draw_line(this_robot, lscan, lmark_b_angle, (255, 255, 255))
self.draw_line(this_robot, lscan, react_angle, (255, 0, 0))
else:
# Compute centroid of all pucks in the robot's ref. frame
cx = 0
cy = 0
n = len(rscan.ranges)
for i in range(n):
if rscan.masks[i] & self.acceptable_puck_mask != 0:
angle = self.index_to_angle(rscan, i)
if angle >= pi / 2 or angle < -pi / 2:
continue
#value = 1.0/rscan.ranges[i]
#value = 1.0/rscan.ranges[i]
value = 1.0
cx = cx + value * cos(angle)
cy = cy + value * sin(angle)
cx /= n
cy /= n
twist.linear = 10.0
twist.angular = 100.0 * cy
self.draw_line(this_robot, lscan, 0, (0, 255, 0))
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
lscan = sensor_suite.landmark_scan
span_angles = self.get_landmark_span_angles(lscan)
sector_widths = self.get_sector_widths(span_angles)
assert len(span_angles) == len(sector_widths)
# If any sector width is greater than the threshold then we will turn
# away from that sector. Otherwise, we will continue forwards. If
# there is more than one such "bad sector" then we choose the one with
# the smallest width.
n = len(sector_widths)
index_of_bad_sector = None
smallest_bad_width = float('inf')
for i in range(0, n):
if (sector_widths[i] > self.threshold
and sector_widths[i] < smallest_bad_width):
index_of_bad_sector = i
smallest_bad_width = sector_widths[i]
#print index_of_bad_sector
if index_of_bad_sector != None:
lmark_a_angle = self.careful_average_angles(span_angles[(index_of_bad_sector + 1) % n])
lmark_b_angle = self.careful_average_angles(span_angles[index_of_bad_sector])
#print "LANDMARK ANGLES (in degrees)"
#print "A (yellow): " + str(degrees(lmark_a_angle))
#print "B (white): " + str(degrees(lmark_b_angle))
# Choose direction to react based on the average of the
# corresponding landmark angles.
react_angle = (lmark_a_angle +
get_smallest_angular_difference(lmark_b_angle,
lmark_a_angle) / 2.0)
twist.linear = 10.0
twist.angular = 5 * sign(normalize_angle_pm_pi(react_angle))
self.draw_line(this_robot, lscan, lmark_a_angle, (255, 255, 0))
self.draw_line(this_robot, lscan, lmark_b_angle, (255, 255, 255))
self.draw_line(this_robot, lscan, react_angle, (255, 0, 0))
else:
# Compute centroid of all pucks in the robot's ref. frame
cx = 0
cy = 0
n = len(rscan.ranges)
for i in range(n):
if rscan.masks[i] & self.acceptable_puck_mask != 0:
angle = self.index_to_angle(rscan, i)
if angle >= pi/2 or angle < -pi/2:
continue
#value = 1.0/rscan.ranges[i]
#value = 1.0/rscan.ranges[i]
value = 1.0
cx = cx + value * cos(angle)
cy = cy + value * sin(angle)
cx /= n
cy /= n
twist.linear = 10.0
twist.angular = 100.0 * cy
self.draw_line(this_robot, lscan, 0, (0, 255, 0))
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
n = len(rscan.ranges)
target = self.get_leftmost_pixel(rscan)
# Convert to an angle
if target == None:
target_angle = None
else:
target_angle = self.index_to_angle(rscan, target)
# EXPERIMENT TO ACHIEVE A MINIMUM SIZE AGGREGATE.
lscan = sensor_suite.landmark_scan
closest_lmark_distance = float('inf')
for i in range(len(lscan.ranges)):
if (lscan.masks[i] & ANY_LANDMARK_MASK != 0
and lscan.ranges[i] < closest_lmark_distance):
closest_lmark_distance = lscan.ranges[i]
if closest_lmark_distance < self.circle_radius:
target_angle = target_angle + self.egress_angle
if self.modulate and target_angle != None:
compass_angle = normalize_angle_pm_pi(this_robot.body.angle)
if cos(2 * compass_angle) < 0:
# Dig in to push the puck more
target_angle = target_angle - self.ingress_angle
else:
# Pull out to skirt the puck
target_angle = target_angle + self.egress_angle
# 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(0, n):
if (rscan.masks[i] == ROBOT_MASK and fabs(rscan.angles[i]) < pi / 4
and rscan.ranges[i] < 2 * this_robot.radius):
react_to_robot = True
react_to_robot_angle = (rscan.ANGLE_MIN + i *
(rscan.ANGLE_MAX - rscan.ANGLE_MIN) /
float(rscan.NUMBER_POINTS))
if react_to_robot:
# Turn left and slow
twist.linear = self.slow_factor * self.linear_speed
twist.angular = self.angular_speed
if visualize: # Reacting to robot
self.draw_line(this_robot, rscan, react_to_robot_angle,
(255, 0, 0))
elif (target_angle == None or target_angle <= 0):
# Turn right
twist.linear = self.linear_speed
twist.angular = -self.angular_speed
if visualize:
if target_angle != None:
self.draw_line(this_robot, rscan, target_angle,
(0, 255, 0))
else:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
self.draw_line(this_robot, rscan, target_angle, (255, 0, 255))
self.summed_angular_speed += twist.angular
#print self.summed_angular_speed
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
scan = sensor_suite.range_scan
# By default we will attend to the centre of the scan
n = len(scan.ranges)
centre_index = n/2
# ...but we will shift attention based on the compass
# John: Uncomment to play with compass-modulated shifting.
"""
compass_angle = normalize_angle_pm_pi(this_robot.body.angle)
sign_dir = 1
if self.current_puck_type != None and (self.current_puck_type & RED_PUCK_MASK) != 0:
sign_dir = -1
if compass_angle < 0:
centre_index = sign_dir * 2
else:
centre_index = -sign_dir * 2
"""
puck_ahead = (scan.masks[centre_index] & self.acceptable_puck_mask) != 0
puck_mask = scan.masks[centre_index]
# Set current_puck_type if uninitialized.
if self.current_puck_type == None and puck_ahead:
self.current_puck_type = puck_mask
# With a small random probability, change current_puck_type to match
# the puck ahead.
# John: Uncomment to play with sorting
"""
if puck_ahead and random() < 0.005:
self.current_puck_type = puck_mask
"""
# Set the two predicates 'react_to_puck' and 'react_to_robot'. Only
# one should be true.
react_to_puck = puck_ahead and puck_mask == self.current_puck_type
react_to_robot = scan.masks[centre_index] == ROBOT_MASK
assert not (react_to_puck and react_to_robot)
# Now react...
if react_to_puck:
# Turn right
twist.linear = 4
twist.angular = 2.0
elif react_to_robot:
# Turn left and slow
twist.linear = 0.5
twist.angular = -2.0
else:
# Turn left
twist.linear = 4
twist.angular = -2.0
if visualize:
"""
if self.current_puck_type == RED_PUCK_MASK:
self.draw_circle(this_robot, 255, 0, 0, 1)
elif self.current_puck_type == GREEN_PUCK_MASK:
self.draw_circle(this_robot, 0, 255, 0, 1)
elif self.current_puck_type == BLUE_PUCK_MASK:
self.draw_circle(this_robot, 0, 0, 255, 1)
else:
self.draw_circle(this_robot, 0, 255, 255, 255)
"""
if react_to_puck:
self.draw_circle(this_robot, 255, 0, 0, 1)
elif react_to_robot:
self.draw_circle(this_robot, 0, 0, 255, 1)
else:
self.draw_circle(this_robot, 0, 255, 0, 1)
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
n = len(rscan.ranges)
target = self.get_target(rscan)
# Convert to an angle
if target == None:
target_angle = None
else:
target_angle = (rscan.ANGLE_MIN + target *
(rscan.ANGLE_MAX - rscan.ANGLE_MIN) /
float(rscan.NUMBER_POINTS))
if self.modulate and target_angle != None:
compass_angle = normalize_angle_pm_pi(this_robot.body.angle)
if cos(2 * compass_angle) < 0:
# Dig in to push the puck more
target_angle = target_angle - self.ingress_angle
else:
# Pull out to skirt the puck
target_angle = target_angle + self.egress_angle
# 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(0, n):
if (rscan.masks[i] == ROBOT_MASK and fabs(rscan.angles[i]) < pi / 4
and rscan.ranges[i] < 2 * this_robot.radius):
react_to_robot = True
react_to_robot_angle = (rscan.ANGLE_MIN + i *
(rscan.ANGLE_MAX - rscan.ANGLE_MIN) /
float(rscan.NUMBER_POINTS))
if react_to_robot:
# Turn left and slow
twist.linear = self.slow_factor * self.linear_speed
twist.angular = self.angular_speed
if visualize: # Reacting to robot
self.draw_line(this_robot, rscan, react_to_robot_angle,
(255, 0, 0))
elif (target_angle == None or target_angle <= 0):
# Turn right
twist.linear = self.linear_speed
twist.angular = -self.angular_speed
if visualize:
if target_angle != None:
self.draw_line(this_robot, rscan, target_angle,
(0, 255, 0))
else:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
self.draw_line(this_robot, rscan, target_angle, (255, 0, 255))
return twist
def stop(self):
"""Stop robot motion."""
self.set_command(Twist())
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
# Based on the closest landmark, determine whether we are inside or
# outside the circle
inside = False
lscan = sensor_suite.landmark_scan
closest_lmark_distance = float('inf')
closest_lmark_angle = None
for i in range(len(lscan.ranges)):
if (lscan.masks[i] & ANY_LANDMARK_MASK != 0
and lscan.ranges[i] < closest_lmark_distance):
closest_lmark_distance = lscan.ranges[i]
closest_lmark_angle = lscan.angles[i]
if (closest_lmark_distance == float('inf') or # No landmarks
closest_lmark_distance < self.circle_radius):
inside = True
# We use a state machine to provide some hysterisis. If we transition
# from inside to outside, we go into the HOMING state for a minimum
# period. However, if we go from
if self.state == "PUSHING":
if not inside:
self.state = "HOMING"
self.homing_countdown = self.homing_timeout
else: # In "HOMING"
self.homing_countdown -= 1
if self.homing_countdown == 0:
self.state = "PUSHING"
if self.state == "PUSHING":
# Push out pucks by moving towards the weighted centroid of all
# visible pucks. Nearby pucks get more weight and will be targeted.
cx = 0
cy = 0
pucks_in_view = False
n = len(rscan.ranges)
for i in range(n):
if rscan.masks[i] & self.acceptable_puck_mask != 0:
angle = self.index_to_angle(rscan, i)
if angle >= pi/2 or angle < -pi/2:
continue
value = 1.0/(1.0 + rscan.ranges[i])
#value = 1.0/rscan.ranges[i]
#value = 1.0
cx = cx + value * cos(angle)
cy = cy + value * sin(angle)
pucks_in_view = True
cx /= n
cy /= n
if pucks_in_view:
twist.linear = self.linear_speed
twist.angular = 1000.0 * cy
else:
# We'll move randomly if no pucks are in view.
twist.linear = self.linear_speed
twist.angular = 5 * (random() - 0.5)
self.draw_line(this_robot, lscan, 0, (0, 255, 0))
else:
# Home towards the landmark.
if (closest_lmark_angle <= 0):
# Turn right
twist.linear = self.linear_speed
twist.angular = - self.angular_speed
if visualize:
if closest_lmark_angle != None:
self.draw_line(this_robot, rscan, closest_lmark_angle, (0, 255, 0))
else:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
self.draw_line(this_robot, rscan, closest_lmark_angle, (255, 0, 255))
self.last_inside = inside
return twist
def react(self, robot, sensor_suite, visualize=False):
range_scan = sensor_suite.range_scan
#puck_scan = sensor_suite.puck_scan
# We seem to have to create a new simulator object each time because
# otherwise it would contain the obstacles from the last time step.
# If there was a 'removeObstacle' method it would be a bit nicer.
sim = rvo2.PyRVOSimulator(1/60., # Time step
1.5, # neighborDist
5, # maxNeighbors
1.5, # timeHorizon (other agents)
1.5, #2 # timeHorizon (obstacles)
robot.radius, # agent radius
MAX_LINEAR_SPEED) # agent max speed
agent = sim.addAgent((0, 0))
# Add range scan points as obstacles for the RVO simulator
n = len(range_scan.ranges)
points = []
for i in range(0, n):
rho = range_scan.INNER_RADIUS + range_scan.ranges[i]
#if not (rho == float('inf') or isnan(rho)):
theta = range_scan.angles[i]
points.append((rho*cos(theta), rho*sin(theta)))
# Add pucks from the puck scan
#for puck in puck_scan.pucks:
# rho = puck.distance
# theta = puck.angle
# points.append((rho*cos(theta), rho*sin(theta)))
# Add fake points behind the robot to make it think twice about going
# backwards.
#n_fake = 0
#start_angle = range_scan.ANGLE_MAX
#stop_angle = range_scan.ANGLE_MIN + 2*pi
#angle_delta = (stop_angle - start_angle) / (n_fake - 1)
#for i in range(n_fake):
# theta = start_angle + i * angle_delta
# rho = 2 * robot.radius
# points.append((rho*cos(theta), rho*sin(theta)))
# if visualize:
# vx,vy = rho*cos(theta), rho*sin(theta)
# self.draw_line_from_robot(robot, vx, vy, 0, 0, 255, 1)
# The scan points will be treated together as a single "negative"
# obstacle, with vertices specified in CW order. This requires the
# following sort.
points.sort(key = lambda p: -atan2(p[1], p[0]))
sim.addObstacle(points)
sim.processObstacles()
# Get the velocity in the robot reference frame with the clockwise
# rotation matrix
cos_theta = cos(robot.body.angle)
sin_theta = sin(robot.body.angle)
cur_vx = robot.body.velocity.x * cos_theta + \
robot.body.velocity.y * sin_theta
cur_vy = -robot.body.velocity.x * sin_theta + \
robot.body.velocity.y * cos_theta
# To prevent oscillation we will generally just test the preferred
# velocities in the immediate neighbourhood (within the pref_vels list)
# of the preferred velocity chosen last time.
if self.last_mag < 20:
# Last time the magnitude of the chosen velocity was very low.
# Do a full search over the preferred velocities.
start_index = 0
stop_index = self.NUMBER_PREF_VELS - 1
elif self.last_index == 0:
start_index = 0
stop_index = 1
elif self.last_index == len(self.pref_vels)-1:
start_index = self.NUMBER_PREF_VELS - 2
stop_index = self.NUMBER_PREF_VELS - 1
else:
# This is the general case.
start_index = self.last_index - 1
stop_index = self.last_index + 1
highest_mag = 0
chosen_vel = None
chosen_index = None
for i in range(start_index, stop_index+1):
pref_vel = self.pref_vels[i]
# Initializing from scratch each time
sim.setAgentPosition(agent, (0, 0))
sim.setAgentVelocity(agent, (cur_vx, cur_vy))
sim.setAgentPrefVelocity(agent, pref_vel)
for j in range(self.SIM_STEPS):
sim.doStep()
(vx, vy) = sim.getAgentVelocity(0)
#print "vel: {}, {}".format(vx, vy)
if visualize:
self.draw_line_from_robot(robot, vx, vy, 255, 255, 255, 3)
mag = sqrt(vx*vx + vy*vy)
if mag > highest_mag:
highest_mag = mag
chosen_vel = (vx, vy)
chosen_index = i
self.last_index = chosen_index
self.last_mag = highest_mag
#print "highest_mag: {}".format(highest_mag)
#chosen_vel = (avg_vx / len(self.pref_vels),
# avg_vy / len(self.pref_vels))
if visualize and chosen_vel != None:
self.draw_line_from_robot(robot, chosen_vel[0], chosen_vel[1], 255, 0, 127, 5)
#print "MAX_LINEAR_SPEED: {}".format(MAX_LINEAR_SPEED)
#print "current_vel: {}, {}".format(cur_vx, cur_vy)
#print "MAG OF current_vel: {}".format(sqrt(cur_vx**2+ cur_vy**2))
#print "chosen_vel: {}, {}".format(chosen_vel[0], chosen_vel[1])
#print "MAG OF chosen_vel: {}".format(sqrt(chosen_vel[0]**2+ chosen_vel[1]**2))
# Now treat (vx, vy) as the goal and apply the simple control law
twist = Twist()
if chosen_vel != None:
twist.linear = 0.1 * chosen_vel[0]
twist.angular = 0.02 * chosen_vel[1]
else:
print "NO AVAILABLE VELOCITY!"
#for r in range_scan.ranges:
# print r
return twist