def __init__(self, world, name, role):
RADIUS = 130 / 5 # The diameter of the telliskivi robot is 260mm. In pixels it makes a radius of 26
self.WHEEL_RADIUS = 105 / 5
self.FORWARD_EDGE_FRONT = 85 / 5
self.FORWARD_EDGE_LEFT = 100 / 5 # 98.86mm
CENTER = Point(12 + RADIUS, 12 +
RADIUS) if (role == "TOPLEFT") else Point(
world.width - 12 - RADIUS, world.height - 12 -
RADIUS)
WorldObject.__init__(self, CENTER, RADIUS)
self.world = world
self.name = name
self.wr = RADIUS
self.hr = RADIUS
self.beacon_point = Point(
world.width +
50, world.cy) if role == "TOPLEFT" else Point(-50, world.cy)
self.beacon_point_en = Point(0,
world.cy) if role == "TOPLEFT" else Point(
world.width, world.cy)
self.goal_center = Point(world.width,
world.cy) if role == "TOPLEFT" else Point(
0, world.cy)
# "Forward" is the unit direction vector where the robot is facing
# (0, 1) is the "default" (height along the Y axis)
self.forward = Point(0, 1)
self.left = Point(-self.forward.y, self.forward.x)
if role == "TOPLEFT":
self.rotate(3.1415 / 2)
else:
self.rotate(-3.1415 / 2)
# Those are state parameters
self.leftSpeed = 0
self.rightSpeed = 0
# Camera sensor parameters
self.CAMERA_DEPTH = 1000 # Let's say the camera sees as far as the end of the field (~5 meters = 1000 px)
self.CAMERA_SIDE = 340 # The camera's side angle is 20 degree, i.e. at 1000 pixels it stretches to 340px
# Write concurrency lock (we assume reads are atomic, and even if not, just ignore read errors)
self.data_lock = thread.allocate_lock()
# Whether there's a ball in the grabber
self.grabbed_ball = None
self.grabbed_ball_lock = thread.allocate_lock()
def simulate(self):
# This is a hack which only works at small simulation steps
leftTurn = (self.leftSpeed - self.rightSpeed) / self.WHEEL_RADIUS / 2
forwardMove = (self.leftSpeed + self.rightSpeed) / 2
self.v = self.forward * forwardMove
if (self.v != 0):
self.center.add(self.v)
if (leftTurn != 0):
self.forward = self.forward + self.left * leftTurn
self.forward.normalize()
self.left = Point(-self.forward.y, self.forward.x)
# If there is a grabbed ball, carry it around
with self.grabbed_ball_lock:
if self.grabbed_ball is not None:
self.grabbed_ball.v = Point(0, 0)
self.grabbed_ball.center = self.center + self.forward * self.grabbed_forward + self.left * self.grabbed_left
# Precompute "edge walls", those will be useful in collision checks
self.edge_walls = [Wall(e[1], e[0]) for e in self.edges()]
all_x = [w.p1.x for w in self.edge_walls]
all_y = [w.p1.y for w in self.edge_walls]
def __init__(self, world, name, role): width = 40 height = 40 center = Point(int(12+height/2), int(12+width/2)) if role == "TOPLEFT" else Point(world.width-(12+height/2), world.height-(12+width/2)) WorldObject.__init__(self, center, int(sqrt(width**2 + height**2)/2) ) self.world = world self.name = name self.wr = width/2 self.hr = height/2 self.beacon_point = Point(world.width + 50, world.cy) if role == "TOPLEFT" else Point(-50, world.cy) self.beacon_point_en = Point(0, world.cy) if role == "TOPLEFT" else Point(world.width, world.cy) self.goal_center = Point(world.width, world.cy) if role == "TOPLEFT" else Point(0, world.cy) # "Forward" is the unit direction vector where the robot is facing # (0, 1) is the "default" (height along the Y axis) self.forward = Point(0, 1) self.left = Point(-self.forward.y, self.forward.x) if role == "TOPLEFT": self.rotate(3.1415/2) else: self.rotate(-3.1415/2) # Those are state parameters self.leftSpeed = 0 self.rightSpeed = 0 # Camera sensor parameters self.CAMERA_DEPTH = 800 # Kaugus kuhu naeme self.CAMERA_SIDE = 500 # Laius kuhu naeme # Write concurrency lock (we assume reads are atomic, and even if not, just ignore read errors) self.data_lock = thread.allocate_lock() # Whether there's a ball in the grabber self.grabbed_ball = None self.grabbed_ball_lock = thread.allocate_lock()
def grab(self):
"If at the moment this function is called there is a ball right at the front of the robot, the ball is 'grabbed'"
if self.grabbed_ball is not None:
return
for b in self.world.objects:
if isinstance(b, Ball):
# First check distance to center
v = b.center - self.center
v_forward = self.forward.inner_product(v)
if v_forward > 0 and v_forward < self.FORWARD_EDGE_FRONT + b.radius + 3:
# See whether the ball is within the front edge
v_left = self.left.inner_product(v)
if (abs(v_left) < self.FORWARD_EDGE_LEFT - 2):
# OK, grab
with self.grabbed_ball_lock:
self.grabbed_ball = b
b.v = Point(0, 0)
self.grabbed_forward = v_forward - 5
self.grabbed_left = v_left
return
def grab(self): "If at the moment this function is called there is a ball right at the front of the robot, the ball is 'grabbed'" if self.grabbed_ball is not None: #pall on juba haaratud, meil peaks olema, et votab koik pallid return for b in self.world.objects: if isinstance(b, Ball): # First check distance to center v = b.center - self.center v_forward = self.forward.inner_product(v) if v_forward > 0 and v_forward < self.hr + b.radius + 3: # See whether the ball is within the front edge v_left = self.left.inner_product(v) if (abs(v_left) < self.wr - 2): # OK, grab with self.grabbed_ball_lock: self.grabbed_ball = b b.v = Point(0, 0) self.grabbed_forward = v_forward - 5 self.grabbed_left = v_left return
def collision_check(self, obj): # If it is not a ball, ignore it if not isinstance(obj, Ball): # objekt ei ole pall # Just check the "bounding circle" dir = obj.center - self.center dist = dir.norm() if (dist < obj.radius + self.radius): # Nudge either us or them, choose randomly to avoid some ugliness dir_normalized = dir * (1/dist) if (random.randint(0,1) == 0): # Them obj.center.add(dir_normalized*(obj.radius + self.radius - dist)) else: # Us self.center.add(dir_normalized*(dist - (obj.radius + self.radius))) else: #objekt on pall # Find which wall is the ball touching for w in self.edge_walls: d = w.dist_to_point(obj.center) - obj.radius if (d > -0.3 and d < 0): # Is the ball within the range of the wall at all? wall_coord = (obj.center - w.p1).inner_product(w.v_normalized) if (wall_coord >= -obj.radius and wall_coord <= w.len+obj.radius): # Yes, it does, something must be done. # Nyyd kontrollime kas esimene ots? v = obj.center - self.center v_left = self.left.inner_product(v) v_forward = self.forward.inner_product(v) if (abs(v_left) < self.wr - 2): # OK, grab with self.grabbed_ball_lock: self.grabbed_ball = obj obj.v = Point(0, 0) self.grabbed_forward = v_forward - 5 self.grabbed_left = v_left return else: #kui pole esimene ots # First, nudge obj.center.add(w.normal * (-d)) # Second, simulate a rebounce (this is a hack, but it's way easier than considering rotations and stuff) obj.v.add(w.normal * (-d*2))
def main():
# Read two parameters identifying modules for the first and the second robots.
if (len(sys.argv) < 3):
print "Usage: python main.py <first_robot> <second_robot> [random seed]"
print ""
print "The <first_robot> and <second_robot> should identify modules containing classes Robot and RobotServer"
print "E.g if you invoke "
print " python main.py telliskivi ekrs"
print "The simulator will import telliskivi.Robot, telliskivi.RobotServer, ekrs.Robot, ekrs.RobotServer"
sys.exit(1)
# Try to import modules
r1module = __import__(sys.argv[1])
r2module = __import__(sys.argv[2])
(a,b,c,d) = (r1module.Robot, r1module.RobotServer, r2module.Robot, r2module.RobotServer) # Testing
random_seed = int(sys.argv[3]) if len(sys.argv) > 3 else None
# random seeds 1,2,3,4 are already interesting use cases
# Init graphics
pygame.init()
window = pygame.display.set_mode((1060, 760)) # This is the size of the field + contestant area. (5300 x 3800)
pygame.display.set_caption('Robotex 2011 Simulator')
screen = pygame.display.get_surface()
# Init world.
world = World(screen)
# Add 11 balls (coordinates are world-coords)
# Make sure the balls are added symmetrically. That means the first ball goes in the center
world.add_object(Ball(Point(world.width/2, world.height/2)))
for i in range(5):
while True:
xpos = random.uniform(10,world.width-10)
ypos = random.uniform(10,world.height-10)
# Make sure the positions do not get in the robot's starting corners ( 0..60px, i.e. 0..60px )
if not ((xpos < 60 and ypos < 60) or (xpos > world.width - 60 and ypos > world.height - 60)):
break
world.add_object(Ball(Point(xpos, ypos)))
world.add_object(Ball(Point(world.width-xpos, world.height-ypos)))
# Create two robots
robot1 = r1module.Robot(world, "Robot A", "TOPLEFT")
robot2 = r2module.Robot(world, "Robot B", "BOTTOMRIGHT")
world.add_object(robot1)
world.add_object(robot2)
# Start robot command servers
r1module.RobotServer(robot1, 5000).serve()
r2module.RobotServer(robot2, 5001).serve()
# Do the simulation/drawing/event cycle
last_sim = -1000
last_draw = -1000
while True:
t = get_ticks()
if (t - last_sim) > 1:
# Simulate world (~1 iteration once every millisecond or so)
# NB: This is kinda hard-coded into the logic currently,
# i.e. World.simulate() and Ball.simulate() and anyone else is
# free to assume that a simulation step is 1ms. In particular,
# the ball computes it's friction coefficient like that.
world.simulate()
last_sim = t
if (t - last_draw) > 40:
# Draw a frame once every 40 milliseconds or so (~25 fps)
BACKGROUND_BLUE = (120,119,253)
screen.fill(BACKGROUND_BLUE)
world.draw(screen)
pygame.display.flip()
last_draw = t
# Process input
input(pygame.event.get())
