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.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): 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): 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): 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): 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()