def collect_turn_data(self, cnt=100, clockwise=None, dT=None, pwr=None):
data = []
random.seed()
for i in range(cnt):
_clockwise = random.choice(
(True, False)) if clockwise is None else clockwise
_pwr = random.randint(1, 100) if pwr is None else pwr
_dT = random.uniform(0.1, 3) if dT is None else dT
h = self.heading()
self.dbprint('Test pwr:%d, dT:%.1f, cw: %s' %
(_pwr, _dT, _clockwise))
self.cmd_mboth(_pwr, _clockwise, _pwr, not _clockwise)
time.sleep(_dT)
self.cmd_mstop()
self.wait_until_stop()
h1 = self.heading()
adiff = abs(angle_diff(h, self.heading()))
self.dbprint('Result:%.2f' % adiff)
data.append({
'output': {
'adiff': adiff
},
'input': {
'pwr': _pwr,
'dT': _dT,
'clockwise': _clockwise
}
})
return data
def search_around(self,
stop_if_cb,
clockwise=True,
pwr=30,
step_angle=10,
max_pwr=100):
min_pwr = pwr
ih = self.heading()
last_diff = 0
growing = True
for step in range(10000):
change = self.tick_turn(clockwise, pwr=pwr,
min_angle=step_angle)['change']
if abs(change) < 1:
pwr += 5
if pwr > max_pwr - 10:
pwr = max_pwr - 10
else:
pwr = min_pwr
self.wait_until_stop()
h = self.heading()
adiff = abs(angle_diff(ih, h))
if growing:
if adiff < last_diff - 5:
growing = False
else:
if adiff > last_diff + 5:
break
last_diff = adiff
if stop_if_cb(self):
break
def get_control_cmd(current_tf, goal_tf):
gx, gy, gtheta = goal_tf
cx, cy, ctheta = current_tf
if gtheta is None:
gtheta = np.arctan2(gy - cy, gx - cx)
dtheta = angle_diff(gtheta, ctheta) # facing towards the next point
dx = gx - cx
dy = gy - cy
r = -ctheta
ddx = dx * np.cos(r) - dy * np.sin(r)
ddy = dy * np.cos(r) + dx * np.sin(r)
return ddx, ddy, dtheta
def auto_attacker2(self, team, role, opp, ball, side=0, tic=0):
"""Strategy for an attacker working with DQN."""
angle_to_ball = angle_to(team[role].pos, ball.pos)
angle_to_goal = angle_to(team[role].pos, goals[not side])
angle_ball_to_goal = angle_to(team[role].pos, goals[not side])
dist_ball_to_goal = dist(ball.pos, goals[not side])
dist_to_goal = dist(team[role].pos, goals[not side])
dist_to_ball = dist(team[role].pos, ball.pos)
print "angle_to_ball", angle_to_ball
print "angle_to_goal", angle_to_goal
print "angle_ball_to_goal", angle_ball_to_goal
print "dist_ball_to_goal", dist_ball_to_goal
print "dist_to_goal", dist_to_goal
print "dist_to_ball", dist_to_ball
# Decision tree
target = None
if abs(angle_diff(angle_to_goal, angle_ball_to_goal)) < 1 and \
dist_ball_to_goal < dist_to_goal:
# run to score!
target = ball.pos
print "GO KICK"
else:
if team[role].pos[0] < ball.pos[0]:
# go behind ball, aligned with goal
vec = vector_to(goals[not side], ball.pos)
vec = normalize_vector(vec, 0.7)
target = tonp(ball.pos) + vec
print "PREPARE TO KICK"
else:
# go behind goal
target = list(ball.pos)
if angle_to_ball > 0:
target[1] -= 0.5
else:
target[0] += 0.5
target[0] += 0.1
print "GO BEHIND BALL"
print team[role].pos, ball.pos, target
team[role].move_to(target)
def valid_route(self, route, targets=None):
if targets is None:
mks = self.course.marks
else:
mks = []
for target in targets:
for mk in self.course.marks:
if mk.target == target:
mks.append(mk)
ans = [0 for mk in mks]
for l in route.segments:
for i, mk in enumerate(mks):
v1 = mk - l.p1
v2 = mk - l.p2
ans[i] += angle_diff(v2.a, v1.a)
for a, mk in zip(ans, mks):
if mk.to_port and a >= 0:
return False
if not mk.to_port and a <= 0:
return False
return True
def valid_route(self, route, targets=None):
if targets is None:
mks = self.course.marks
else:
mks = []
for target in targets:
for mk in self.course.marks:
if mk.target == target:
mks.append(mk)
ans = [0 for mk in mks]
for l in route.segments:
for i, mk in enumerate(mks):
v1 = mk - l.p1
v2 = mk - l.p2
ans[i] += angle_diff(v2.a, v1.a)
for a, mk in zip(ans, mks):
if mk.to_port and a >= 0:
return False
if not mk.to_port and a <= 0:
return False
return True
def tick_turn_old(self, clockwise, min_angle=None, pwr=50):
rs = 0
init_t = time.time()
init_h = self.heading()
self.dbprint('start h: %d, pwr: %s' % (init_h, pwr))
try:
self.cmd_mboth(pwr, clockwise, pwr, not clockwise)
for i in range(int(self.WHOLE_TURN_TIME / self.STEP_TIME)):
h = self.heading()
x, y, z = self.degsec()
if abs(z) > 5:
pwr *= 0.1
#self.dbprint('moving...%g PWR:%g' % (z, pwr))
self.cmd_mboth(pwr, clockwise, pwr, not clockwise)
if min_angle is None:
x, y, z = self.degsec()
self.dbprint('g:%d %d %d, h:%d' % (x, y, z, h))
if abs(z) > 15:
self.dbprint('it moves')
break
else:
hdiff = angle_diff(h, init_h)
#self.dbprint('h=%g, hdiff=%g' % (h, hdiff))
if abs(hdiff) > min_angle:
self.dbprint('%g degrees trigger stop' % hdiff)
break
# self.update_state()
time.sleep(self.STEP_TIME)
finally:
self.cmd_mstop()
self.wait_until_stop()
h = self.heading()
self.add_memory(abs(init_h - h), pwr, pwr, time.time() - init_t)
rs = init_h - h
self.dbprint('last h: %d, change: %g' % (h, rs))
return rs
def update_angle(old_angle, diff):
new_angle = old_angle + diff
while new_angle > np.pi or new_angle < -np.pi:
new_angle = angle_diff(np.array([[new_angle, 0]])).ravel()[0]
return new_angle
def move_straight(self,
fwd=True,
max_steps=5,
max_secs=1,
fix_heading=True,
heading=None,
power=50):
self.cmd_reset_counters()
counted_offset = 0
if heading is None:
heading = self.heading()
elif abs(angle_diff(heading, self.heading())) > 5:
self.turn_in_ticks(heading)
offset = 0
try:
t = time.time()
while True:
if (t + max_secs) <= time.time():
self.dbprint('%d secs is out' % max_secs)
break
self.update_state()
hdiff = angle_diff(self.heading(), heading)
if abs(hdiff) > 20 and fix_heading:
self.dbprint("STOP and CORRECT")
tt = time.time()
counted = self.steps_counted()
self.turn_in_ticks(heading, err=5)
counted_offset = counted - self.steps_counted()
self.dbprint("COUNTER OFFSET=%d" % counted_offset)
hdiff = angle_diff(self.heading(), heading)
max_secs += time.time() - tt
if hdiff > 1:
offset = 20 if fwd else -20
elif hdiff < -1:
offset = -20 if fwd else 20
else:
offset = 0
# to avoid pwr < 0
lpwr = max(0, power - offset)
rpwr = max(0, power + offset)
self.dbprint('^%d hdiff:%g off:%d pw:%d<>%d cnt:%d<>%d' %
(int(self.heading()), hdiff, offset, lpwr, rpwr,
self.state['lcount'], self.state['rcount']))
if fwd and self.state['sonar'] >= 0 and self.state[
'sonar'] < self.MIN_DISTANCE:
self.hit_warn = self.state['sonar']
self.dbprint('STOP distance=%s' % self.state['sonar'])
break
steps = self.steps_counted() + counted_offset
if steps > max_steps:
self.dbprint('Max steps reached (%d > %d)' %
(steps, max_steps))
hdiff = angle_diff(self.heading(), heading)
if abs(hdiff) > 3:
self.dbprint("FINAL CORRECTION")
self.turn_in_ticks(heading, err=5)
break
self.cmd_mboth(lpwr, fwd, rpwr, fwd)
time.sleep(self.STEP_TIME)
self.update_state()
finally:
self.cmd_mstop()
def tick_turn(self,
clockwise,
min_angle=None,
pwr=50,
mleft=True,
mright=True):
rs = 0
max_pwr = pwr
in_t = time.time()
in_h = self.heading()
if not min_angle is None:
azim = (in_h + min_angle) if clockwise else (in_h - min_angle)
in_x, in_y, in_z = self.degsec()
self.dbprint('start h: %.2f, z: %.2f, pwr: %s' % (in_h, in_z, pwr))
moved = False
try:
while not moved:
if mleft and mright:
self.cmd_mboth(pwr, clockwise, pwr, not clockwise)
elif mleft:
self.cmd_mleft(pwr, clockwise)
elif mright:
self.cmd_mright(pwr, not clockwise)
for i in range(int(self.WHOLE_TURN_TIME / self.STEP_TIME)):
# time.sleep(self.STEP_TIME)
dt = time.time() - in_t
h = self.heading()
x, y, z = self.degsec()
# self.dbprint('dh: %.2f, dz: %.2f, dt:%.2f' % (h - in_h, z - in_z, dt))
if min_angle is None:
self.dbprint('g:%d %d %d, h:%.2f' % (x, y, z, h))
if abs(z) >= 2:
self.dbprint('it moves')
moved = True
break
else:
# remaining angle
adiff = angle_diff(azim, h)
# passed angle
pdiff = angle_diff(h, in_h)
# f(dt, pwr, z) =
# remain distance depends on z, mass and friction
# f((z, dt) - friction?
# (z / dt) - acceleration = K * (pwr - friction)
# time to finish
# curr angle velocity (with offset)
if z - in_z != 0:
rt = abs(adiff / (z - in_z))
if rt < dt:
moved = True
# stop
self.dbprint('Moved FAST enough %.2f < %.2f' %
(rt, dt))
self.cmd_mstop()
self.wait_until_stop()
h = self.heading()
adiff = min_angle - abs(h - in_h)
break
if not moved:
pwr += 5
if pwr > 100:
self.dbprint('NOT MOVED and STUCK')
break
self.dbprint('NOT MOVED power now gets: %s' % (pwr, ))
finally:
self.cmd_mstop()
# update_state takes too long
self.update_state()
self.wait_until_stop()
self.update_state()
h = self.heading()
self.add_memory(abs(in_h - h), pwr, pwr, time.time() - in_t)
rs = in_h - h
self.dbprint('last h: %.2f, CHANGE: %g' % (h, rs))
return {'change': rs, 'pwr': pwr}
def move_straight(self,
fwd=True,
max_steps=5,
max_secs=1,
heading=None,
power=50,
**kwds):
self.cmd_reset_counters()
counted_offset = 0
pid = Pid(2., 0, 0)
pid.range(-power, power)
if heading is None:
heading = self.heading()
elif abs(angle_diff(heading, self.heading())) > 5:
self.turn_in_ticks(heading)
pid.set(heading)
offset = 0
try:
t = time.time()
while (t + max_secs) > time.time():
self.update_state()
hdiff = angle_diff(self.heading(), heading)
if abs(hdiff) > 20:
self.dbprint("STOP and CORRECT")
tt = time.time()
counted = self.steps_counted()
self.turn_in_ticks(heading, err=5)
counted_offset = counted - self.steps_counted()
self.dbprint("COUNTER OFFSET=%d" % counted_offset)
hdiff = angle_diff(self.heading(), heading)
max_secs += time.time() - tt
if hdiff > 1:
offset = 20 if fwd else -20
elif hdiff < -1:
offset = -20 if fwd else 20
else:
offset = 0
lpwr = power - offset
rpwr = power + offset
self.dbprint('^%d hdiff:%g off:%d pw:%d<>%d cnt:%d<>%d' %
(int(self.heading()), hdiff, offset, lpwr, rpwr,
self.state['lcount'], self.state['rcount']))
if fwd and self.dist_fwd() >= 0 and self.dist_fwd(
) < self.MIN_DISTANCE:
self.hit_warn = self.dist_fwd()
self.dbprint('STOP distance=%s' % self.dist_fwd())
break
steps = self.steps_counted() + counted_offset
if steps > max_steps:
self.dbprint('Max steps reached (%d > %d)' %
(steps, max_steps))
hdiff = angle_diff(self.heading(), heading)
if abs(hdiff) > 3:
self.dbprint("FINAL CORRECTION")
self.turn_in_ticks(heading, err=5)
break
self.cmd_mboth(lpwr, fwd, rpwr, fwd)
time.sleep(self.STEP_TIME)
self.update_state()
pid.step(input=self.heading())
offset = pid.get()
finally:
self.cmd_mstop()
def heading_diff(self, azim, err=10):
heading = self.heading()
diff = angle_diff(azim, heading)
return diff if abs(diff) > err else 0
