def tick(self): if self.bb.parameters.attackRight: print 'f**k right' self.bb.reset_particle_point( VecPos(-self.bb.parameters.GoaliePointX, 0)) else: print 'f**k left' self.bb.reset_particle_point( VecPos(self.bb.parameters.GoaliePointX, 0)) self.crouch() if self.bb.see_ball: self.gazeBall() if self.bb.attackRight: if self.bb.ball_global.x < -450 + ATTACK_DIATANCE_X and abs( self.bb.ball_global.y) < 200: print 'GoalieScanField Succcess' return self.success() else: return self.running() else: if self.bb.ball_global.x > 450 - ATTACK_DIATANCE_X and abs( self.bb.ball_global.y) < 200: print 'GoalieScanField Succcess' return self.success() else: return self.running() else: if self.timer.elapsed() > 1: self.timer.restart() self.next_plat() self.lookAt(self.curPlat.x, self.curPlat.y, 10, 10) # print('LookAround running') return self.running()
def init(self): self.attackRight = True self.angleDiff = 0 if self.attackRight: self.goaliePos = VecPos(-440, 0, 0) else: self.goaliePos = VecPos(440, 0, 180) pass
def calc_defend_pos(self): ball = self.bb.ball_global if attackRight(): dest = VecPos(-330, 0, 0) else: dest = VecPos(330, 0, 180) return dest
def init(self): self.path = [VecPos(15, -100), # VecPos(20, -50), VecPos(15, 0), # VecPos(20, 50), VecPos(15, 100), # VecPos(40, 0) ] self.keep = False self.timer = Timer(2) self.iter = iter(self.path) self.curplat = self.iter.next()
def calc_doge_point(rub, robot_pos, ball_global, target, final_dest, side): doge = side is 'up' and DOGE_POINT_UP or DOGE_POINT_DOWN if -30 < rub.x < 0 and abs(rub.y) < 20: doge = side is 'up' and VecPos(0, 20) or VecPos(0, -20) theta = angle_between(ball_global, target) dogex = ball_global.x + doge.y * sin(theta) - doge.x * cos(theta) dogey = ball_global.y - doge.y * cos(theta) + doge.x * sin(theta) d = VecPos(dogex, dogey) angle = degree_between(d, final_dest) return VecPos(dogex, dogey, angle)
def tick(self): self.lookAt(45, 0) ball_field = self.bb.ball_field final_dest, _, rub = calc_attack_point() if self.got_dest(final_dest): self.step() return self.success() else: if rub.y > 0: direction = CLOCK_WISE else: direction = ANTI_CLOCK_WISE r = SAFE_DIST theta = ANGLE_PER_TICK * direction beta = angle_normalization( 180.0 - degrees(atan2(ball_field.y, ball_field.x))) alpha = radians(theta - beta) r_vec = VecPos(r * cos(alpha), r * sin(alpha)) des = ball_field + r_vec des.z = angle_normalization(alpha + 180) x, y, t = self.walksolver.solveField(des) t *= 0.5 self.walk(x, y, t) return self.running()
def push(self): """ Push forward waiting for a pass :return: """ y = self.bb.robot_pos.y if self.bb.parameters.attackRight: dest = VecPos(200, y, 0) else: dest = VecPos(-200, y, 180) if not self.got_dest_loose(dest): self.gotoGlobalOmni(dest) self.ready = False else: self.ready = True
def init(self): self.attackRight = True self.keepPoint = VecPos(0, 0, 0) self.angleDiff = 0 # self.maxVelocity = 0 self.attackPointY = 0 pass
def calc_final_dest(enable_kick, ball_field, ball_global, target): theta = angle_between(target, ball_global) closer_to_left_foot = ball_field.y > 0 # FIXME(MWX): may blur if not enable_kick or (not cfg.left_kick and not cfg.right_kick): kick_point = closer_to_left_foot and cfg.left_kick_point or cfg.right_kick_point else: kick_point = cfg.left_kick and cfg.left_kick_point or cfg.right_kick_point res = VecPos() res.x = ball_global.x + kick_point.x * cos(theta) res.y = ball_global.y + kick_point.x * sin(theta) theta2 = angle_between(target, res) res.z = degrees(theta2 + PI) return res
def init(self): self.last_state = None self.initial = Initial() self.ready = Ready() self.set = Set() self.playing = Playing() self.finished = Finished() self.cur_state = None self.freeKickTimer = Timer(10) self.lastFreeKickBallPos = VecPos() self.enemy_freekick = False self.trackBall = TrackBall()
def tick(self): x = self.bb.robot_pos.x y = self.bb.robot_pos.y magicy = 30 if self.bb.parameters.attackRight: # set target if y > 0: target = VecPos(550, magicy) else: target = VecPos(550, -magicy) if fabs(y) > 130: target.x -= 100 # set enable kick if x > 450 - KICK_ABAILITY and x < 450 - 100: self.bb.enable_kick = True else: self.bb.enable_kick = False else: if y > 0: target = VecPos(-550, magicy) else: target = VecPos(-550, -magicy) if fabs(y) > 130: target.x += 100 if x < -(450 - KICK_ABAILITY) and x > -(450 - 100): self.bb.enable_kick = True else: self.bb.enable_kick = False self.bb.attack_target = target return self.running()
def calc_block_point(self): ball_pos = self.bb.ball_global if self.bb.parameters.attackRight: goal = VecPos(-550, 0) else: goal = VecPos(550, 0) # goal.x *= -1 theta = degree_to_radian(degree_between(goal, ball_pos)) block_point = VecPos() block_point.x = ball_pos.x - AVOID_DIS * 1.5 * cos(theta) block_point.y = ball_pos.y - AVOID_DIS * 1.5 * sin(theta) block_point.z = degrees(theta) return block_point
from math import degrees, cos, sin from DecisionMaking.BehaviorTree.Branch import parallel from DecisionMaking.BehaviorTree.Decorator import seeBall from DecisionMaking.BehaviorTree.Task import Action from headskills.TrackBall import TrackBall from utils.CalcAttackPoint import calc_attack_point from utils.VecPos import VecPos from utils.mathutil import angle_between, PI_2, PI, get_dis, abs_angle_diff, get_magnitude, degree_to_radian, degree_between from roles.Striker import _Striker # Avoid AVOID_DIS = 50 AVOID_POINT_DOWN = VecPos(0, -AVOID_DIS * 1.5) AVOID_POINT_UP = VecPos(0, AVOID_DIS * 1.5) _striker = _Striker() @seeBall class _BlockBall(Action): def tick(self): destination = self.calc_block_point() if self.bb.parameters.attackRight and self.bb.ball_global.x > AVOID_DIS: return self.success() elif not self.bb.parameters.attackRight and self.bb.ball_global.x < -AVOID_DIS: return self.success() if not self.got_dest(destination): # print 'avoid ball go to dest' dis = get_dis(self.bb.ball_global, self.bb.robot_pos) if dis < AVOID_DIS:
from utils.VecPos import VecPos from DecisionMaking.BehaviorTree.Task import Action from DecisionMaking.BehaviorTree.Branch import parallel from headskills.TrackBall import TrackBall from utils.mathutil import * attackRight = True GoalPoint_Back = VecPos(-440, 0, 90) GoalPoint_Front = VecPos(440, 0, -90) class GoToGoalieFPoint(Action): def init(self): pass def tick(self): if attackRight: if not self.got_dest_tight(GoalPoint_Back): self.gotoGlobal(GoalPoint_Back) return self.running() else: return self.success() else: if not got_dest_tight(GoalPoint_Front): self.gotoGlobal(GoalPoint_Front) return self.running() else: return self.success()
def tick(self): if self.bb.see_ball: self.lostBallTimer.restart() if self.lostBallTimer.elapsed() > 15: self.gotoGlobal(self.bb.get_default_dest()) if self.got_dest_loose(self.bb.get_default_dest()): self.lostBallTimer.restart() elif self.bb.closest_to_ball() or self.bb.team_lost_ball(): self.ready = False self.strike.tick() else: """ Face ball if see ball, else seek ball Approach close to ball Keep 1m away, don't block attack path """ if self.bb.see_ball: bally = self.bb.ball_global.y ballx = self.bb.ball_global.x rx = self.bb.robot_pos.x ry = self.bb.robot_pos.y dx = rx - ballx dy = ry - bally dist = sqrt(dx * dx + dy * dy) # for attack right case change = False if attackRight(): if ballx > rx or abs(bally - ry) > 150 or dist > 200 or dist < 50: if ry > bally: y = bally + 100 else: y = bally - 100 angle = degree_between(self.bb.robot_pos, self.bb.ball_global) if not self.gotoGlobal(VecPos(ballx + 100, y, angle)): change = True else: if ballx < rx or abs(bally - ry) > 150 or dict > 200 or dist < 50: if ry > bally: y = bally + 100 else: y = bally - 100 angle = degree_between(self.bb.robot_pos, self.bb.ball_global) if not self.gotoGlobal(VecPos(ballx + 100, y, angle)): change = True trackBall.tick() if not change: self.faceBall() else: # to do, max turn cnt, else go back self.seekball.tick() return self.running()
from utils.VecPos import VecPos from DecisionMaking.BehaviorTree.Task import Action from DecisionMaking.BehaviorTree.Branch import parallel from headskills.TrackBall import TrackBall from utils.mathutil import * GazePlats = [VecPos(0, 0), VecPos(0, 60), VecPos(0, 0), VecPos(0, -60)] Penalty_Back = VecPos(-240, 0, 0) Penalty_Front = VecPos(240, 0, -180) class WalkAroundPenalty(Action): def init(self): self.currentTarget = 0 self.penalty_1_arrived = False self.penalty_2_arrived = False self.iter = iter(GazePlats) def tick(self): # self.currentTarget += 1 # if self.currentTarget == 4: # self.currentTarget = 0 if self.bb.visionInfo.see_ball: # self.gazeBall() return self.success() else: if not self.penalty_1_arrived: # cur_x = Penalty_Back.x # cur_y = Penalty_Back.y # cur_t = 0
def tick(self): #print 'GCPlay' self.trackBall.tick() gc_info = self.bb.GCInfo if gc_info is None or not gc_info.connected: self.playing.tick() else: if gc_info.ourIndirectFreeKick or gc_info.ourDirectFreeKick or gc_info.ourPenaltyKick: if gc_info.state2Freeze: self.trackBall.tick() self.crouch() elif gc_info.state2Ready: self.trackBall.tick() if self.bb.closest_to_ball(): ball = self.bb.ball_global if attackRight(): self.gotoGlobal(VecPos(ball.x - 80, ball.y, 0)) else: self.gotoGlobal(VecPos(ball.x + 80, ball.y, 0)) elif gc_info.enemyIndirectFreeKick or gc_info.enemyDirectFreeKick or gc_info.enemyPenaltyKick: self.enemy_freekick = True self.freeKickTimer.restart() self.lastFreeKickBallPos = self.bb.ball_global if gc_info.state2Freeze: self.crouch() elif gc_info.state2Ready: if self.bb.closest_to_ball(): ball = self.bb.ball_global if attackRight(): self.gotoGlobal(VecPos(ball.x - 50, ball.y, 0)) else: self.gotoGlobal(VecPos(ball.x + 50, ball.y, 0)) else: self.playing.tick() elif self.enemy_freekick: if self.bb.closest_to_ball(): if not self.freeKickTimer.finished(): # print self.freeKickTimer.elapsed() ball_now = self.bb.ball_global diff = (ball_now - self.lastFreeKickBallPos).length() # print diff if diff < 20: self.crouch() self.gazeBall() else: self.playing.tick() else: self.enemy_freekick = False else: self.enemy_freekick = False else: if gc_info.state is GCInfo.INITIAL: self.cur_state = self.initial elif gc_info.state is GCInfo.READY: self.cur_state = self.ready elif gc_info.state is GCInfo.SET: self.cur_state = self.set elif gc_info.state is GCInfo.PLAYING: self.cur_state = self.playing elif gc_info.state is GCInfo.FINISHED: self.cur_state = self.finished else: self.cur_state = self.playing if gc_info.state != self.last_state: if self.cur_state: self.cur_state.fresh() if self.cur_state: self.cur_state.tick() self.last_state = gc_info.state
from utils.VecPos import VecPos StrikerLeftPosKickoff = VecPos(-50, 0, 0) StrikerLeftPos = VecPos(-150, 75, 0) DefenderLeftPos = VecPos(-300, 0, 0) MidfielderLeftPos = VecPos(-150, 200, 0) GoalieLeftPos = VecPos(-400, 0, 0) StrikerRightPos = StrikerLeftPos.mirror() DefenderRightPos = DefenderLeftPos.mirror() MidfielderRightPos = MidfielderLeftPos.mirror() GoalieRightPos = GoalieLeftPos.mirror()
from DecisionMaking.BehaviorTree.Task import Action from utils.mathutil import get_angle, radian_to_degree from Timer import Timer from utils.VecPos import VecPos GazePlats = [VecPos(45, 0), VecPos(0, 0)] class FindBallAroundOnce(Action): def init(self): self.cycle = 0 self.turned = 0 self.turnCnt = 0 self.iter = iter(GazePlats) self.curPlat = self.iter.next() self.timer = Timer() self.fieldAnglePre = self.bb.field_angle # print 'init' # print self.bb.field_angle def reset_(self): self.turned = 0 self.cycle = 0 def tick(self): self.cycle += 1 # print 'f**k ao', self.cycle, self.turnCnt if self.bb.visionInfo.see_ball: self.gazeBall() return self.success()
from DecisionMaking.BehaviorTree.Task import Action from Timer import Timer from utils.VecPos import VecPos GazePlats = [VecPos(0, 0), VecPos(0, 40), VecPos(0, 0), VecPos(0, -40)] ATTACK_DIATANCE_X = 200 ATTACK_DIATANCE_Y = 165 class GoalieScanField(Action): """ Head skill: Find ball, when robot is not turning, scan field, else look down """ def init(self): self.timer = Timer() self.iter = iter(GazePlats) self.curPlat = self.iter.next() def tick(self): if self.bb.parameters.attackRight: print 'f**k right' self.bb.reset_particle_point( VecPos(-self.bb.parameters.GoaliePointX, 0)) else: print 'f**k left' self.bb.reset_particle_point( VecPos(self.bb.parameters.GoaliePointX, 0)) self.crouch() if self.bb.see_ball: self.gazeBall()
from DecisionMaking.BehaviorTree.Task import Action from Timer import Timer from utils.VecPos import VecPos GazePlats = [ VecPos(40, 0), VecPos(0, 0) ] class FindUpDown(Action): def init(self): self.timer = Timer() self.iter = iter(GazePlats) self.curPlat = self.iter.next() def tick(self): if self.bb.visionInfo.see_ball: self.gazeBall() return self.success() else: if self.timer.elapsed() > 1: self.timer.restart() if not self.next_plat(): print "failure" return self.failure() self.lookAt(self.curPlat.x, self.curPlat.y, 10, 10) print "lookAt ({}, {})".format(self.curPlat.x, self.curPlat.y) print "running" return self.running()
from utils.VecPos import VecPos from utils.mathutil import * from Blackboard import getbb # config magic = 40 DOGE_POINT = VecPos(magic, 0) DOGE_POINT_UP = VecPos(magic, magic) DOGE_POINT_DOWN = VecPos(magic, -magic) DOGE_ANGLE = degree_between(DOGE_POINT, DOGE_POINT_UP) rub = None final_dest = None dest = None bb = getbb() cfg = bb.parameters def get_attack_result(): global final_dest, dest, rub return final_dest, dest, rub def get_rub(): global rub if not rub: raise Exception('rub not initialised, call calc_attack_point first') return rub def calc_attack_point():
from utils.VecPos import VecPos from DecisionMaking.BehaviorTree.Task import Action from DecisionMaking.BehaviorTree.Branch import parallel from headskills.TrackBall import TrackBall from utils.mathutil import * GoalPoint_Left = VecPos(-420, 0, 0) GoalPoint_Right = VecPos(420, 0, 180) class GoToGoaliePoint(Action): def init(self): pass def tick(self): if self.bb.attackRight: print 'Go to goalie point tick 11' if not self.got_dest(GoalPoint_Left): self.gotoGlobal(GoalPoint_Left) return self.running() else: return self.success() else: print 'Go to goalie point tick 22' if not self.got_dest(GoalPoint_Right): self.gotoGlobal(GoalPoint_Right) return self.running() else: return self.success()
from DecisionMaking.BehaviorTree.Task import Action from Timer import Timer from utils.VecPos import VecPos GazePlats = [VecPos(0, 0), VecPos(0, 60), VecPos(0, 0), VecPos(0, -60)] class LookAround(Action): """ Head skill: Find ball, when robot is not turning, scan field, else look down """ def init(self): self.timer = Timer() self.iter = iter(GazePlats) self.curPlat = self.iter.next() def tick(self): # print("*********LookAround tick") if self.gazeBall(): # if ball is seen, then reinit FindBall self.iter = iter(GazePlats) self.curPlat = self.iter.next() # print('LookAround success') return self.success() else: if self.timer.elapsed() > 1: self.timer.restart() self.next_plat() self.lookAt(self.curPlat.x, self.curPlat.y, 10, 10) # print('LookAround running')
def __init__(self): self.robotId = rospy.get_param('~RobotId') # self.robotId = 1 self.skill = rospy.get_param('~skill') self.motionInitTime = rospy.get_param( '/dmotion_{}/dmotion/hardware/imu_prepare_time'.format( self.robotId)) self.attackRight = rospy.get_param('/ZJUDancer/AttackRight') roleid = rospy.get_param('~dbehavior/role') if roleid == "Striker": self.defaultRole = BehaviorInfo.Striker elif roleid == "Defender": self.defaultRole = BehaviorInfo.Defender elif roleid == "Mid": self.defaultRole = BehaviorInfo.MidFielder else: self.defaultRole = BehaviorInfo.Goalie self.enableLog = False # Constants self.maxPitch = 45 self.minPitch = 0 self.maxYaw = 120 self.walk_speed = 450 / 26 self.turn_speed = 10 self.GoaliePointX = 420 # kick if self.robotId is 1: self.left_kick = True self.right_kick = True self.left_kick_point = VecPos(15, 4) self.right_kick_point = VecPos(15, -4) elif self.robotId is 2: self.left_kick = True self.right_kick = True self.left_kick_point = VecPos(15, 4) self.right_kick_point = VecPos(15, -4) elif self.robotId is 3: self.left_kick = True self.right_kick = True self.left_kick_point = VecPos(15, 4) self.right_kick_point = VecPos(15, -4) elif self.robotId is 4: self.left_kick = True self.right_kick = True self.left_kick_point = VecPos(15, 4) self.right_kick_point = VecPos(15, -4) elif self.robotId is 5: self.left_kick = True self.right_kick = True self.left_kick_point = VecPos(15, 4) self.right_kick_point = VecPos(15, -4) elif self.robotId is 6: self.left_kick = True self.right_kick = True self.left_kick_point = VecPos(15, 4) self.right_kick_point = VecPos(15, -4) else: self.left_kick = True self.right_kick = True self.left_kick_point = VecPos(15, 4) self.right_kick_point = VecPos(15, -4)
from utils.VecPos import VecPos from DecisionMaking.BehaviorTree.Task import Action from DecisionMaking.BehaviorTree.Branch import parallel from headskills.TrackBall import TrackBall from utils.mathutil import * DEST_REGION = 15 DEST_RE_ANGLE = 15 x = 450 * .8 y = 300 * .8 goals = [ VecPos(x, y, 0), VecPos(x, -y, -90), VecPos(-x, -y, -180), VecPos(-x, y, 90) ] class _WalkAround(Action): def init(self): self.currentTarget = 0 def tick(self): if self.got_dest(goals[self.currentTarget]): self.currentTarget += 1 if self.currentTarget == 4: self.currentTarget = 0 self.gotoGlobal(goals[self.currentTarget])
def fast(self, localDest): sx_star, sy_star, st_star = 0, 0, 0 mirror = False dest = localDest.copy() if dest.y < 0: dest.y = -dest.y dest.z = -dest.z mirror = True xInput = self.vx thetaP = dest.slope() thetaAttack = dest.z thetaSafe = min(thetaP, THETA_SAFE) thetaAttackP = thetaAttack - thetaP aDest = dest.copy() aDest.rotate(90) if thetaAttackP > thetaP: thetaMax = thetaP elif thetaAttackP > thetaSafe: thetaMax = thetaAttackP else: thetaMax = thetaSafe thetaMax = min(thetaMax, thetaSafe) thetaMax = min(thetaMax, thetaP) dt = 1 stepnumMin = 999999 i = 0 if abs(thetaMax) < TOP_THETA_MAX: sx_star, sy_star, st_star = TOP_X_MAX, 0, thetaMax else: cross_i = VecPos() while i < thetaMax: a_i = aDest.copy() a_i.rotate(i) b_i = a_i.dot(dest) r_i = b_i / (a_i.y - a_i.length() + epso) sx, sy, st = self.getx_max(r_i) st = min(thetaP, st) c_i = i + thetaP cross_i.x = r_i * sind(c_i) cross_i.y = r_i * (1 - cosd(c_i)) line_i = (dest - cross_i).copy() k1 = 15.0 k2 = 10.0 stepnum_arc = c_i / (st == 0 and epso or st) stepnum_line = line_i.length() / TOP_X_MAX if thetaP < AD_THETA_MAX + 20: k2 = 25.0 stepnum_speed = abs(max(-sx + xInput, 0)) / TOP_X_MAX * k1 * thetaP * pi / 180 + \ abs(TOP_X_MAX - sx) * k2 / TOP_X_MAX stepnum = stepnum_arc + stepnum_line + stepnum_speed if stepnumMin > stepnum: stepnumMin = stepnum sx_star, sy_star, st_star = sx, sy, st i += dt if mirror: st_star = -st_star # sy_star = -sy_star # print 'fast', sx_star, sy_star, st_star, localDest.x, localDest.y, localDest.z return sx_star, sy_star, st_star
# | | # | | # | | # | | # | | # | | # | | # maxKeepDown L-----> | | # | | # | | # |------------- # | # | # |--------------------------------------------------- keepPointLeft = VecPos(-600, 0, 0) keepPointRight = VecPos(600, 0, 180) keepLineLeft = VecPos(-440, 0, 0) keepLineRight = VecPos(440, 0, 180) scramblePointX = 5 ballMinMoveSpeed = 5 adjustDist = 300 # the ball distance to adjust self position adjustBallX = 20 # when ball's y is larger than adjustBallY, robot will adjust its position dangerDist = 60 # the ball distance to take action by ball velocity scrambleDist = 60 # the ball distance to decide which side to scramble maxMoveUp = 80 maxMoveDown = -80 dangerVelocity = 30
from DecisionMaking.BehaviorTree.Task import Action from Timer import Timer from utils.VecPos import VecPos from utils.mathutil import get_magnitude GazePlats = [ VecPos(15, -100), VecPos(15, 0), VecPos(15, 100) ] class FindBall(Action): """ Head skill: Find ball, when robot is not turning, scan field, else look down """ def init(self): self.timer = Timer(1) self.iter = iter(GazePlats) self.curPlat = self.iter.next() def tick(self): if not self.bb.visionInfo.see_ball and self.bb.ball_lost.elapsed() < 10 and get_magnitude(self.bb.last_seen_ball_field) < 50: return self.failure() if self.gazeBall(): self.iter = iter(GazePlats) self.curPlat = self.iter.next() return self.success() else: