def run(self):
commands.stand()
global last_head_time
last_head_time = 0
if self.getTime() > 2.0:
memory.speech.say("Be a man")
self.finish()
def run(self):
sm = self.sm
ball = core.world_objects.getObjPtr(core.WO_BALL)
kreq = core.kick_request
kreq.ball_seen_ = ball.seen
kreq.ball_image_center_x_ = ball.imageCenterX
kreq.ball_image_center_y_ = ball.imageCenterY
kreq.ball_rel_x_ = ball.relPos.x
kreq.ball_rel_y_ = ball.relPos.y
if sm.inState(sm.startup):
print "_____kicks.py "
commands.stand()
if sm.timeSinceTransition() > 3.0:
print "_____kicks.py 2 "
sm.transition(sm.kicking)
kreq.kick_running_ = True
else: return
if sm.inState(sm.kicking):
print "_____kicks.py 3 "
core.walk_request.noWalk()
if sm.timeSinceTransition() > 1.0:
sm.transition(sm.finish)
print "_____kicks.py 4 "
kreq.set(core.Kick.STRAIGHT, self.foot, 0, self.desiredDistance)
if sm.inState(sm.finish):
self.finish()
def run(self):
commands.setWalkVelocity(0, 0, -0.25)
if self.getTime() > 6.0:
commands.stand()
if self.getTime() > 7.0:
self.finish()
def run(self):
if self.getFrames() <= 3:
memory.walk_request.noWalk()
memory.kick_request.setFwdKick()
if self.getFrames() > 10 and not memory.kick_request.kick_running_:
commands.stand()
self.finish()
def run(self):
commands.stand()
commands.setHeadPan(self.direction * 50 * core.DEG_T_RAD, 0.5)
if abs(core.joint_values[core.HeadYaw]) > 45 * core.DEG_T_RAD:
if core.joint_values[core.HeadYaw] > 0:
self.direction = -1
else:
self.direction = 1
ct = 0
seen = False
for b in self.beacons:
beacon = world_objects.getObjPtr(b)
if beacon.seen:
seen = True
ct += 1
if ct >= 2:
self.finish()
if seen:
self.seenCt += 1
else:
self.seenCt = max(self.seenCt - 1, 0)
if self.seenCt > 5:
self.seenCt = 0
self.finish()
def defense_sit(self):
global current_state, DefendingStates, pose_sent, pose_start_time, num_times_sent
if(self.toPose({
core.HeadYaw: 1.8433177928247,
core.HeadPitch: -22.497878144835,
core.LHipYawPitch: -4.18321199506924,
core.LHipRoll: 0.261268396131328,
core.LHipPitch: -46.5802099129511,
core.LKneePitch: 123.485476193518,
core.LAnklePitch: -70.5794592600033,
core.LAnkleRoll: -0.0902951008275686,
core.RHipYawPitch: -5.18321199506924,
core.RHipRoll: 0.266076849307017,
core.RHipPitch: -47.2002681461832,
core.RKneePitch: 124.72075688605,
core.RAnklePitch: -70.3988690583481,
core.RAnkleRoll: -0.0854866476518796,
core.LShoulderPitch: -92.0202358571845,
core.LShoulderRoll: 2.54645844712083,
core.RShoulderPitch: -92.1129420147891,
core.RShoulderRoll: 2.55126690029652,
}
, 1.5
)):
#print "pose time: " + str(pose_start_time)
#print "current time: " + str(self.getTime())
if (self.getTime() - pose_start_time) > 6.5:
pose_start_time = 0
pose_sent = False
num_times_sent = 0
#print "CHANGING TO START"
current_state = DefendingStates.start
elif (self.getTime() - pose_start_time) > 3.5:
commands.stand()
def defense_sit(self):
global current_state, DefendingStates, pose_sent, pose_start_time, num_times_sent
if (self.toPose(
{
core.HeadYaw: 1.8433177928247,
core.HeadPitch: -22.497878144835,
core.LHipYawPitch: -4.18321199506924,
core.LHipRoll: 0.261268396131328,
core.LHipPitch: -46.5802099129511,
core.LKneePitch: 123.485476193518,
core.LAnklePitch: -70.5794592600033,
core.LAnkleRoll: -0.0902951008275686,
core.RHipYawPitch: -5.18321199506924,
core.RHipRoll: 0.266076849307017,
core.RHipPitch: -47.2002681461832,
core.RKneePitch: 124.72075688605,
core.RAnklePitch: -70.3988690583481,
core.RAnkleRoll: -0.0854866476518796,
core.LShoulderPitch: -92.0202358571845,
core.LShoulderRoll: 2.54645844712083,
core.RShoulderPitch: -92.1129420147891,
core.RShoulderRoll: 2.55126690029652,
}, 1.5)):
#print "pose time: " + str(pose_start_time)
#print "current time: " + str(self.getTime())
if (self.getTime() - pose_start_time) > 6.5:
pose_start_time = 0
pose_sent = False
num_times_sent = 0
#print "CHANGING TO START"
current_state = DefendingStates.start
elif (self.getTime() - pose_start_time) > 3.5:
commands.stand()
def run(self):
commands.stand()
commands.setWalkVelocity(0, 0, 0.05)
if self.getTime() > 5.0:
commands.setWalkVelocity(0, 0, 0)
memory.speech.say("Starting localization")
self.finish()
def run(self):
commands.setWalkVelocity(0, 0, -0.25)
if self.getTime() > 6.0:
commands.stand()
if self.getTime() > 7.0:
self.finish()
def run(self):
commands.stand()
commands.setHeadPanTilt(0.0, 0.0, 1.5)
if self.getTime() > 4.5:
location = memory.planning.getDestPose()
# print("[%f,%f,%f]" % (location.translation.x, location.translation.y, location.rotation))
self.finish()
def run(self):
sm = self.sm
ball = core.world_objects.getObjPtr(core.WO_BALL)
kreq = core.kick_request
kreq.ball_seen_ = ball.seen
kreq.ball_image_center_x_ = ball.imageCenterX
kreq.ball_image_center_y_ = ball.imageCenterY
kreq.ball_rel_x_ = ball.relPos.x
kreq.ball_rel_y_ = ball.relPos.y
if sm.inState(sm.startup):
commands.stand()
if sm.timeSinceTransition() > 3.0:
sm.transition(sm.kicking)
kreq.kick_running_ = True
else: return
if sm.inState(sm.kicking):
core.walk_request.noWalk()
if sm.timeSinceTransition() > 1.0:
sm.transition(sm.finish)
kreq.set(core.Kick.STRAIGHT, self.foot, 0, self.desiredDistance)
if sm.inState(sm.finish):
self.finish()
def run(self):
commands.stand()
global last_head_time
last_head_time = 0
if self.getTime() > 2.0:
memory.speech.say("Be a man")
self.finish()
def run(self):
commands.stand()
commands.setHeadPan(self.direction * 50 * core.DEG_T_RAD, 0.5)
if abs(core.joint_values[core.HeadYaw]) > 45 * core.DEG_T_RAD:
if core.joint_values[core.HeadYaw] > 0:
self.direction = -1
else:
self.direction = 1
ct = 0
seen = False
for b in self.beacons:
beacon = world_objects.getObjPtr(b)
if beacon.seen:
seen = True
ct += 1
if ct >= 2:
self.finish()
if seen:
self.seenCt += 1
else:
self.seenCt = max(self.seenCt - 1, 0)
if self.seenCt > 5:
self.seenCt = 0
self.finish()
def run(self):
robot = memory.world_objects.getObjPtr(5)
print("robot ", robot.loc)
print("orientation ", robot.orientation)
commands.stand()
if self.getTime() > 5.0:
memory.speech.say("playing stand complete")
self.finish()
def run(self):
commands.stand()
commands.setHeadPan(self.direction * 50 * core.DEG_T_RAD, 0.5)
if abs(core.joint_values[core.HeadYaw]) > 45 * core.DEG_T_RAD:
if core.joint_values[core.HeadYaw] > 0:
self.direction = -1
else:
self.direction = 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):
commands.stand()
commands.setHeadPan(self.direction * 50 * core.DEG_T_RAD, 0.5)
if abs(core.joint_values[core.HeadYaw]) > 45 * core.DEG_T_RAD:
if core.joint_values[core.HeadYaw] > 0:
self.direction = -1
else:
self.direction = 1
def run(self):
robot = core.world_objects.getObjPtr(core.robot_state.WO_SELF)
print robot.loc.x
print robot.loc.y
print robot.orientation
print "one"
if self.getTime() > 30.0:
commands.stand()
self.postSuccess()
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 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 run(self):
print "Track {0}".format(self.lostCount)
commands.stand()
ball = world_objects.getObjPtr(core.WO_BALL)
angle = core.joint_values[core.HeadYaw]
if ball.seen:
self.lostCount = max(0,self.lostCount - 1)
angle = ball.visionBearing
print angle
commands.setHeadPan(angle, 0.2)
else:
self.lostCount = self.lostCount + 1
if self.lostCount > 7:
self.lostCount = 0
self.finish()
def run(self):
# commands.setHeadTilt(tilt_angle)
commands.stand()
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)}
beacon_seen = [beacons[beacon].seen for beacon in beacons]
no_seen_beacons = sum([int(bs) for bs in beacon_seen])
sd_pos = self.robot.sd.getMagnitude()
sd_or = self.robot.sdOrientation
print("robot ", self.robot.loc," orientation: ",self.robot.orientation)
print("Position covariance ",self.robot.sd.getMagnitude(),"cov_or: ",self.robot.sdOrientation)
err_x = self.goal_x - self.robot.loc.x
err_y = self.goal_y - self.robot.loc.y
if (sd_pos > xy_threshold or sd_or > t_threshold) and not self.centerFlag:
if no_seen_beacons < 2 and core.joint_values[core.HeadYaw] <=0.75 and self.yawFlag:
self.postSignal("lookleft")
elif no_seen_beacons < 2 and core.joint_values[core.HeadYaw] >= -0.75:
self.yawFlag = False
self.postSignal("lookright")
elif no_seen_beacons>=2:
self.postSignal("holdlook")
self.yawFlag = False
else:
self.yawFlag = True
else:
if (err_x**2+err_y**2)**0.5 < center_threshold and (err_x**2+err_y**2)**0.5 > 0.0 and sd_pos<75:
self.postSignal("reachedcenter")
elif (err_x**2+err_y**2)**0.5 < center_threshold and (err_x**2+err_y**2)**0.5 > 0.0 and sd_pos>=75:
self.postSignal("turn")
elif self.robot.loc.getMagnitude()>=500 and not self.centerFlag:
global point_x,point_y,point_t
point_x = -250*math.copysign(1,self.robot.loc.x)
point_y = -250*math.copysign(1,self.robot.loc.x)
point_t = math.atan2(point_y,point_x)
self.postSignal("movepoint")
elif no_seen_beacons>=2 or 1 == 1:
self.centerFlag = True
self.postSignal("movecenter")
elif no_seen_beacons<2:
self.centerFlag = True
self.postSignal("turn")
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):
global robotlocx, robotlocy
robotlocx=0.5*robotlocx+0.5*self.robot.loc.x
robotlocy=0.5*robotlocy+0.5*self.robot.loc.y
# commands.setHeadTilt(tilt_angle)
commands.stand()
ball = mem_objects.world_objects[core.WO_BALL]
commands.stand()
# print('x ', ball.px,' y ', ball.py)
if self.hld_bdrin == 6:
self.hld_bdrin = 6
else:
self.hld_bdrin = ball.dirn
print("Balldir ",self.hld_bdrin)
if not self.lineflag:
# commands.setHeadPan(ball.bearing, 0.1)
print('Moving to line')
# print('Line distance: ', line.visionDistance )
self.hld_bdrin = 5
self.postSignal("LineUp")
if self.line.visionDistance < 120:
self.lineflag = True
elif self.lineflag and not ball.seen and self.hld_bdrin >= 5:
self.postSignal("ball search")
elif self.lineflag and not self.rotateflag and ball.seen and self.hld_bdrin >= 5:
self.hld_bdrin = 5
self.postSignal("rotate")
if self.line.visionDistance > 200:
self.lineflag = False
if abs(ball.visionBearing) > 0.04:
self.rotateflag = False
elif self.hld_bdrin == 0 and self.lineflag:
self.hld_bdrin = 6
print('Blocking left')
self.postSignal("left")
elif self.hld_bdrin == 2 and self.lineflag:
print('Blocking center')
self.postSignal("center")
self.hld_bdrin = 6
elif self.hld_bdrin == 1 and self.lineflag:
print('Blocking right')
self.postSignal("right")
self.hld_bdrin = 6
def run(self):
print "Search: {0}, {1}".format(self.direction, self.seenCount)
commands.setStiffness(cfgstiff.One)
commands.stand()
current_angle = core.joint_values[core.HeadYaw]
commands.setHeadPan(self.direction * 55 * DEG_T_RAD, 1.0)
ball = world_objects.getObjPtr(core.WO_BALL)
if ball.seen:
self.seenCount = self.seenCount + 1
else:
self.seenCount = max(0,self.seenCount - 1)
if self.direction == 1 and current_angle > 45 * DEG_T_RAD:
self.direction = -1
if self.direction == -1 and current_angle < -45 * DEG_T_RAD:
self.direction = 1
if self.seenCount > 3:
self.seenCount = 0
self.finish()
print self.seenCount
def run(self):
robot = core.world_objects.getObjPtr(core.robot_state.WO_SELF)
print "** Current Location X: %d Y: %d" % (robot.loc.x, robot.loc.y)
print "** Confidence: " + str(robot.radius)
print "** Distance: " + str(robot.visionDistance)
print "** Bearing: " + str(robot.visionBearing)
print "** Orientation: " + str(robot.orientation)
print "** RADIUS: " + str(robot.radius)
commands.stand()
if self.getTime() > 4:
if (robot.radius < 0.40 and robot.radius > 0.0):
self.postSignal(Choices.Stand)
elif (robot.radius < 0.65 and robot.radius > 0.0):
commands.stand()
elif robot.visionDistance > DTHRESHOLD:
print "_______Distance __"
value = min(0.001 * robot.visionDistance, 0.30)
commands.setWalkVelocity(value, 0.0, kp*robot.visionBearing)
else:
commands.stand()
print "_____Localization Complete______ " + str(robot.visionDistance)
#core.speech.say("Localization Complete")
self.postSignal(Choices.Stand)
if (self.getTime() > 9 and robot.visionDistance < ATHRESHOLD and robot.visionDistance > DTHRESHOLD):
core.speech.say("Approach")
self.postSignal(Choices.Stand)
def run(self):
print "Stand"
commands.stand()
beacon_b_y = world_objects.getObjPtr(core.WO_BEACON_BLUE_YELLOW)
beacon_y_b = world_objects.getObjPtr(core.WO_BEACON_YELLOW_BLUE)
beacon_b_p = world_objects.getObjPtr(core.WO_BEACON_BLUE_PINK)
beacon_p_b = world_objects.getObjPtr(core.WO_BEACON_PINK_BLUE)
beacon_p_y = world_objects.getObjPtr(core.WO_BEACON_PINK_YELLOW)
beacon_y_p = world_objects.getObjPtr(core.WO_BEACON_YELLOW_PINK)
if beacon_b_y.seen:
print "BLUE YELLOW DISTANCE: {0}".format(beacon_b_y.visionDistance)
if beacon_y_b.seen:
print "YELLOW BLUE DISTANCE: {0}".format(beacon_y_b.visionDistance)
if beacon_b_p.seen:
print "BLUE PINK DISTANCE: {0}".format(beacon_b_p.visionDistance)
if beacon_p_b.seen:
print "PINK BLUE DISTANCE: {0}".format(beacon_p_b.visionDistance)
if beacon_p_y.seen:
print "PINK YELLOW DISTANCE: {0}".format(beacon_p_y.visionDistance)
if beacon_y_p.seen:
print "YELLOW PINK DISTANCE: {0}".format(beacon_y_p.visionDistance)
def run(self):
print "Stand"
commands.stand()
beacon_b_y = world_objects.getObjPtr(core.WO_BEACON_BLUE_YELLOW)
beacon_y_b = world_objects.getObjPtr(core.WO_BEACON_YELLOW_BLUE)
beacon_b_p = world_objects.getObjPtr(core.WO_BEACON_BLUE_PINK)
beacon_p_b = world_objects.getObjPtr(core.WO_BEACON_PINK_BLUE)
beacon_p_y = world_objects.getObjPtr(core.WO_BEACON_PINK_YELLOW)
beacon_y_p = world_objects.getObjPtr(core.WO_BEACON_YELLOW_PINK)
if beacon_b_y.seen:
print "BLUE YELLOW DISTANCE: {0}".format(beacon_b_y.visionDistance)
if beacon_y_b.seen:
print "YELLOW BLUE DISTANCE: {0}".format(beacon_y_b.visionDistance)
if beacon_b_p.seen:
print "BLUE PINK DISTANCE: {0}".format(beacon_b_p.visionDistance)
if beacon_p_b.seen:
print "PINK BLUE DISTANCE: {0}".format(beacon_p_b.visionDistance)
if beacon_p_y.seen:
print "PINK YELLOW DISTANCE: {0}".format(beacon_p_y.visionDistance)
if beacon_y_p.seen:
print "YELLOW PINK DISTANCE: {0}".format(beacon_y_p.visionDistance)
def run(self):
commands.stand()
#commands.setStiffness(cfgstiff.Zero, 0.3)
if self.getTime() > 5:
memory.speech.say("playing stand complete")
self.finish()
def run(self):
commands.stand()
def run(self):
global walk_start_time, EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir, kick_sent, last_mode_change_time
#detect mode switches
hf = sensors.getValue(core.headFront)
hm = sensors.getValue(core.headMiddle)
hr = sensors.getValue(core.headRear)
# print "Head: " + str(hf) + ", " + str(hm) + ", " + str(hr)
if(mode is not Modes.passive and hf and hm and hr): #need to switch of passive mode
self.stop()
memory.speech.say("Passive Mode!")
mode = Modes.passive
last_mode_change_time = self.getTime()
if(mode is not Modes.attacking and hf and hm and not hr and (self.getTime() - last_mode_change_time) > 1.0): #need to switch to attack mode
memory.speech.say("Attack Mode!")
mode = Modes.attacking
enemy_state = EnemyGoalStates.unknown
kick_sent = False
r_goal_dist_filtered = 0.
current_state = AttackingStates.start
last_mode_change_time = self.getTime()
if(mode is not Modes.defending and hm and hr and not hf and (self.getTime() - last_mode_change_time) > 1.0): #need to switch to defense mode
memory.speech.say("Defense Mode!")
commands.stand()
pose_sent = False
num_times_sent = 0
pose_start_time = 0
mode = Modes.defending
current_state = DefendingStates.walk
walk_start_time = self.getTime()
last_mode_change_time = self.getTime()
#detect kidnapping
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)
# print "tilt " + str((lfl+lrl)/2.-(lfr+lrr)/2.)
if(numpy.abs(max_force) < 0.10 and mode is not Modes.passive):
#print("***************************************************")
#print("***************************************************")
#print("***************************************************")
#print("numpy.abs(max_force) = ") + str(numpy.abs(max_force))
memory.speech.say("Put me down!")
self.stop()
mode = Modes.passive
#execute the appropriate function
if(mode is Modes.passive):
#print "passive"
self.track_ball()
self.stop()
return
else:
function_map = {}
if(mode is Modes.attacking):
#print "Attack mode"
function_map = {AttackingStates.start:self.attack_start,
AttackingStates.approach:self.attack_approach,
AttackingStates.rotate:self.attack_rotate,
AttackingStates.dribble:self.attack_dribble,
AttackingStates.align:self.attack_align,
AttackingStates.kick:self.attack_kick}
else:
#print "Defense mode"
function_map = {DefendingStates.start:self.defense_start,
DefendingStates.walk_center:self.defense_walk_center,
DefendingStates.walk_left:self.defense_walk_left,
DefendingStates.walk_right:self.defense_walk_right,
DefendingStates.walk:self.defense_walk,
DefendingStates.block:self.defense_block,
DefendingStates.block_left:self.defense_block_left,
DefendingStates.block_right:self.defense_block_right,
DefendingStates.sit:self.defense_sit
}
#print "Current state: " + str(current_state)
function_map[current_state]()
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):
global robotlocx, robotlocy
commands.setHeadTilt(tilt_angle)
commands.stand()
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)
}
beacon_seen = [beacons[beacon].seen for beacon in beacons]
no_seen_beacons = sum([int(bs) for bs in beacon_seen])
sd_pos = self.robot.sd.getMagnitude()
sd_or = self.robot.sdOrientation
print("robot ", self.robot.loc, " orientation: ",
self.robot.orientation)
print("Position covariance ", self.robot.sd.getMagnitude(), "cov_or: ",
self.robot.sdOrientation)
print('scanFlag ', self.scanFlag)
robotlocx = 0.01 * robotlocx + 0.99 * self.robot.loc.x
robotlocy = 0.01 * robotlocy + 0.99 * self.robot.loc.y
# print("robot particle:",self.robot.loc)
# print("smooth robot estimate",robotlocx,robotlocy)
# err_x = self.goal_x - self.robot.loc.x #robotlocx
# err_y = self.goal_y - self.robot.loc.y #robotlocy
err_x = self.goal_x - robotlocx
err_y = self.goal_y - robotlocy
if self.turnFlag:
if no_seen_beacons > 0:
self.turnFlag = False
self.postSignal("turn")
if (sd_pos > xy_threshold or sd_or > t_threshold
) and not self.scanFlag and not self.turnFlag:
self.scanFlag = True
self.yawFlag = True
self.postSignal("holdlook")
# else:
# self.postSignal("turn")
elif not self.scanFlag and not self.turnFlag:
if sd_pos < 80:
self.closeflag = True
if (err_x**2 + err_y**2)**0.5 < center_threshold and (
err_x**2 + err_y**2)**0.5 > 0.0 and sd_pos < 30:
self.postSignal("reachedcenter")
elif (err_x**2 + err_y**2)**0.5 < center_threshold and (
err_x**2 + err_y**2)**0.5 > 0.0 and sd_pos >= 30:
self.closeflag = False
self.scanFlag = True
print('Setting scanflag true because err > thresh & covar>100')
elif (err_x**2 + err_y**2)**0.5 >= 300:
global point_x, point_y, point_t
point_x = -50 * math.copysign(1, self.robot.loc.x)
point_y = -50 * math.copysign(1, self.robot.loc.y)
point_t = math.atan2(point_y, point_x)
print("Desired angle: ", point_t)
self.yawFlag = True
self.postSignal("movepoint")
elif ((err_x**2 + err_y**2)**0.5 >= center_threshold and
(err_x**2 + err_y**2)**0.5 < 300) and sd_pos <= 45:
#self.centerFlag = True
self.yawFlag = True
self.postSignal("movecenter")
else:
self.scanFlag = True
self.postSignal("holdlook")
# #self.centerFlag = True
# if no_seen_beacons < 2 and core.joint_values[core.HeadYaw] <=0.75 and self.yawFlag:
# self.postSignal("lookleft")
# elif no_seen_beacons < 2 and core.joint_values[core.HeadYaw] >= -0.75:
# self.yawFlag = False
# self.postSignal("lookright")
# elif no_seen_beacons>=2:
# self.postSignal("holdlook")
# self.yawFlag = False
# else:
# self.postSignal("turn")
print("We missed something so setting scanflag to true")
elif self.scanFlag and not self.turnFlag:
# if no_seen_beacons < 2 and core.joint_values[core.HeadYaw] <=0.95 and self.yawFlag:
if core.joint_values[core.HeadYaw] <= 0.95 and self.yawFlag:
# if sd_pos<35:
# self.scanFlag = False
self.postSignal("lookleft")
# elif no_seen_beacons < 2 and core.joint_values[core.HeadYaw] >= -0.95:
elif core.joint_values[core.HeadYaw] >= -0.95:
# if sd_pos<35:
# self.scanFlag = False
self.yawFlag = False
self.postSignal("lookright")
# elif no_seen_beacons>=2:
# self.yawFlag = False
# self.scanFlag=False
# self.postSignal("holdlook")
else:
self.scanFlag = False
self.turnFlag = True
self.postSignal("holdlook")
def run(self):
commands.stand()
commands.setHeadTilt(-30.0)
if self.getTime() > 1.5:
self.finish()
def run(self):
commands.stand()
#commands.setStiffness(cfgstiff.Zero, 0.3)
if self.getTime() > 5:
memory.speech.say("playing stand complete")
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 run(self):
commands.stand()
if self.getTime() > 3.0:
# memory.speech.say("Standing complete")
self.finish()
def run(self):
commands.stand()
if self.getTime() > 5.0:
memory.speech.say("playing stand complete")
self.finish()
def run(self):
commands.setWalkVelocity(0, 0, -.25)
if self.getTime() > 4.0:
commands.stand()
self.postSuccess()
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 run(self):
commands.setWalkVelocity(.5, 0, 0)
if self.getTime() > 2.0:
commands.stand()
self.postSuccess()
def run(self):
print "______ SCAN _____"
commands.stand()
if self.getTime() > 1.0:
self.postSuccess()
def run(self): print "Stand" commands.stand()
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 run(self):
commands.stand()
commands.setHeadTilt(-15)
def run(self):
commands.stand()
commands.setStiffness(ChangeStiff.OneLegSoft)
if self.getTime() > 3.0:
memory.speech.say("I am ready")
self.finish()
def run(self):
commands.stand()
commands.setHeadPanTilt(0.0, 0.0, 1.0)
print("ball x: %f, ball y: %f, goal x: %f, goal y: %f" %
(self.ball.loc.x, self.ball.loc.y, self.goal.loc.x,
self.goal.loc.y))
def run(self):
commands.stand()
if self.getTime() > 3.0:
memory.speech.say("Goalie!")
self.finish()
def run(self):
global walk_start_time, EnemyGoalStates, enemy_state, Modes, mode, states, current_state, Fields, field, rotation_dir, kick_sent, last_mode_change_time
#detect mode switches
hf = sensors.getValue(core.headFront)
hm = sensors.getValue(core.headMiddle)
hr = sensors.getValue(core.headRear)
# print "Head: " + str(hf) + ", " + str(hm) + ", " + str(hr)
if (mode is not Modes.passive and hf and hm
and hr): #need to switch of passive mode
self.stop()
memory.speech.say("Passive Mode!")
mode = Modes.passive
last_mode_change_time = self.getTime()
if (mode is not Modes.attacking and hf and hm and not hr
and (self.getTime() - last_mode_change_time) >
1.0): #need to switch to attack mode
memory.speech.say("Attack Mode!")
mode = Modes.attacking
enemy_state = EnemyGoalStates.unknown
kick_sent = False
r_goal_dist_filtered = 0.
current_state = AttackingStates.start
last_mode_change_time = self.getTime()
if (mode is not Modes.defending and hm and hr and not hf
and (self.getTime() - last_mode_change_time) >
1.0): #need to switch to defense mode
memory.speech.say("Defense Mode!")
commands.stand()
pose_sent = False
num_times_sent = 0
pose_start_time = 0
mode = Modes.defending
current_state = DefendingStates.walk
walk_start_time = self.getTime()
last_mode_change_time = self.getTime()
#detect kidnapping
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)
# print "tilt " + str((lfl+lrl)/2.-(lfr+lrr)/2.)
if (numpy.abs(max_force) < 0.10 and mode is not Modes.passive):
#print("***************************************************")
#print("***************************************************")
#print("***************************************************")
#print("numpy.abs(max_force) = ") + str(numpy.abs(max_force))
memory.speech.say("Put me down!")
self.stop()
mode = Modes.passive
#execute the appropriate function
if (mode is Modes.passive):
#print "passive"
self.track_ball()
self.stop()
return
else:
function_map = {}
if (mode is Modes.attacking):
#print "Attack mode"
function_map = {
AttackingStates.start: self.attack_start,
AttackingStates.approach: self.attack_approach,
AttackingStates.rotate: self.attack_rotate,
AttackingStates.dribble: self.attack_dribble,
AttackingStates.align: self.attack_align,
AttackingStates.kick: self.attack_kick
}
else:
#print "Defense mode"
function_map = {
DefendingStates.start: self.defense_start,
DefendingStates.walk_center: self.defense_walk_center,
DefendingStates.walk_left: self.defense_walk_left,
DefendingStates.walk_right: self.defense_walk_right,
DefendingStates.walk: self.defense_walk,
DefendingStates.block: self.defense_block,
DefendingStates.block_left: self.defense_block_left,
DefendingStates.block_right: self.defense_block_right,
DefendingStates.sit: self.defense_sit
}
#print "Current state: " + str(current_state)
function_map[current_state]()
def run(self):
commands.stand()
if self.getTime() > 20.0:
memory.speech.say("playing stand complete")
self.finish()
def run(self):
commands.stand()
if self.getTime() > 5.0:
memory.speech.say("Starting localization")
self.finish()
def run(self):
commands.stand()
if self.getTime() > 5.0:
print "standing up"
self.finish()
def run(self):
commands.stand()
if self.getTime() > 5.0:
self.finish()
def run(self):
if self.getTime() > self.delay:
self.started = True
# commands.setStiffness()
commands.stand()
ball = mem_objects.world_objects[core.WO_BALL]
selfRobot = mem_objects.world_objects[core.WO_TEAM5]
relBall = ball.loc.globalToRelative(selfRobot.loc, selfRobot.orientation)
if ball.seen:
# commands.setHeadPan(ball.bearing, 0.1)
self.lastSeenCt = max(0,self.lastSeenCt - 1)
else:
self.lastSeenCt += 1
# if self.lastSeenCt > 120:
commands.setHeadPan(0,0.1)
eta = float('inf')
# print "Ball {6}: {0}, {1} Velocity: {2}, {3} Vision: {4} {5}".format(relBall.x, relBall.y, ball.absVel.x, ball.absVel.y, ball.visionDistance, ball.visionBearing*core.RAD_T_DEG, ball.seen)
if ball.absVel.x < 0:
eta = -1.0 * relBall.x / ball.relVel.x
#if abs(ball.loc.x / ball.relVel.x) < 3.0 and ball.relVel.x < 0: # Ball will reach us in 3 seconds
if eta < 10 and relBall.x < 1500 and eta > 3 and (self.getTime() - self.time0 <= 5.0):
# intercept = ball.loc.y + (ball.absVel.y * eta)
intercept = relBall.y + ball.absVel.y * eta
print eta
print ball.loc
print relBall
print ball.absVel
# print ball.relVel
print intercept
if intercept < 500 and intercept > -500:
self.blockCt += 1
if self.blockCt > 0:
UTdebug.log(15, "Ball is close, blocking!")
if intercept > 120:
choice = "left"
elif intercept < 0:
choice = "right"
else:
choice = "center"
self.postSignal(choice)
else:
self.blockCt = max(self.blockCt - 1, 0)
elif self.getTime() - self.time0 <= 5.0 and (self.getTime() > self.delay or self.started):
target_pos = self.get_target_position(ball)
self.move_to_position(*target_pos)
if relBall.x < 1000.0:
self.time0 = self.getTime()
if self.getTime() - self.time0 > 5.0:
commands.setHeadPan(50 * core.DEG_T_RAD,0.2)
if self.getTime() - self.time0 > 6.5:
self.time0 = self.getTime()
elif ball.seen:
commands.setHeadPan(ball.visionBearing,0.2)
else:
commands.setHeadPan(self.direction * 30 * core.DEG_T_RAD,0.5)
if core.joint_values[core.HeadYaw] > 25 * core.DEG_T_RAD:
self.direction = -1
elif core.joint_values[core.HeadYaw] < -25 * core.DEG_T_RAD:
self.direction = 1
def run(self):
commands.stand()
if self.getTime() > 4.0:
self.finish()
def run(self):
commands.stand()
