pavel/kick-it
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Good stable approach

Browse Source 
1. Refactored state machine
2. Goal search robuster by looking up
3. More accurate ball distance approximation
4. Eye of the tiger
5. Hasta la vista baby
This commit is contained in:
Pavel Lutskov
2018-07-01 13:38:30 +02:00
parent 9762b22c83
commit e54858fbfd
4 changed files with 132 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

164
pykick/__main__.py
View File

@@ -11,25 +11,23 @@ if __name__ == '__main__':
try: # Hit Ctrl-C to stop, cleanup and exit
cfg = read_config()
with InterruptDelayed():
with InterruptDelayed(): # Ignore Ctrl-C for a while
striker = Striker(
nao_ip=cfg['ip'], nao_port=cfg['port'],
res=cfg['res'], ball_hsv=cfg['ball'],
goal_hsv=cfg['goal'], field_hsv=cfg['field'],
ball_min_radius=cfg['ball_min_radius'],
)
striker.speak('tiger')
sleep(4.75)
striker.mover.stand_up(1.0)
sleep(9)
print('Initialized')
striker.mover.stand_up()
striker.speak('Initialized')
state = 'init'
# init_soll = 0.0
# align_start = 0.15
# curve_start = -0.1
# curve_stop = 0.1
# soll = init_soll
striker.speak("Initialized")
approach_steps = 0
loop_start = time()
while True:
loop_end = time()
print('Loop time:', loop_end - loop_start)
@@ -41,126 +39,96 @@ if __name__ == '__main__':
if state == 'init':
striker.mover.set_head_angles(0, 0)
striker.speak("Start the Ball tracking")
striker.ball_tracking(tol=0.05)
striker.speak(
"I have found the Ball, starting with. Goal search"
)
striker.mover.stand_up()
sleep(0.5)
bdist = striker.distance_to_ball()
striker.speak('Ball distance is %.2f' % bdist)
_, _, gcc = striker.goal_search()
print('Goal center', gcc, 'Ball dist', bdist)
if abs(gcc) < 0.4:
striker.speak('Direct approach')
if abs(gcc) < 0.4 or bdist <= 0.2:
print('Straight approach')
state = 'straight_approach'
approach = 0
continue
if gcc < 0:
striker.speak("I have found the. goal on the right")
approach = 1
elif 0.20 < bdist < 0.50:
print('Rdist is hypo')
state = 'rdist_is_hypo'
approach = 1 if gcc < 0 else - 1
else:
striker.speak("I have found the. goal on the left")
approach = -1
print('Bdist is hypo')
state = 'bdist_is_hypo'
approach = 1 if gcc < 0 else - 1
if approach == 1:
striker.speak('Goal on the right')
elif approach == -1:
striker.speak('Goal on the left')
else:
striker.speak('Direct approach')
striker.mover.set_head_angles(0, 0)
#approach = 1 if goal_center < 0 else -1
#approach = 1
sleep(0.5)
state = 'ball_approach'
elif state == 'tracking':
# start ball approach when ball is visible
print('Soll angle')
striker.ball_tracking(tol=0.15)
# break
if approach_steps < 2 and approach != 0:
state = 'ball_approach'
else:
state = 'straight_approach'
elif state == 'straight_approach':
striker.ball_tracking(tol=0.20)
bil = striker.get_ball_angles_from_camera(
striker.lower_camera
) # Ball in lower
print('Ball in lower!', bil)
if bil is not None and bil[1] > 0.15:
striker.speak('Ball is close enough, stop approach')
if bil is not None and bil[1] > 0.20:
striker.mover.stop_moving()
striker.speak('Align to goal')
striker.speak('Aligning to goal')
state = 'goal_align'
else:
striker.speak('Continue running')
striker.run_to_ball(1)
state = 'tracking'
elif state == 'ball_approach':
sleep(0.8)
bil = striker.get_ball_angles_from_camera(
striker.lower_camera
) # Ball in lower
try:
d = striker.distance_to_ball()
except ValueError:
if bil is not None:
state = 'straight_approach'
striker.speak('Ball is close. ' +
'But not close enough maybe')
else:
state = 'tracking'
continue
print('Distance to ball', d)
striker.speak("The distance to the ball is approximately "
+ str(round(d,2)) + " Meters")
angle = striker.walking_direction(approach, d)
d_run = d * cos(angle)
elif state == 'bdist_is_hypo':
angle = striker.walking_direction(approach, bdist, 'bdist')
rdist = bdist * cos(angle)
print('Approach angle', angle)
striker.mover.move_to(0, 0, angle)
striker.mover.wait()
striker.run_to_ball(d_run)
striker.run_to_ball(rdist)
striker.mover.wait()
striker.speak("I think I have reached the ball. " +
"I will start rotating")
approach_steps += 1
striker.mover.move_to(0, 0, -pi/2 * approach)
striker.mover.wait()
state = 'tracking'
state = 'init'
elif state == 'rdist_is_hypo':
angle = striker.walking_direction(approach, bdist, 'rdist')
rdist = bdist / cos(angle)
print('Approach angle', angle)
striker.mover.move_to(0, 0, angle)
striker.mover.wait()
striker.run_to_ball(rdist)
striker.mover.wait()
striker.mover.move_to(0, 0, -(pi/2 - angle) * approach)
striker.mover.wait()
state = 'init'
elif state == 'goal_align':
try:
if striker.align_to_goal():
striker.speak('Ball and goal are aligned')
state = "align"
except ValueError:
state = 'tracking'
continue
elif state == 'align':
striker.speak("I will try now to align to the ball")
striker.mover.set_head_angles(0, 0.25, 0.3)
sleep(0.5)
try:
success = striker.align_to_ball()
sleep(0.3)
if success:
striker.speak('Hasta la vista, Baby')
state = 'kick'
striker.speak('hasta')
except ValueError:
pass
# striker.mover.set_head_angles(0, 0, 0.3)
# state = 'tracking'
elif state == 'simple_kick':
striker.mover.set_head_angles(0,0.25,0.3)
striker.ball_tracking(tol=0.10, soll=0)
print('Doing the simple kick')
# just walk a short distance forward, ball should be near
# and it will probably be kicked in the right direction
striker.speak("Simple Kick")
sleep(1)
striker.mover.move_to_fast(0.5, 0, 0)
striker.mover.wait()
break
striker.ball_tracking()
elif state == 'kick':
print('KICK!')
@@ -168,8 +136,8 @@ if __name__ == '__main__':
sleep(0.3)
striker.mover.kick(fancy=True, foot='L')
striker.mover.stand_up()
striker.speak('Nice kick. Lets do the dance')
sleep(2)
striker.speak("Nice kick. Let's do a dance")
striker.mover.dance()
break
finally:
@@ -177,6 +145,30 @@ if __name__ == '__main__':
striker.mover.rest()
# ____________________ STRIKER NEW ________________________________
#
# Ball tracking --> Distance to ball --> Goal angle
# ^ |
# | |
# | yes v
# | Ball distance <-- Goal angle > thr
# | / | \ |
# | > 50 cm / |(20,50) \ < 20cm | no
# | / v \ v
# +- Distance is < Walk is hypo \ Straight approach
# | hypo | > until goal align
# | | (bil > 0.2)
# -----------------------+ |
# |
# | / | /^ | / v
# |( | ( |( Ball Goal align
# | \ | \_ | \ <-- align <-- (if lost ball run backwards)
#
#__________________________________________________________________
# ____________________ STRIKER __________________________
#
# +----> Ball tracking (see below) <-------------+

