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:
