def attack_kick(self):
commands.setHeadTilt(-20)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir, kick_sent, kick_start_time, recovering_from_kick
if not kick_sent:
#print "sending kick"
# memory.speech.say("Kicking")
memory.walk_request.noWalk()
memory.kick_request.setFwdKick()
kick_start_time = self.getTime()
kick_sent = True
recovering_from_kick = False
elif not recovering_from_kick and kick_sent and (
self.getTime() - kick_start_time
) > 0.5 and not memory.kick_request.kick_running_:
# print "kick is done, recovering"
# self.walk(-0.2, -0.1, 0.0)
# recovering_from_kick = True
# elif recovering_from_kick and (self.getTime() - kick_start_time) > 7.0:
# print "hopefully done recovering"
kick_sent = False
recovering_from_kick = False
self.stop()
mode = Modes.passive
current_state = AttackingStates.dribble
def run(self):
commands.setHeadPan(self.direction * 45 * core.DEG_T_RAD, self.time)
if self.tilt is not None:
commands.setHeadTilt(self.tilt)
if self.getTime() > self.time:
self.direction *= -1
def attack_start(self):
commands.setHeadTilt(-10)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir
o_ball = mem_objects.world_objects[core.WO_BALL]
o_beacon_lm_seen = mem_objects.world_objects[
core.WO_BEACON_BLUE_PINK].seen or mem_objects.world_objects[
core.WO_BEACON_PINK_BLUE].seen
o_beacon_rf_seen = mem_objects.world_objects[
core.WO_BEACON_YELLOW_PINK].seen or mem_objects.world_objects[
core.WO_BEACON_PINK_YELLOW].seen
o_beacon_rr_seen = mem_objects.world_objects[
core.WO_BEACON_YELLOW_BLUE].seen or mem_objects.world_objects[
core.WO_BEACON_BLUE_YELLOW].seen
rotation_dir = 1. if (
not o_beacon_lm_seen and
(o_beacon_rf_seen or o_beacon_rr_seen)) else -1.
if rotation_dir > 0:
memory.speech.say("Started on the left")
else:
memory.speech.say("Started on the right")
if (o_ball.seen):
current_state = AttackingStates.approach
else:
self.walk(0.0, 0.0, rotation_dir * 0.2)
def run(self):
commands.setHeadTilt(
-10) # Tilt head up so we can see goal (default = -22)
pose.Sit()
if self.getTime() > 5.0:
memory.speech.say("Sitting complete")
self.finish()
def run(self):
commands.setHeadPan(self.pan, self.time)
if self.tilt is not None:
commands.setHeadTilt(self.tilt)
if self.getTime() > self.time:
self.finish()
def run(self):
commands.setHeadTilt(
-10) # Tilt head up so we can see goal (default = -22)
commands.setWalkVelocity(0.4, 0, 0)
if self.getTime() > 15.0:
self.finish()
def run(self):
commands.setHeadPan(self.direction * 45 * core.DEG_T_RAD, self.time)
if self.tilt is not None:
commands.setHeadTilt(self.tilt)
if self.getTime() > self.time:
self.direction *= -1
def processFrame():
global currentState, lastState, currentTask
commands.setHeadTilt()
lastState = currentState
currentState = game_state.state()
if currentState == core.PLAYING and lastState != currentState and lastState != core.PENALISED:
behavior_mem.timePlayingStarted = vision_frame_info.seconds_since_start
if util.currentFrame() % 30 == 0:
util.checkTemperatures()
if areDistinct(currentState, lastState):
if currentTask:
currentTask.finish()
currentTask = createStateTask(currentState)
if util.checkFallen(
) and robot_state.WO_SELF != core.WO_TEAM_COACH and not game_state.isPenaltyKick:
commands.setStiffness(cfgstiff.Zero)
kick_request.abortKick()
walk_request.noWalk()
return
currentTask.processFrame()
def run(self):
commands.setWalkVelocity(0.0, 0.0, 0.0)
commands.setHeadPanTilt(core.DEG_T_RAD * self.pan, self.tilt,
self.time)
commands.setHeadTilt(self.tilt)
if self.getTime() > (self.time + 0.3):
self.finish()
def run(self):
robot = memory.world_objects.getObjPtr(5)
print("robot ", robot.loc)
print("orientation ", robot.orientation)
commands.setHeadTilt(0.3)
commands.setWalkPedantic(0, 0, -0.01)
def run(self):
commands.setHeadPan(0,0.1)
commands.setHeadTilt(tilt_angle)
robot= memory.world_objects.getObjPtr(5)
# memory.speech.say("I'm still turning")
print("Looking for another beacon")
sgn = math.copysign(1,robot.loc.x)*math.copysign(1,robot.orientation)
commands.setWalkVelocity(0,0,sgn*0.1)
def run(self):
if self.getTime() < 1.0:
memory.speech.say("I am ready")
commands.standStraight()
commands.setHeadTilt(-18) # Tilt head up so we can see goal (default = -22)
if self.getTime() > 2.0:
commands.stand()
def run(self):
if not memory.body_model.feet_on_ground_:
memory.speech.say("Flying")
self.reset()
else:
memory.speech.say("Spining")
commands.setHeadPan(0, 1)
# nao = memory.world_objects.getObjPtr(memory.robot_state.WO_SELF)
# t = nao.orientation
# x = nao.loc.x
# y = nao.loc.y
# print("Robot x: {}\t y:{}\t t: {}".format(x, y, t))
# origin = memory.world_objects.getObjPtr(core.WO_TEAM_COACH)
# print("bearing: {}".format(origin.orientation))
beacon_list = [
core.WO_BEACON_BLUE_YELLOW,
core.WO_BEACON_YELLOW_BLUE,
core.WO_BEACON_BLUE_PINK,
core.WO_BEACON_PINK_BLUE,
core.WO_BEACON_PINK_YELLOW,
core.WO_BEACON_YELLOW_PINK,
]
commands.setHeadTilt(-18) # Tilt head up so we can see goal (default = -22)
# commands.setWalkVelocity(0, 0, -0.15)
# if self.getTime() > 5.0:
# any_beacon_seen = False
# for key in beacon_list:
# beacon = memory.world_objects.getObjPtr(key)
# if beacon.seen:
# any_beacon_seen = True
vel_x = 0
vel_y = 0
vel_turn = 0.30
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
if self.getTime() > 10.0:
beacon_x_right_threshold = 360.0 / 2 + 40
beacon_x_left_threshold = 360.0 / 2 - 40
any_beacon_aligned = False
for key in beacon_list:
beacon = memory.world_objects.getObjPtr(key)
if beacon.seen:
if (beacon.imageCenterX < beacon_x_right_threshold) and (
beacon.imageCenterX > beacon_x_left_threshold
):
any_beacon_aligned = True
if any_beacon_aligned:
self.finish()
def defense_walk(self):
global walk_start_time, current_state, DefendingStates
commands.setHeadTilt(-13)
commands.setWalkVelocity(0.3, 0.0, 0.05)
#print "walk time: " + str(walk_start_time)
#print "current time: " + str(self.getTime())
if (self.getTime() - walk_start_time) > 9.0:
current_state = DefendingStates.start
# commands.setWalkVelocity(0.0,0.0,0.0)
commands.stand()
def defense_walk(self):
global walk_start_time, current_state, DefendingStates
commands.setHeadTilt(-13)
commands.setWalkVelocity(0.3,0.0,0.05)
#print "walk time: " + str(walk_start_time)
#print "current time: " + str(self.getTime())
if (self.getTime() - walk_start_time) > 9.0:
current_state = DefendingStates.start
# commands.setWalkVelocity(0.0,0.0,0.0)
commands.stand()
def attack_dribble(self):
commands.setHeadTilt(-16)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir, r_goal_dist_filtered
o_self = mem_objects.world_objects[robot_state.WO_SELF]
o_ball = mem_objects.world_objects[core.WO_BALL]
o_goal = mem_objects.world_objects[core.WO_OPP_GOAL]
o_enemy = mem_objects.world_objects[core.WO_OPPONENT1]
if (not o_goal.seen):
#print "Lost the goal!"
self.stop()
current_state = AttackingStates.rotate
return
if (not o_ball.seen):
#print "Lost the ball!"
self.walk(-0.3, 0.0, 0.0)
return
gx = o_goal.visionDistance * numpy.cos(o_goal.visionBearing)
gy = o_goal.visionDistance * numpy.sin(o_goal.visionBearing)
bx = o_ball.visionDistance * numpy.cos(o_ball.visionBearing)
by = o_ball.visionDistance * numpy.sin(o_ball.visionBearing)
dx_gb = gx - bx
dy_gb = gy - by
dt_gb = numpy.arctan2(dy_gb, dx_gb)
r_goal_ball = numpy.sqrt(dx_gb * dx_gb + dy_gb * dy_gb)
alpha = 0.3
r_goal_dist_filtered = alpha * r_goal_ball + (
1.0 - alpha) * r_goal_dist_filtered
r_goal_threshold = 750. * numpy.sqrt(2.)
# r_goal_threshold = 725 * numpy.sqrt(2.)
#print "gx: " + str(gx)
#print "gy: " + str(gy)
#print "bx: " + str(bx)
#print "by: " + str(by)
#print "dx_gb: " + str(dx_gb)
#print "dy_gb: " + str(dy_gb)
#print "dt_gb: " + str(dt_gb)
if (r_goal_dist_filtered < r_goal_threshold):
#print "Stopping with threshold " + str(r_goal_dist_filtered)
# memory.speech.say("Distance " + str(int(r_goal_dist_filtered)) + ", aligning")
current_state = AttackingStates.align
return
x_vel = 0.3 if numpy.abs(by) < 200 else 0.
y_vel = tanhController((gy + by) / 2.0, 20.0 / 1000.0, 0.35)
t_vel = tanhController(dy_gb, 10.0 / 1000.0, 0.2)
#print "vel x,y,t = " + str(x_vel) + "," + str(y_vel) + "," + str(t_vel)
self.walk(x_vel, y_vel, t_vel)
def attack_dribble(self):
commands.setHeadTilt(-16)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir, r_goal_dist_filtered
o_self = mem_objects.world_objects[robot_state.WO_SELF]
o_ball = mem_objects.world_objects[core.WO_BALL]
o_goal = mem_objects.world_objects[core.WO_OPP_GOAL]
o_enemy = mem_objects.world_objects[core.WO_OPPONENT1]
if(not o_goal.seen):
#print "Lost the goal!"
self.stop()
current_state = AttackingStates.rotate
return
if(not o_ball.seen):
#print "Lost the ball!"
self.walk(-0.3, 0.0, 0.0)
return
gx = o_goal.visionDistance * numpy.cos(o_goal.visionBearing)
gy = o_goal.visionDistance * numpy.sin(o_goal.visionBearing)
bx = o_ball.visionDistance * numpy.cos(o_ball.visionBearing)
by = o_ball.visionDistance * numpy.sin(o_ball.visionBearing)
dx_gb = gx - bx
dy_gb = gy - by
dt_gb = numpy.arctan2(dy_gb, dx_gb)
r_goal_ball = numpy.sqrt(dx_gb * dx_gb + dy_gb * dy_gb)
alpha = 0.3
r_goal_dist_filtered = alpha*r_goal_ball + (1.0-alpha)*r_goal_dist_filtered
r_goal_threshold = 750. * numpy.sqrt(2.)
# r_goal_threshold = 725 * numpy.sqrt(2.)
#print "gx: " + str(gx)
#print "gy: " + str(gy)
#print "bx: " + str(bx)
#print "by: " + str(by)
#print "dx_gb: " + str(dx_gb)
#print "dy_gb: " + str(dy_gb)
#print "dt_gb: " + str(dt_gb)
if(r_goal_dist_filtered < r_goal_threshold):
#print "Stopping with threshold " + str(r_goal_dist_filtered)
# memory.speech.say("Distance " + str(int(r_goal_dist_filtered)) + ", aligning")
current_state = AttackingStates.align
return
x_vel = 0.3 if numpy.abs(by) < 200 else 0.
y_vel = tanhController((gy + by)/2.0, 20.0/1000.0, 0.35)
t_vel = tanhController(dy_gb, 10.0/1000.0, 0.2)
#print "vel x,y,t = " + str(x_vel) + "," + str(y_vel) + "," + str(t_vel)
self.walk(x_vel, y_vel, t_vel)
def run(self):
if (not memory.body_model.feet_on_ground_):
memory.speech.say("Flying")
self.reset()
else:
memory.speech.say("Spining")
commands.setHeadPan(0, 1)
# nao = memory.world_objects.getObjPtr(memory.robot_state.WO_SELF)
# t = nao.orientation
# x = nao.loc.x
# y = nao.loc.y
# print("Robot x: {}\t y:{}\t t: {}".format(x, y, t))
# origin = memory.world_objects.getObjPtr(core.WO_TEAM_COACH)
# print("bearing: {}".format(origin.orientation))
beacon_list = [
core.WO_BEACON_BLUE_YELLOW, core.WO_BEACON_YELLOW_BLUE,
core.WO_BEACON_BLUE_PINK, core.WO_BEACON_PINK_BLUE,
core.WO_BEACON_PINK_YELLOW, core.WO_BEACON_YELLOW_PINK
]
commands.setHeadTilt(
-18) # Tilt head up so we can see goal (default = -22)
# commands.setWalkVelocity(0, 0, -0.15)
# if self.getTime() > 5.0:
# any_beacon_seen = False
# for key in beacon_list:
# beacon = memory.world_objects.getObjPtr(key)
# if beacon.seen:
# any_beacon_seen = True
vel_x = 0
vel_y = 0
vel_turn = 0.30
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
if self.getTime() > 10.0:
beacon_x_right_threshold = 360.0 / 2 + 40
beacon_x_left_threshold = 360.0 / 2 - 40
any_beacon_aligned = False
for key in beacon_list:
beacon = memory.world_objects.getObjPtr(key)
if beacon.seen:
if (beacon.imageCenterX < beacon_x_right_threshold) and (
beacon.imageCenterX > beacon_x_left_threshold):
any_beacon_aligned = True
if any_beacon_aligned:
self.finish()
def run(self):
# print("localization player ",self.local)
# print("localalization state ",self.localstate)
commands.setHeadTilt(-0.0)
# if core.joint_values[core.HeadYaw] <=1.0:
# commands.setHeadPan(core.joint_values[core.HeadYaw]+0.06,0.03)
print("robot ", self.robot.loc)
print("orientation ", self.robot.orientation)
print("covariance ", self.robot.sd.getMagnitude())
# print(localization_mem)
if self.getTime() > 120.0:
self.finish()
def run(self):
global last_head_time, last_head_pan , last_keeper_direction
commands.setHeadTilt(-14)
if ((self.getTime() - last_head_time) > 4):
if(last_head_pan >= 0):
last_head_pan = -1
else:
last_head_pan = 1
commands.setHeadPan( last_head_pan, 3.5)
commands.setWalkVelocity(0.2,0.0*last_head_pan,0.05*last_head_pan)
last_head_time = self.getTime()
def run(self):
# These are max values for Gene to adjust itself
vel_y_gain = -0.50
vel_x_gain = 0.50
vel_turn_gain = 1.00
vel_y = -0.50 # This is the tangential velocity, fix it (try -0.50 cw or left)
vel_x = 0
vel_turn = 0.30
# Change turn direction based on the last seen beacon (ccw or right)
global turn_dir
if turn_dir:
vel_y = 0.50
goal_aligned = False
# Target position of the ball in bottom camera
# # x_desired = 360.0 / 2
# x_desired = 120 # Ball near left foot
# # y_desired = 240.0 / 2
# y_desired = 200 # Ball near the bottom of the frame when looking up
x_desired = 360.0 / 2
y_desired = 220.0
ball = memory.world_objects.getObjPtr(core.WO_BALL)
goal = memory.world_objects.getObjPtr(core.WO_OPP_GOAL)
# Make sure that the ball is always at the same position relative to Gene
if ball.seen:
if ball.fromTopCamera:
memory.speech.say("Align goal The ball is in top frame")
self.postSignal("restart")
else:
# We Calculate the deviation from the center of the frame
# to adjust our rotation speed
# Y determines vel_x to make sure that Gene
# is always a fix distance from the ball
vel_x = vel_x_gain * (y_desired - ball.imageCenterY) / (y_desired)
# X determines the vel_turn. Since we want to always turn
# that's why there's an offset
# vel_turn = 0.25 - (x_desired - ball.imageCenterX) / (x_desired) * 0.25
vel_turn = vel_turn_gain * (x_desired - ball.imageCenterX) / (x_desired)
if goal.seen:
vel_y, goal_aligned = align_goal(vel_y) # Exit condition
if goal_aligned:
self.finish()
# commands.setWalkvelocity(0, -0.50, 0.25)
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
commands.setHeadTilt(-10) # Tilt head up so we can see goal (default = -22)
def run(self):
commands.setStiffness()
commands.stand()
# TODO: Check this
#commands.setHeadTilt(-15)
commands.setHeadTilt(0)
#try:
# os.remove("beacon_data_sim.txt")
#except OSError:
# pass
if self.getTime() > 3.0:
self.finish()
def attack_approach(self):
commands.setHeadTilt(-14)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir
o_self = mem_objects.world_objects[robot_state.WO_SELF]
o_ball = mem_objects.world_objects[core.WO_BALL]
o_goal = mem_objects.world_objects[core.WO_OPP_GOAL]
o_enemy = mem_objects.world_objects[core.WO_OPPONENT1]
if (o_ball.seen):
if (self.walk_to(o_ball, 250)):
current_state = AttackingStates.rotate
return
def attack_approach(self):
commands.setHeadTilt(-14)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir
o_self = mem_objects.world_objects[robot_state.WO_SELF]
o_ball = mem_objects.world_objects[core.WO_BALL]
o_goal = mem_objects.world_objects[core.WO_OPP_GOAL]
o_enemy = mem_objects.world_objects[core.WO_OPPONENT1]
if(o_ball.seen):
if(self.walk_to(o_ball, 250)):
current_state = AttackingStates.rotate
return
def run(self):
commands.setHeadTilt(-10) # Tilt head up so we can see goal (default = -22)
memory.speech.say("Turning!")
commands.setWalkVelocity(0, 0, -0.15)
if self.getTime() > 5.0:
any_beacon_seen = False
for key in BEACON_LIST:
beacon = memory.world_objects.getObjPtr(key)
if beacon.seen:
any_beacon_seen = True
if any_beacon_seen:
self.finish()
def run(self):
commands.setHeadTilt(
-10) # Tilt head up so we can see goal (default = -22)
memory.speech.say('Turning!')
commands.setWalkVelocity(0, 0, -0.15)
if self.getTime() > 5.0:
any_beacon_seen = False
for key in BEACON_LIST:
beacon = memory.world_objects.getObjPtr(key)
if beacon.seen:
any_beacon_seen = True
if any_beacon_seen:
self.finish()
def toCenter(self, dir):
vel_x = 0.4
vel_y = 0
vel_turn = 0.05 # This is needed because forward odo is not reliable
turn_speed = 0.3
origin = memory.world_objects.getObjPtr(core.WO_TEAM_COACH)
bearing = origin.orientation
distance = origin.distance
# Calculate forward velocity
dist = distance / 1000.0
DIST_THRESHOLD = 1.5
dist = min(dist, DIST_THRESHOLD)
DIST_CONSTANT = 2 / 3.
vel_x = dist * DIST_CONSTANT
MAX_VEL_XOCITY = 0.50
vel_x *= MAX_VEL_XOCITY
MIN_VEL_XOCITY = 0.30
vel_x = max(MIN_VEL_XOCITY, vel_x)
# Walk towards the center!
MAX_BEARING_VELOCITY = 0.3
vel_turn = max(-MAX_BEARING_VELOCITY, min(MAX_BEARING_VELOCITY, bearing))
# if (bearing > 0):
# vel_turn += turn_speed
# else:
# vel_turn -= turn_speed
commands.setHeadTilt(
-18) # Tilt head up so we can see goal (default = -22)
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
# commands.setHeadPan(dir * 1.5, 1)
print("distance: {}\t\t bearing: {}".format(distance, bearing))
# Kidnapped relocalize
if (not memory.body_model.feet_on_ground_):
ToCenterBoth.num_times_no_beacon = 0
self.postSignal("spin")
print("flying")
# Closer
if (distance < 50):
ToCenterBoth.num_times_no_beacon = 0
self.postSignal("check")
def toCenter(self, dir):
vel_x = 0.4
vel_y = 0
vel_turn = 0.05 # This is needed because forward odo is not reliable
turn_speed = 0.3
origin = memory.world_objects.getObjPtr(core.WO_TEAM_COACH)
bearing = origin.orientation
distance = origin.distance
# Calculate forward velocity
dist = distance / 1000.0
DIST_THRESHOLD = 1.5
dist = min(dist, DIST_THRESHOLD)
DIST_CONSTANT = 2 / 3.0
vel_x = dist * DIST_CONSTANT
MAX_VEL_XOCITY = 0.50
vel_x *= MAX_VEL_XOCITY
MIN_VEL_XOCITY = 0.30
vel_x = max(MIN_VEL_XOCITY, vel_x)
# Walk towards the center!
MAX_BEARING_VELOCITY = 0.3
vel_turn = max(-MAX_BEARING_VELOCITY, min(MAX_BEARING_VELOCITY, bearing))
# if (bearing > 0):
# vel_turn += turn_speed
# else:
# vel_turn -= turn_speed
commands.setHeadTilt(-18) # Tilt head up so we can see goal (default = -22)
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
# commands.setHeadPan(dir * 1.5, 1)
print ("distance: {}\t\t bearing: {}".format(distance, bearing))
# Kidnapped relocalize
if not memory.body_model.feet_on_ground_:
ToCenterBoth.num_times_no_beacon = 0
self.postSignal("spin")
print ("flying")
# Closer
if distance < 50:
ToCenterBoth.num_times_no_beacon = 0
self.postSignal("check")
def run(self):
nao = memory.world_objects.getObjPtr(memory.robot_state.WO_SELF)
# Calculate the angle of the vector to origin in robot's frame
t = nao.orientation
x = nao.loc.x
y = nao.loc.y
speed = 0.4
dx = 0 - x
dy = 0 - y
angle = math.atan(dy / dx)
# Quadrant 1
if (x > 0 and y > 0):
angle += math.pi
# Quadrant 2
if (x < 0 and y > 0):
angle += 0
# Quadrant 3
if (x < 0 and y < 0):
angle += 0
# Quadrant 4
if (x > 0 and y < 0):
angle += math.pi
# Take the robot's orientation into account
angle -= t
vel_turn = 0.3 # Constant spin velocity
# vel_turn = 0.0 # Constant spin velocity
vel_y = -(speed * math.sin(angle))
vel_x = +(speed * math.cos(angle))
print("Robot x: {}\t y:{}\t t: {}".format(x, y, t))
# print("vel_x: {}\t vel_y: {}\t angle: {}".format(vel_x, vel_y, angle))
vel_x = 0
vel_y = 0
commands.setHeadTilt(
0) # Tilt head up so we can see goal (default = -22)
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
if self.getTime() > 30.0:
self.finish()
def run(self):
nao = memory.world_objects.getObjPtr(memory.robot_state.WO_SELF)
# Calculate the angle of the vector to origin in robot's frame
t = nao.orientation
x = nao.loc.x
y = nao.loc.y
speed = 0.4
dx = 0 - x
dy = 0 - y
angle = math.atan(dy / dx)
# Quadrant 1
if x > 0 and y > 0:
angle += math.pi
# Quadrant 2
if x < 0 and y > 0:
angle += 0
# Quadrant 3
if x < 0 and y < 0:
angle += 0
# Quadrant 4
if x > 0 and y < 0:
angle += math.pi
# Take the robot's orientation into account
angle -= t
vel_turn = 0.3 # Constant spin velocity
# vel_turn = 0.0 # Constant spin velocity
vel_y = -(speed * math.sin(angle))
vel_x = +(speed * math.cos(angle))
print ("Robot x: {}\t y:{}\t t: {}".format(x, y, t))
# print("vel_x: {}\t vel_y: {}\t angle: {}".format(vel_x, vel_y, angle))
vel_x = 0
vel_y = 0
commands.setHeadTilt(0) # Tilt head up so we can see goal (default = -22)
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
if self.getTime() > 30.0:
self.finish()
def run(self):
commands.setHeadTilt(tilt_angle)
out_Pan = min([max([1-2*core.joint_values[core.HeadYaw],-0.7]),0.7])
print("Head angle ",out_Pan)
commands.setHeadPan(out_Pan,2)
commands.stand()
robot = memory.world_objects.getObjPtr(5)
ball = memory.world_objects.getObjPtr(core.WO_BALL)
goal = memory.world_objects.getObjPtr(core.WO_OWN_GOAL)
beacons = {"WO_BEACON_BLUE_YELLOW" : memory.world_objects.getObjPtr(core.WO_BEACON_BLUE_YELLOW),
"WO_BEACON_YELLOW_BLUE" : memory.world_objects.getObjPtr(core.WO_BEACON_YELLOW_BLUE),
"WO_BEACON_BLUE_PINK" : memory.world_objects.getObjPtr(core.WO_BEACON_BLUE_PINK),
"WO_BEACON_PINK_BLUE" : memory.world_objects.getObjPtr(core.WO_BEACON_PINK_BLUE),
"WO_BEACON_PINK_YELLOW" : memory.world_objects.getObjPtr(core.WO_BEACON_PINK_YELLOW),
"WO_BEACON_YELLOW_PINK" : memory.world_objects.getObjPtr(core.WO_BEACON_YELLOW_PINK)}
x_start = -1200
x_end = 1200
y_start = -1200
y_end = 1200
discretize_loc = 200
x_bins = [e for e in range(x_start,x_end+1,discretize_loc)]
y_bins = [e for e in range(y_start,y_end+1,discretize_loc)]
# xy_bins = [-800,-705,-611,-517,-423,-329,-235,-141,-47,47,141,235,329,423,517,611,705,800]
t_start = 90
t_end = 270
discretize_trans = 30
t_bins = [e for e in range(t_start,t_end+1,discretize_trans)]
# t_bins = [0,18,36,54,72,108,126,144,162,180,198,216,234,252,270,288,306,324,342,360]
rad_2_deg = 57.29
r_orientation = (robot.orientation*rad_2_deg) % 360 #(robot.orientation+3.14) % (2*3.14)*rad_2_deg
takeClosest = lambda num,collection:min(collection,key=lambda x:abs(x-num))
# state = (int(robot.loc.x-robot.loc.x%discretize_loc),int(robot.loc.y-robot.loc.y%discretize_loc),int(r_orientation-(r_orientation%discretize_trans)))
state = (takeClosest(robot.loc.x,x_bins),takeClosest(robot.loc.y,y_bins),takeClosest(r_orientation,t_bins))
# ball_state = ball.
print("Position ",robot.loc.x," ",robot.loc.y," - Trans ",r_orientation)
print("State ",state[0]," ",state[1]," - Trans ",state[2])
pol_str = self.policy.get(state,"none")
# print("Flag: ",self.test_flag)
# if self.test_flag is True:
# self.postSignal("none")
# else:
# self.test_flag = True
# self.postSignal("turnleft")
print(pol_str)
self.postSignal(pol_str)
def run(self):
commands.setStiffness()
commands.setHeadTilt(-15)
beacon1 = mem_objects.world_objects[core.WO_BEACON_BLUE_YELLOW]
if beacon1.seen:
if len(self.data) < 100:
self.data.append(
[beacon1.beacon_height, beacon1.visionBearing])
mat = np.array(self.data)
print(mat.shape)
if len(self.data) > 2:
print(np.mean(self.data, axis=0))
print(np.sqrt(np.var(self.data, axis=0)))
print()
state = game_state
if state.isPenaltyKick:
self.data = []
state.isPenaltyKick = False
def run(self):
# Stand straight up because the head can't move if crouching
commands.standStraight()
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
memory.speech.say("I see the ball!")
print('Ball seen!!!!')
ball_x, ball_y = ball.imageCenterX, ball.imageCenterY
print('Ball seen at ({},{})'.format(ball_x, ball_y))
x_head_turn = -(ball_x-(320.0 / 2.0)) / 160.0
print('X HEAD TURN: {}'.format(x_head_turn))
commands.setHeadPan(x_head_turn)
y_head_tilt = -(ball_y-(240.0 / 2.0)) / 120.0 * 30
print('Y HEAD TILT: {}'.format(y_head_tilt))
commands.setHeadTilt(y_head_tilt)
else:
memory.speech.say("Where's the ball!?")
def run(self):
# Stand straight up because the head can't move if crouching
commands.standStraight()
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
memory.speech.say("I see the ball!")
print('Ball seen!!!!')
ball_x, ball_y = ball.imageCenterX, ball.imageCenterY
print('Ball seen at ({},{})'.format(ball_x, ball_y))
x_head_turn = -(ball_x - (320.0 / 2.0)) / 160.0
print('X HEAD TURN: {}'.format(x_head_turn))
commands.setHeadPan(x_head_turn)
y_head_tilt = -(ball_y - (240.0 / 2.0)) / 120.0 * 30
print('Y HEAD TILT: {}'.format(y_head_tilt))
commands.setHeadTilt(y_head_tilt)
else:
memory.speech.say("Where's the ball!?")
def attack_start(self):
commands.setHeadTilt(-10)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir
o_ball = mem_objects.world_objects[core.WO_BALL]
o_beacon_lm_seen = mem_objects.world_objects[core.WO_BEACON_BLUE_PINK].seen or mem_objects.world_objects[core.WO_BEACON_PINK_BLUE].seen
o_beacon_rf_seen = mem_objects.world_objects[core.WO_BEACON_YELLOW_PINK].seen or mem_objects.world_objects[core.WO_BEACON_PINK_YELLOW].seen
o_beacon_rr_seen = mem_objects.world_objects[core.WO_BEACON_YELLOW_BLUE].seen or mem_objects.world_objects[core.WO_BEACON_BLUE_YELLOW].seen
rotation_dir = 1. if (not o_beacon_lm_seen and (o_beacon_rf_seen or o_beacon_rr_seen)) else -1.
if rotation_dir > 0:
memory.speech.say("Started on the left")
else:
memory.speech.say("Started on the right")
if(o_ball.seen):
current_state = AttackingStates.approach
else:
self.walk(0.0, 0.0, rotation_dir*0.2)
def run(self):
commands.setHeadTilt(0.3)
ball = memory.world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
print("Ball at ", ball.visionDistance, " bearing ",
ball.visionBearing, " Camera: ", ball.fromTopCamera)
goal = memory.world_objects.getObjPtr(core.WO_UNKNOWN_GOAL)
WO_BEACON_BLUE_YELLOW = memory.world_objects.getObjPtr(
core.WO_BEACON_BLUE_YELLOW)
WO_BEACON_YELLOW_BLUE = memory.world_objects.getObjPtr(
core.WO_BEACON_YELLOW_BLUE)
WO_BEACON_BLUE_PINK = memory.world_objects.getObjPtr(
core.WO_BEACON_BLUE_PINK)
WO_BEACON_PINK_BLUE = memory.world_objects.getObjPtr(
core.WO_BEACON_PINK_BLUE)
WO_BEACON_PINK_YELLOW = memory.world_objects.getObjPtr(
core.WO_BEACON_PINK_YELLOW)
WO_BEACON_YELLOW_PINK = memory.world_objects.getObjPtr(
core.WO_BEACON_YELLOW_PINK)
# print(WO_BEACON_PINK_BLUE.seen)
# if WO_BEACON_BLUE_YELLOW.seen:
# print("WO_BEACON_BLUE_YELLOW seen ","Bearing: ",WO_BEACON_BLUE_YELLOW.visionBearing," Range: ",WO_BEACON_BLUE_YELLOW.visionDistance)
# self.file.write('{},{}\n'.format(WO_BEACON_BLUE_YELLOW.visionBearing,WO_BEACON_BLUE_YELLOW.visionDistance))
# if WO_BEACON_YELLOW_BLUE.seen:
# print("WO_BEACON_YELLOW_BLUE seen ","Bearing: ",WO_BEACON_YELLOW_BLUE.visionBearing," Range: ",WO_BEACON_YELLOW_BLUE.visionDistance)
# self.file.write('{},{}\n'.format(WO_BEACON_YELLOW_BLUE.visionBearing,WO_BEACON_YELLOW_BLUE.visionDistance))
# if WO_BEACON_BLUE_PINK.seen:
# print("WO_BEACON_BLUE_PINK seen ","Bearing: ",WO_BEACON_BLUE_PINK.visionBearing," Range: ",WO_BEACON_BLUE_PINK.visionDistance)
# self.file.write('{},{}\n'.format(WO_BEACON_BLUE_PINK.visionBearing,WO_BEACON_BLUE_PINK.visionDistance))
# if WO_BEACON_PINK_BLUE.seen:
# print("WO_BEACON_PINK_BLUE seen ","Bearing: ",WO_BEACON_PINK_BLUE.visionBearing," Range: ",WO_BEACON_PINK_BLUE.visionDistance)
# self.file.write('{},{}\n'.format(WO_BEACON_PINK_BLUE.visionBearing,WO_BEACON_PINK_BLUE.visionDistance))
# if WO_BEACON_PINK_YELLOW.seen:
# print("WO_BEACON_PINK_YELLOW seen ","Bearing: ",WO_BEACON_PINK_YELLOW.visionBearing," Range: ",WO_BEACON_PINK_YELLOW.visionDistance)
# self.file.write('{},{}\n'.format(WO_BEACON_PINK_YELLOW.visionBearing,WO_BEACON_PINK_YELLOW.visionDistance))
# if WO_BEACON_YELLOW_PINK.seen:
# print("WO_BEACON_YELLOW_PINK seen ","Bearing: ",WO_BEACON_YELLOW_PINK.visionBearing," Range: ",WO_BEACON_YELLOW_PINK.visionDistance)
# self.file.write('{},{}\n'.format(WO_BEACON_YELLOW_PINK.visionBearing,WO_BEACON_YELLOW_PINK.visionDistance))
if self.getTime() > 120.0:
self.file.close()
self.finish()
def run(self):
moveTime = self.stepTime - self.pauseTime
if self.skipFirstPause or (self.isPaused and (self.timer.elapsed() > self.pauseTime)):
self.isPaused = False
self.skipFirstPause = False
currPan = percepts.joint_angles[core.HeadPan]
diff = self.dest - currPan
dir = (diff / abs(diff))
target = currPan + self.stepSize * dir
if dir > 0 and target > self.dest:
target = self.dest
elif dir < 0 and target < self.dest:
target = self.dest
commands.setHeadTilt()
commands.setHeadPan(target,moveTime)
if abs(diff) < 5 * core.DEG_T_RAD:
self.finish()
self.timer.reset()
if not self.isPaused and self.timer.elapsed() > moveTime:
self.timer.reset()
self.isPaused = True
def processFrame():
global currentState, lastState, currentTask
commands.setHeadTilt()
lastState = currentState
currentState = game_state.state()
if currentState == core.PLAYING and lastState != currentState and lastState != core.PENALISED:
behavior_mem.timePlayingStarted = vision_frame_info.seconds_since_start
if util.currentFrame() % 30 == 0: util.checkTemperatures()
if areDistinct(currentState, lastState):
if currentTask: currentTask.finish()
currentTask = createStateTask(currentState)
if util.checkFallen() and robot_state.WO_SELF != core.WO_TEAM_COACH and not game_state.isPenaltyKick:
commands.setStiffness(cfgstiff.Zero)
kick_request.abortKick()
walk_request.noWalk()
return
currentTask.processFrame()
def attack_kick(self):
commands.setHeadTilt(-20)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir, kick_sent, kick_start_time, recovering_from_kick
if not kick_sent:
#print "sending kick"
# memory.speech.say("Kicking")
memory.walk_request.noWalk()
memory.kick_request.setFwdKick()
kick_start_time = self.getTime()
kick_sent = True
recovering_from_kick = False
elif not recovering_from_kick and kick_sent and (self.getTime() - kick_start_time) > 0.5 and not memory.kick_request.kick_running_:
# print "kick is done, recovering"
# self.walk(-0.2, -0.1, 0.0)
# recovering_from_kick = True
# elif recovering_from_kick and (self.getTime() - kick_start_time) > 7.0:
# print "hopefully done recovering"
kick_sent = False
recovering_from_kick = False
self.stop()
mode = Modes.passive
current_state = AttackingStates.dribble
def run(self):
moveTime = self.stepTime - self.pauseTime
if self.skipFirstPause or (self.isPaused and
(self.timer.elapsed() > self.pauseTime)):
self.isPaused = False
self.skipFirstPause = False
currPan = percepts.joint_angles[core.HeadPan]
diff = self.dest - currPan
dir = (diff / abs(diff))
target = currPan + self.stepSize * dir
if dir > 0 and target > self.dest:
target = self.dest
elif dir < 0 and target < self.dest:
target = self.dest
commands.setHeadTilt()
commands.setHeadPan(target, moveTime)
if abs(diff) < 5 * core.DEG_T_RAD:
self.finish()
self.timer.reset()
if not self.isPaused and self.timer.elapsed() > moveTime:
self.timer.reset()
self.isPaused = True
def run(self):
commands.setHeadTilt()
commands.setStiffness()
if behavior_mem.test_odom_new:
self.otimer.reset()
behavior_mem.test_odom_new = False
if self.otimer.elapsed() > behavior_mem.test_odom_walk_time:
if self.stance != core.Poses.SITTING:
self.stance = core.Poses.SITTING
return pose.Sit()
return
velX = behavior_mem.test_odom_fwd
velY = behavior_mem.test_odom_side
velTheta = behavior_mem.test_odom_turn
reqstance = behavior_mem.test_stance
if reqstance != self.stance:
self.stance = reqstance
if reqstance == core.Poses.SITTING:
return pose.Sit()
elif not util.isStanding():
return pose.Stand()
if self.stance == core.Poses.STANDING:
commands.setWalkVelocity(velX, velY, velTheta)
def attack_rotate(self):
commands.setHeadTilt(-16)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir
o_self = mem_objects.world_objects[robot_state.WO_SELF]
o_ball = mem_objects.world_objects[core.WO_BALL]
o_goal = mem_objects.world_objects[core.WO_OPP_GOAL]
o_enemy = mem_objects.world_objects[core.WO_OPPONENT1]
if (o_ball.seen and o_goal.seen):
gy = o_goal.visionDistance * numpy.sin(o_goal.visionBearing)
if (numpy.abs(gy) <= 200):
self.stop()
current_state = AttackingStates.dribble
return
bx = o_ball.visionDistance * numpy.cos(o_ball.visionBearing)
x_vel = tanhController(bx - 250, 10.0, 0.3) # 0.1 #-0.05
y_vel = 0.4
t_vel = tanhController(o_ball.visionBearing, 10.0, 0.3)
if (rotation_dir > 0):
y_vel = y_vel * -1.
self.walk(x_vel, y_vel, t_vel)
def run(self):
global have_lock, facing_center, last_head_time
if ((self.getTime() - last_head_time) > 2.0):
commands.setHeadPan(random.uniform(-1.5, 1.5), 2.0)
commands.setHeadTilt(-14)
last_head_time = self.getTime()
lfl = sensors.getValue(core.fsrLFL)
lfr = sensors.getValue(core.fsrLFR)
lrl = sensors.getValue(core.fsrLRL)
lrr = sensors.getValue(core.fsrLRR)
rfl = sensors.getValue(core.fsrRFL)
rfr = sensors.getValue(core.fsrRFR)
rrl = sensors.getValue(core.fsrRRL)
rrr = sensors.getValue(core.fsrRRR)
max_force = numpy.amax([lfl,lfr,lrl,lrr,rfl,rfr,rrl,rrr])
# print "lfl is " + str(lfl)
# print "lfr is " + str(lfr)
# print "lrl is " + str(lrl)
# print "lrr is " + str(lrr)
# print "rfl is " + str(rfl)
# print "rfr is " + str(rfr)
# print "rrl is " + str(rrl)
# print "rrr is " + str(rrr)
# print "max is " + str(max_force)
if(numpy.abs(max_force) < 0.15):
self.kidnapped()
return
if not have_lock and not facing_center:
self.search()
elif have_lock and not facing_center:
self.turn()
elif have_lock and facing_center:
self.walk()
def run(self):
global have_lock, facing_center, last_head_time
if ((self.getTime() - last_head_time) > 2.0):
commands.setHeadPan(random.uniform(-1.5, 1.5), 2.0)
commands.setHeadTilt(-14)
last_head_time = self.getTime()
lfl = sensors.getValue(core.fsrLFL)
lfr = sensors.getValue(core.fsrLFR)
lrl = sensors.getValue(core.fsrLRL)
lrr = sensors.getValue(core.fsrLRR)
rfl = sensors.getValue(core.fsrRFL)
rfr = sensors.getValue(core.fsrRFR)
rrl = sensors.getValue(core.fsrRRL)
rrr = sensors.getValue(core.fsrRRR)
max_force = numpy.amax([lfl, lfr, lrl, lrr, rfl, rfr, rrl, rrr])
# print "lfl is " + str(lfl)
# print "lfr is " + str(lfr)
# print "lrl is " + str(lrl)
# print "lrr is " + str(lrr)
# print "rfl is " + str(rfl)
# print "rfr is " + str(rfr)
# print "rrl is " + str(rrl)
# print "rrr is " + str(rrr)
# print "max is " + str(max_force)
if (numpy.abs(max_force) < 0.15):
self.kidnapped()
return
if not have_lock and not facing_center:
self.search()
elif have_lock and not facing_center:
self.turn()
elif have_lock and facing_center:
self.walk()
def run(self):
commands.setHeadTilt()
commands.setStiffness()
if behavior_mem.test_odom_new:
self.otimer.reset()
behavior_mem.test_odom_new = False
if self.otimer.elapsed() > behavior_mem.test_odom_walk_time:
if self.stance != core.Poses.SITTING:
self.stance = core.Poses.SITTING
return pose.Sit()
return
vel_x = behavior_mem.test_odom_fwd
vel_y = behavior_mem.test_odom_side
vel_theta = behavior_mem.test_odom_turn
reqstance = behavior_mem.test_stance
if reqstance != self.stance:
self.stance = reqstance
if reqstance == core.Poses.SITTING:
return pose.Sit()
elif not util.isStanding():
return pose.Stand()
if self.stance == core.Poses.STANDING:
commands.setWalkVelocity(vel_x, vel_y, vel_theta)
def attack_rotate(self):
commands.setHeadTilt(-16)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir
o_self = mem_objects.world_objects[robot_state.WO_SELF]
o_ball = mem_objects.world_objects[core.WO_BALL]
o_goal = mem_objects.world_objects[core.WO_OPP_GOAL]
o_enemy = mem_objects.world_objects[core.WO_OPPONENT1]
if(o_ball.seen and o_goal.seen):
gy = o_goal.visionDistance * numpy.sin(o_goal.visionBearing)
if(numpy.abs(gy) <= 200):
self.stop()
current_state = AttackingStates.dribble
return
bx = o_ball.visionDistance * numpy.cos(o_ball.visionBearing)
x_vel = tanhController(bx - 250, 10.0, 0.3) # 0.1 #-0.05
y_vel = 0.4
t_vel = tanhController(o_ball.visionBearing, 10.0, 0.3)
if(rotation_dir > 0):
y_vel = y_vel * -1.
self.walk(x_vel, y_vel, t_vel)
def run(self):
commands.setHeadTilt(-10) # Tilt head up so we can see goal (default = -22)
commands.setWalkVelocity(0.4, 0, 0)
if self.getTime() > 15.0:
self.finish()
def run(self):
woself = core.world_objects.getObjPtr(core.robot_state.WO_SELF)
if inDuration(0.0, 2.0, self): ##START UP
commands.setStiffness()
commands.stand()
commands.setHeadTilt(-21)
# print('X(' + str(woself.loc.x) + ') Y(' + str(woself.loc.y) + ') ORIENT(' + str(woself.orientation) + ')\n' )
elif inDuration(0.0, self.walkEnd, self):
commands.setHeadPan(0.0, 1.0, False)
# print('IN DURATION ' + str(self.walkEnd) + ' < ' + str(self.getTime()) + '\n')
# actualDegrees = (woself.orientation * 180.0) / math.pi
# print('X(' + str(woself.loc.x) + ') Y(' + str(woself.loc.y) + ') ORIENT(' + str(woself.orientation) + ') DEGREES(' + str(actualDegrees) + ')\n' )
# commands.setWalkVelocity(0.3, 0.0, 0.0)
centAngle = math.atan(math.fabs(woself.loc.x)/math.fabs(woself.loc.y))
print('CENT BEFORE: ' + str(centAngle * (180/math.pi)))
if(woself.loc.x >= 0 and woself.loc.y >= 0):
centAngle = -(centAngle + (math.pi/2.0))
elif(woself.loc.x < 0 and woself.loc.y >= 0):
centAngle = -centAngle
elif(woself.loc.x > 0 and woself.loc.y < 0):
centAngle = centAngle + (math.pi/2.0)
else:
centAngle = centAngle
# print(' CENT ANGLE: ' + str(centAngle*(180.0/math.pi)) + ' TO THETA ' + str(woself.orientation*(180.0/math.pi)) + '\n')
# angleDiff = centAngle-self.safeOrientation
angleDiff = centAngle - woself.orientation#self.safeOrientation
if(angleDiff > math.pi):
angleDiff -= 2.0*math.pi
elif(angleDiff < -math.pi):
angleDiff += 2.0*math.pi
walkVelocity = 0.2
turnVelocity = 0.0
# print('FABS ' + str(math.fabs(angleDiff-math.pi)) + ' ' + str(math.fabs(angleDiff+math.pi)) + '\n')
# if(math.fabs(angleDiff-math.pi) < math.pi/8 or math.fabs(angleDiff+math.pi) < math.pi/8):
# turnVelocity = 0.0
# walkVelocity = -0.5
if(angleDiff < math.pi/8 and angleDiff > -math.pi/8):
turnVelocity = 0.0
elif(angleDiff > 0):
turnVelocity = -0.2
else:
turnVelocity = 0.2
if( woself.loc.x < 100 and woself.loc.x > -100 and woself.loc.y < 100 and woself.loc.y > -100):
commands.stand()
else:
commands.setWalkVelocity(walkVelocity, 0.0, turnVelocity)
else:
commands.stand()
numSteps = self.period / head.DiscreteScan.stepTime + 1
stepSize = (2 * self.maxPan / numSteps) * 1.05
st = self.state
self.orientationSamples.append(woself.orientation)
if len(self.orientationSamples)+1 >= 30:
self.orientationSamples.pop()
if self.sweepCounter > self.numSweeps:
self.walkEnd = self.getTime() + 6.0
self.sweepCounter = 0
self.safeOrientation = 0
for i in self.orientationSamples:
self.safeOrientation += i
self.safeOrientation = self.safeOrientation / len(self.orientationSamples)
self.orientationSamples = []
self.safeX = woself.loc.x
self.safeY = woself.loc.y
return
if st.inState(st.firstScan):
self.intDirection = self.direction
self.subtask = head.DiscreteScan(dest = self.direction * self.maxPan, stepSize = stepSize)
st.transition(st.nextScans)
return
if st.inState(st.nextScans) and self.subtask.finished():
self.sweepCounter += 1
self.intDirection *= -1
core.behavior_mem.completeBallSearchTime = core.vision_frame_info.seconds_since_start
self.subtask = head.DiscreteScan(dest = self.intDirection * self.maxPan, stepSize = stepSize, skipFirstPause = True)
return
def attack_align(self):
commands.setHeadTilt(-18)
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir, kick_sent, attack_left, r_goal_dist_filtered
o_self = mem_objects.world_objects[robot_state.WO_SELF]
o_ball = mem_objects.world_objects[core.WO_BALL]
o_goal = mem_objects.world_objects[core.WO_OPP_GOAL]
o_enemy = mem_objects.world_objects[core.WO_OPPONENT1]
if(enemy_state is EnemyGoalStates.unknown):
if(not o_enemy.seen):
#print "Can't find the enemy!"
enemy_state = EnemyGoalStates.center
self.walk(-0.3, 0.0, 0.0)
return
if(not o_goal.seen):
#print "Can't find the goal!"
self.walk(-0.3, 0.0, 0.0)
return
else:
gx = o_goal.visionDistance * numpy.cos(o_goal.visionBearing)
gy = o_goal.visionDistance * numpy.sin(o_goal.visionBearing)
ex = o_enemy.visionDistance * numpy.cos(o_enemy.visionBearing)
ey = o_enemy.visionDistance * numpy.sin(o_enemy.visionBearing)
center_threshold = o_goal.radius*0.1
#print "Threshold: " + str(center_threshold)
shift = gy - ey
if(numpy.abs(shift) < center_threshold):
# attack_left = bool(random.getrandbits(1))
attack_left = False
side_string = str("left" if attack_left else "right")
memory.speech.say("Enemy is in the center, shooting to the " + side_string)
enemy_state = EnemyGoalStates.center
elif(shift > 0.):
memory.speech.say("Enemy is on the right, shooting to the left")
# attack_left = True
attack_left = False
enemy_state = EnemyGoalStates.left
else:
memory.speech.say("Enemy is on the left, shooting to the right")
attack_left = False
enemy_state = EnemyGoalStates.right
if(not o_goal.seen or not o_ball.seen):
#print "Can't find the goal / ball"
self.walk(-0.3, 0.0, 0.05)
return
gx = o_goal.visionDistance * numpy.cos(o_goal.visionBearing)
gy = o_goal.visionDistance * numpy.sin(o_goal.visionBearing)
bx = o_ball.visionDistance * numpy.cos(o_ball.visionBearing)
by = o_ball.visionDistance * numpy.sin(o_ball.visionBearing)
ex = o_enemy.visionDistance * numpy.cos(o_enemy.visionBearing)
ey = o_enemy.visionDistance * numpy.sin(o_enemy.visionBearing)
tx = gx
ty = gy
dx_gb = gx - bx
dy_gb = gy - by
dt_gb = numpy.arctan2(dy_gb, dx_gb)
r_goal_ball = numpy.sqrt(dx_gb * dx_gb + dy_gb * dy_gb)
alpha = 0.3
r_goal_dist_filtered = alpha*r_goal_ball + (1.0-alpha)*r_goal_dist_filtered
if(r_goal_dist_filtered > 2000):
current_state = AttackingStates.dribble
# memory.speech.say("Distance " + str(int(r_goal_dist_filtered)) + ", going back")
return
if(o_enemy.seen):
if(attack_left):
enemy_edge = ey + numpy.sqrt(o_enemy.radius)
goal_edge = gy + o_goal.radius #yes it's supposed to be different
ty = (enemy_edge+goal_edge)/2.0
else:
enemy_edge = ey - numpy.sqrt(o_enemy.radius)
goal_edge = gy - o_goal.radius #yes it's supposed to be different
ty = (enemy_edge+goal_edge)/2.0
else:
if(attack_left):
ty += 3.0 * o_goal.radius / 8.
else:
ty -= 3.0 * o_goal.radius / 8.
#print "gx: " + str(gx)
#print "gy: " + str(gy)
#print "tx: " + str(tx)
#print "ty: " + str(ty)
#print "bx: " + str(bx)
#print "by: " + str(by)
threshold = 65
ball_x_target = 95
ball_y_target = -100
goal_y_target = -100
#override?
tx = gx
ty = gy
if(attack_left):
ball_y_target += 0
ty += 250
else:
ball_y_target -= 0
ty -= 250
if (numpy.abs(bx - ball_x_target) <= threshold) and (numpy.abs(by - ball_y_target) <= threshold) and (numpy.abs(ty - goal_y_target) <= threshold) and (numpy.abs(ty - by) <= threshold):
kick_sent = False
current_state = AttackingStates.kick
return
x_vel = tanhController(-(ball_x_target - bx), 10.0/1000.0, 0.3)
y_vel = tanhController(-(ball_y_target - by), 10.0/1000.0, 0.3)
t_vel = tanhController((ty - by), 12.0/1000.0, 0.3)
#print "vel x,y,t = " + str(x_vel) + "," + str(y_vel) + "," + str(t_vel)
self.walk(x_vel, y_vel, t_vel)
def run(self):
commands.setHeadTilt(-10) # Tilt head up so we can see goal (default = -22)
commands.stand()
if self.getTime() > 3.0:
self.finish()
def run(self):
commands.setHeadTilt(-10) # Tilt head up so we can see goal (default = -22)
pose.Sit()
if self.getTime() > 5.0:
memory.speech.say("Sitting complete")
self.finish()
def defense_walk_center(self): commands.setHeadTilt(-13) commands.setWalkVelocity(0.0,0.0,0.0)
def run(self):
# These are max values for Gene to adjust itself
vel_y_gain = -0.50
vel_x_gain = 0.50
vel_turn_gain = 1.00
vel_y = -0.50 # This is the tangential velocity, fix it (try -0.50)
vel_x = 0
vel_turn = 0.30
goal_aligned = False
# Target position of the ball in bottom camera
# # x_desired = 360.0 / 2
# x_desired = 120 # Ball near left foot
# # y_desired = 240.0 / 2
# y_desired = 200 # Ball near the bottom of the frame when looking up
x_desired = 125.0
y_desired = 190.0
# Goal centered threshold
goal_x_right_threshold = 360.0 / 2 + 30
goal_x_left_threshold = 360.0 / 2 - 30
ball = memory.world_objects.getObjPtr(core.WO_BALL)
goal = memory.world_objects.getObjPtr(core.WO_OPP_GOAL)
# Make sure that the ball is always at the same position relative to Gene
if ball.seen:
if ball.fromTopCamera:
memory.speech.say("Align goal The ball is in top frame")
self.postSignal("restart")
else:
# We Calculate the deviation from the center of the frame
# to adjust our rotation speed
# Y determines vel_x to make sure that Gene
# is always a fix distance from the ball
vel_x = vel_x_gain * (y_desired - ball.imageCenterY) / (y_desired)
# X determines the vel_turn. Since we want to always turn
# that's why there's an offset
# vel_turn = 0.25 - (x_desired - ball.imageCenterX) / (x_desired) * 0.25
vel_turn = vel_turn_gain * (x_desired - ball.imageCenterX) / (x_desired)
if goal.seen:
if not goal.fromTopCamera:
memory.speech.say("Why is the goal in bottom frame?")
else:
# Align goal toward center of the frame
memory.speech.say("I see goal")
# Slow down when the goal is closer to alignment (center of top frame)
vel_y = vel_y / 2
if (goal.imageCenterX < goal_x_right_threshold) and (goal.imageCenterX > goal_x_left_threshold):
goal_aligned = True
# Exit condition
if goal_aligned:
self.finish()
# commands.setWalkvelocity(0, -0.50, 0.25)
commands.setWalkVelocity(vel_x, vel_y, vel_turn)
commands.setHeadTilt(-10) # Tilt head up so we can see goal (default = -22)
def run(self): commands.setHeadTilt(-13) commands.setWalkVelocity(0.20,0.1,0.04)
def defense_start(self):
global EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir
global if_seen_history_constant , last_inf_constant
global if_seen_history , if_seen_history_counter , last_x , last_y , last_xv , last_yv , last_bearing , last_distance , last_state_counter,last_av_px
global friction, dt
global last_head_time, last_head_pan, body_turning_flag, block_trigger_flag, pose_sent, num_times_sent
# commands.stand()
commands.setHeadTilt(-13)
ball = mem_objects.world_objects[core.WO_BALL]
if_seen_history[if_seen_history_counter] = ball.seen;
if_seen_history_counter = (if_seen_history_counter + 1)%if_seen_history_constant;
seen_times = sum(if_seen_history)
x = 0.
y = 0.
d = 0.
bearing = 0.
xv = 0.
yv = 0.
px = 0.
if(seen_times > ( if_seen_history_constant/2 )): # It is actually seen
#print "seen_times = " + str(seen_times)
if(ball.seen):
bearing = ball.visionBearing
distance = ball.visionDistance
x = ball.loc.x
y = ball.loc.y
xv = ball.absVel.x
yv = ball.absVel.y
else:
return
if( abs(xv) > 3000 or abs(yv) > 3000) :
return
#print "x = " + str(x) + " y = " + str(y) + " xv = " + str(xv) +" yv = " + str(yv)
last_av_bearing = sum(last_bearing)/last_inf_constant
last_x[last_state_counter] = x
last_y[last_state_counter] = y
last_xv[last_state_counter] = xv
last_yv[last_state_counter] = yv
last_distance[last_state_counter] = distance
last_bearing[last_state_counter] = bearing
# av_x = x
# av_y = y
# av_xv = xv
# av_yv = yv
# av_distance = distance
# av_bearing = bearing
av_x = sum(last_x)/last_inf_constant
av_y = sum(last_y)/last_inf_constant
av_xv = sum(last_xv)/last_inf_constant
av_yv = sum(last_yv)/last_inf_constant
av_distance = sum(last_distance)/last_inf_constant
av_bearing = sum(last_bearing)/last_inf_constant
px = av_x + (av_xv*abs(av_xv)/(2*1000*friction))
last_av_px[last_state_counter] = px
av_px = sum(last_av_px)/last_inf_constant
last_state_counter = (last_state_counter + 1)%last_inf_constant
#print "av_vx = " + str(av_xv) + "\tav_vy = " + str(av_yv)
#print("av_px = ") + str(av_px) + " \tseen_times = " + str(seen_times)
#print "Avg : av_x = " + str(av_x) + " av_y = " + str(av_y) + " av_xv = " + str(av_xv) +" av_yv = " + str(av_yv)
if(av_bearing > 1.4):
av_bearing = 1.4
elif(av_bearing < -1.4):
av_bearing = -1.4
d_turning = abs(av_bearing - last_av_bearing)/2.5
if(d_turning < 0.1):
d_turning = 0.1
elif(d_turning > 2):
d_turning = 2
commands.setHeadPan(av_bearing, d_turning)
#print "av_xy = " + str(av_xv) + "\tav_y = " + str(av_yv) + "\tflag = " + str(body_turning_flag) + "\tPAN: " + str(core.joint_values[core.HeadYaw])
if( (abs(av_xv) < 300 and abs(av_yv) < 300 ) or body_turning_flag == 1):
head_pan = core.joint_values[core.HeadYaw]
if(head_pan > 0.2):
body_turning_flag = 1
commands.setWalkVelocity(0.1, 0.4, 0.13)
elif(head_pan < -0.2):
body_turning_flag = 1
commands.setWalkVelocity(0.15,-0.3,-0.05)
else:
commands.stand()
body_turning_flag = 0
return
if(av_xv > -200 or (abs(av_yv)+0.01)/(abs(av_xv)+0.01) > 1):
#print(" No!!!!: Vx > 0 or Vx / Vy large , Vx = ") + str(av_xv) + " Vy = " + str(av_yv) + " seen_times = " + str(seen_times)
block_trigger_flag = 0
return
elif( av_px > -1100 ):
#print(" No!!!!: Ball too short px = ") + str(av_px) + " seen_times = " + str(seen_times)
block_trigger_flag = 0
return
elif( av_distance < 1200 and av_xv < -100):
block_trigger_flag = block_trigger_flag + 1
lamda = av_yv / av_xv
intercept = av_y - lamda*av_x
hit_goal_line = lamda*(-1300) + intercept
if(block_trigger_flag > 2):
#print "========================================================================="
#print "========================================================================="
#print "========================================================================="
#print "========================================================================="
#print "========================================================================="
#print "========================================================================="
block_trigger_flag = 0
if( hit_goal_line < -200 and hit_goal_line > -550):
#print " Left : Yes: av_px = " + str(av_px) + " hit_goal_line = " + str(hit_goal_line)
pose_sent = False
num_times_sent = 0
current_state = DefendingStates.block_right
#choice = "right"
#self.postSignal(choice)
elif( hit_goal_line > 200 and hit_goal_line < 550):
#print " Right : av_px = " + str(av_px) + " hit_goal_line = " + str(hit_goal_line)
pose_sent = False
num_times_sent = 0
current_state = DefendingStates.block_left
# choice = "left"
# self.postSignal(choice)
elif( hit_goal_line > -200 and hit_goal_line < 200):
#print " Center: av_px = " + str(av_px) + " hit_goal_line = " + str(hit_goal_line)
pose_sent = False
num_times_sent = 0
current_state = DefendingStates.block
# choice = "center"
# self.postSignal(choice)
else: # Think ball is still not seen
#memory.speech.say("No ball")
commands.stand()
if ((self.getTime() - last_head_time) > 3):
if(last_head_pan > 0 ):
last_head_pan = -1.2
elif(last_head_pan <= 0):
last_head_pan = 1.2
commands.setHeadPan( last_head_pan , 2.5 )
last_head_time = self.getTime()
last_x = [0]*last_inf_constant
last_y = [0]*last_inf_constant
last_xv = [0]*last_inf_constant
last_yv = [0]*last_inf_constant
last_distance =[0]*last_inf_constant
last_bearing = [0]*last_inf_constant
def defense_walk_right(self): commands.setHeadTilt(-13) commands.setWalkVelocity(0.0,-0.5,0.05)
def run(self):
commands.setHeadPan(self.pan, self.time)
if self.tilt is not None:
commands.setHeadTilt(self.tilt)
if self.getTime() > self.time: self.finish()