16
pykick/movements.py
View File

@@ -1,6 +1,6 @@
# from time import sleep
import argparse
from math import radians
from math import radians, pi
from naoqi import ALProxy
@@ -169,7 +169,17 @@ class NaoMover(object):
if not self.ready_to_move:
self.mp.moveInit()
self.ready_to_move = True
self.mp.post.moveTo(front, side, rotation)
if rotation != 0:
n_cycles = int(abs(rotation) // (pi/4))
sign = 1 if rotation > 0 else -1
rest = abs(rotation) % (pi/4)
print('Rotation', rotation, 'Cycles', n_cycles, 'Rest', rest)
for _ in range(n_cycles):
self.mp.post.moveTo(0, 0, pi/4 * sign)
self.wait()
self.mp.post.moveTo(0, 0, rest * sign)
else:
self.mp.post.moveTo(front, side, rotation)
def move_to_fast(self, front, side, rotation, wait=False):
if not self.ready_to_move:
@@ -210,7 +220,7 @@ class NaoMover(object):
clear_existing
)
self.wait()
self.stand_up(0.5)
self.stand_up(0.7)
def wait(self):
self.mp.waitUntilMoveIsFinished()

8
pykick/nao_defaults.json
View File

@@ -11,19 +11,19 @@
255
]
],
"cam": 1,
"goal": [
[
0,
0,
100
89
],
[
180,
65,
73,
255
]
],
"fps": 10,
"res": 2,
"ball_min_radius": 0.01,
"field": [
@@ -38,7 +38,7 @@
255
]
],
"fps": 10,
"cam": 1,
"ip": "192.168.0.11",
"port": 9559
}

64
pykick/striker.py
View File

