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 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