@@ -2,7 +2,7 @@ from __future__ import print_function
from __future__ import division
from threading import Thread
from math import pi, tan, asin, radians
from math import pi, tan, asin, atan, radians
from time import sleep, time, strftime
from collections import deque
@@ -53,7 +53,8 @@ class Striker(object):
self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1]))
# Talking
self.speaker = ALProxy('ALTextToSpeech', bytes(nao_ip), nao_port)
self.player = ALProxy('ALAudioPlayer', bytes(nao_ip), nao_port)
self.tts = ALProxy('ALTextToSpeech', bytes(nao_ip), nao_port)
self.speach_queue = deque(maxlen=4)
self.speach_history = []
self.tts_thread = Thread(target=self._speaker)
@@ -80,7 +81,14 @@ class Striker(object):
def _speaker(self):
while not self.is_over:
while self.speach_queue:
self.speaker.say(self.speach_queue.pop())
text = self.speach_queue.pop()
if text in ('hasta', 'tiger'):
file_id = self.player.loadFile(
'/home/nao/audio/' + text + '.mp3'
)
self.player.play(file_id)
else:
self.tts.say(text)
sleep(0.5)
def _pov(self):
@@ -102,25 +110,22 @@ class Striker(object):
"""Intelligently rotate the robot to search for stuff."""
self.mover.stop_moving()
self.rotating = False
yaw = self.mover.get_head_angles()[0]
mag = yaw
yaw, pitch = self.mover.get_head_angles()
# determine direction of head rotation
sign = 1 if yaw >= 0 else -1
# the robot starts to move arround his z-Axis in the direction where his
# head is aligned when the head yaw angle has reached his maximum
if mag > 0.8:
self.mover.set_head_angles(-pi / 8, 0, 0.5)
if yaw > 0.8:
self.mover.set_head_angles(-pi / 8, pitch, 0.5)
sleep(0.5)
elif mag < -0.8:
elif yaw < -0.8:
self.mover.move_to_fast(0, 0, -pi / 4)
self.mover.wait()
#self.speak("Where is the ball? I am searching for it")
# rotate head to the left, if head yaw angle is equally zero or larger
# rotate head to the right, if head yaw angle is smaller than zero
else:
#self.speak("I have found the ball")
self.mover.change_head_angles(sign * pi / 8, 0, 0.5)
sleep(0.3)
@@ -175,15 +180,23 @@ class Striker(object):
return goal_l, goal_r, goal_c
def distance_to_ball(self):
camera = 'upper'
angles = self.get_ball_angles_from_camera(self.upper_camera)
if angles == None:
raise ValueError('No ball in sight')
camera = 'lower'
angles = self.get_ball_angles_from_camera(self.lower_camera)
if angles == None:
raise ValueError('No ball in sight')
y_angle = angles[1]
y_angle = pi/2 - y_angle - radians(15)
y_angle = pi/2 - y_angle - radians(15) - (radians(39)
if camera == 'lower'
else 0)
print('Ball distance through', camera, 'camera')
print('Angles', angles)
return 0.5 * tan(y_angle)
def walking_direction(self, lr, d):
return asin(0.5 / d) * lr
def walking_direction(self, lr, d, hypo):
return (asin(0.5 / d) if hypo == 'bdist' else atan(0.2 / d)) * lr
def ball_tracking(self, soll=0, tol=0.15):
"""Track the ball using the feed from top and bottom camera.
@@ -215,11 +228,9 @@ class Striker(object):
# stop visibility check
if not in_sight:
self.speak('No ball visible search it')
self.scan_rotation()
continue
# self.speak('I see the ball')
ball_locked = self.turn_to_ball(x, y, soll=soll, tol=tol)
print()
@@ -257,7 +268,6 @@ class Striker(object):
if abs(d_yaw) > tol:
self.mover.stop_moving()
print('Going to rotate by', d_yaw)
self.speak('Going to rotate')
self.mover.set_head_angles(soll, head_pitch, 0.3)
self.mover.move_to(0, 0, d_yaw)
self.mover.wait()
@@ -275,7 +285,7 @@ class Striker(object):
goal_x, goal_y = 0.095, 0.4
dx, dy = goal_x - x, goal_y - y
dx = -dx * 0.2 if abs(dx) > 0.05 else 0
dx = -dx * 0.2 if abs(dx) > 0.03 else 0
dy = dy * 0.3 if abs(dy) > 0.05 else 0
if dx == 0 and dy == 0:
return True
@@ -293,14 +303,11 @@ class Striker(object):
ball_angles = self.get_ball_angles_from_camera(self.lower_camera,
mask=False)
if ball_angles is None:
self.speak("Cannot see the ball")
raise ValueError('No ball')
x, y = ball_angles
print(x, y)
self.speak("Turn to ball")
self.turn_to_ball(x, y, tol=0.15)
self.speak('Trying to find the goal')
sleep(0.2)
goal = self.goal_search()
@@ -308,13 +315,9 @@ class Striker(object):
return False
gcl, gcr, gcc = goal
self.speak('Goal found')
print('Goal:', gcl, gcr, gcc)
if gcl > 0 > gcr:
self.speak("Goal and ball are aligned")
print('Goal ball aligned!')
#raise SystemExit
return True
if y > 0.35:
@@ -327,19 +330,24 @@ class Striker(object):
# return False
sign = -1 if gcc > 0 else 1
num_steps = int(min(abs(gcc), 0.1) // 0.05)
for _ in range(num_steps):
if sign == 1:
self.speak('Goal is on the right')
elif sign == -1:
self.speak('Goal is on the left')
for _ in range(2):
self.mover.move_to(0, 0.05 * sign, 0)
self.mover.wait()
return False
def goal_search(self):
self.speak('Searching for goal')
goal_angles = None
positions = [0, pi/6, pi/4, pi/3, pi/2]
angles = [-p for p in positions] + [p for p in positions][1:]
for angle in angles:
self.mover.set_head_angles(angle, 0)
self.mover.set_head_angles(angle, -0.3)
sleep(0.5)
for i in range(5):
print(i, goal_angles)