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

Silent but efficient

Browse Source
This commit is contained in:
Pavel Lutskov
2018-07-01 16:59:53 +02:00
parent e54858fbfd
commit cbdac79911
2 changed files with 85 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

156
pykick/__main__.py
View File

@@ -18,12 +18,13 @@ if __name__ == '__main__':
goal_hsv=cfg['goal'], field_hsv=cfg['field'], goal_hsv=cfg['goal'], field_hsv=cfg['field'],
ball_min_radius=cfg['ball_min_radius'], ball_min_radius=cfg['ball_min_radius'],
) )
striker.speak('tiger')
sleep(4.75) # striker.speak('tiger')
# sleep(4.75)
striker.mover.stand_up(1.0) striker.mover.stand_up(1.0)
sleep(9) # sleep(9)
print('Initialized') # print('Initialized')
striker.speak('Initialized') striker.speak('Initialized')
state = 'init' state = 'init'
loop_start = time() loop_start = time()
@@ -47,12 +48,12 @@ if __name__ == '__main__':
_, _, gcc = striker.goal_search() _, _, gcc = striker.goal_search()
print('Goal center', gcc, 'Ball dist', bdist) print('Goal center', gcc, 'Ball dist', bdist)
if abs(gcc) < 0.4 or bdist <= 0.2: if abs(gcc) < 0.2 or bdist <= 0.35:
print('Straight approach') print('Straight approach')
state = 'straight_approach' state = 'straight_approach'
approach = 0 approach = 0
elif 0.20 < bdist < 0.50: elif 0.35 < bdist < 0.50:
print('Rdist is hypo') print('Rdist is hypo')
state = 'rdist_is_hypo' state = 'rdist_is_hypo'
approach = 1 if gcc < 0 else - 1 approach = 1 if gcc < 0 else - 1
@@ -75,11 +76,10 @@ if __name__ == '__main__':
striker.ball_tracking(tol=0.20) striker.ball_tracking(tol=0.20)
bil = striker.get_ball_angles_from_camera( bil = striker.get_ball_angles_from_camera(
striker.lower_camera striker.lower_camera
) # Ball in lower ) # Ball in lower print('Ball in lower!', bil)
print('Ball in lower!', bil)
if bil is not None and bil[1] > 0.20: if bil is not None and bil[1] > 0.20:
striker.mover.stop_moving() striker.mover.stop_moving()
striker.speak('Aligning to goal') striker.speak('Ball is close enough. Aligning to goal')
state = 'goal_align' state = 'goal_align'
else: else:
striker.run_to_ball(1) striker.run_to_ball(1)
@@ -87,26 +87,30 @@ if __name__ == '__main__':
elif state == 'bdist_is_hypo': elif state == 'bdist_is_hypo':
angle = striker.walking_direction(approach, bdist, 'bdist') angle = striker.walking_direction(approach, bdist, 'bdist')
rdist = bdist * cos(angle) rdist = bdist * cos(angle)
print('Approach angle', angle) print('Approach angle', angle, 'Run distance', rdist)
striker.mover.move_to(0, 0, angle) striker.mover.move_to(0, 0, angle)
striker.mover.wait() striker.mover.wait()
striker.run_to_ball(rdist) if rdist > 1.5:
striker.mover.wait() striker.run_to_ball(1.5)
striker.mover.move_to(0, 0, -pi/2 * approach) striker.mover.wait()
striker.mover.wait() else:
striker.run_to_ball(rdist)
striker.mover.wait()
striker.mover.move_to(0, 0, -pi/2 * approach)
striker.mover.wait()
state = 'init' state = 'init'
elif state == 'rdist_is_hypo': elif state == 'rdist_is_hypo':
angle = striker.walking_direction(approach, bdist, 'rdist') angle = striker.walking_direction(approach, bdist, 'rdist')
rdist = bdist / cos(angle) rdist = bdist / cos(angle)
print('Approach angle', angle) print('Approach angle', angle, 'Run distance', rdist)
striker.mover.move_to(0, 0, angle) striker.mover.move_to(0, 0, angle)
striker.mover.wait() striker.mover.wait()
striker.run_to_ball(rdist) striker.run_to_ball(rdist)
striker.mover.wait() striker.mover.wait()
striker.mover.move_to(0, 0, -(pi/2 - angle) * approach) striker.mover.move_to(0, 0, (-pi/2 - angle) * approach)
striker.mover.wait() striker.mover.wait()
state = 'init' state = 'init'
@@ -126,7 +130,7 @@ if __name__ == '__main__':
sleep(0.3) sleep(0.3)
if success: if success:
state = 'kick' state = 'kick'
striker.speak('hasta') # striker.speak('hasta')
except ValueError: except ValueError:
striker.ball_tracking() striker.ball_tracking()
@@ -136,81 +140,53 @@ if __name__ == '__main__':
sleep(0.3) sleep(0.3)
striker.mover.kick(fancy=True, foot='L') striker.mover.kick(fancy=True, foot='L')
striker.mover.stand_up() striker.mover.stand_up()
striker.speak('Nice kick. Lets do the dance') striker.mover.set_head_angles(0, 0)
sleep(2) striker.speak('Trying to confirm the goal')
striker.mover.dance() sleep(3)
ball = striker.get_ball_angles_from_camera(striker.upper_camera)
goal = striker.goal_search()
if ball is not None and goal is not None:
ball_x = ball[0]
gcl, gcr, _ = goal
print('Ball, goal', ball, goal)
if not (gcl > ball_x > gcr):
striker.speak('I Failed')
sleep(0.5)
break
else:
striker.speak('I succeeded, confirmed')
sleep(0.5)
break
striker.speak('I succeeded, presumably')
striker.mover.stand_up()
sleep(0.5)
# striker.speak('Nice kick. Lets do the dance')
# sleep(2)
# striker.mover.dance()
break break
finally: finally:
striker.close() striker.close()
striker.mover.rest() striker.mover.rest()
# _________________________ STRIKER _____________________________
# [ ]
# [ Ball tracking --> Distance to ball --> Goal angle ]
# ____________________ STRIKER NEW ________________________________ # [ ^ | ]
# # [ | | ]
# Ball tracking --> Distance to ball --> Goal angle # [ | yes v ]
# ^ | # [ | Ball distance <-- Goal angle > thr ]
# | | # [ | / | \ | ]
# | yes v # [ | > 50 cm / |(35,50) \ < 35cm | no ]
# | Ball distance <-- Goal angle > thr # [ | / v \ v ]
# | / | \ | # [ +- Distance is < Walk is hypo \ Straight approach ]
# | > 50 cm / |(20,50) \ < 20cm | no # [ | hypo | > until goal align ]
# | / v \ v # [ | | (bil > 0.2) ]
# +- Distance is < Walk is hypo \ Straight approach # [ -----------------------+ | ]
# | hypo | > until goal align # [ | ]
# | | (bil > 0.2) # [ | / | /^ | / v ]
# -----------------------+ | # [ |( | ( |( Ball Goal align ]
# | # [ | \ | \_ | \ <-- align <-- (if lost ball run backwards) ]
# | / | /^ | / v # [_______________________________________________________________]
# |( | ( |( Ball Goal align
# | \ | \_ | \ <-- align <-- (if lost ball run backwards)
#
#__________________________________________________________________
# ____________________ STRIKER __________________________
#
# +----> Ball tracking (see below) <-------------+
# | |
# | | |
# | | |
# | v |
# | Ball in lower cam? |
# | / \ |
# lost | yes / \ cannot do |
# ball | v v |
# +-- Goal align Ball is only in top camera --+
# | Move closer.
# |
# successful |
# v
# Kick it! (Fancy or simple)
#
# _______________________________________________________
# ____________________ TRACKING _________________________
#
# yes
# check if ball visible ---> rotate head to the ball
# ^ | |
# | | no |
# | v |
# +--- ball scan rotation |
# | |
# | no V
# | +---------- already rotating body?
# | | |
# | v | yes
# | head angle too big? v
# | / \ head angle
# | yes / \ no is below threshold?
# | v v | |
# | stop successful | no | yes
# | moving exit | v
# +----- and start | stop rotating body
# | rotating body | |
# | | |
# +---------------------------------+---------+
#
# _______________________________________________________

26
pykick/striker.py
View File

@@ -17,12 +17,13 @@ class Striker(object):
def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv, def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv,
ball_min_radius): ball_min_radius):
# Timestamp # Maintenance
self.run_id = strftime('%Y%m%d%H%M%S') self.run_id = strftime('%Y%m%d%H%M%S')
self.is_over = False
self.last_goal = 'right'
# Motion # Motion
self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port) self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port)
self.is_over = False
# Sight # Sight
self.upper_camera = NaoImageReader( self.upper_camera = NaoImageReader(
@@ -196,7 +197,7 @@ class Striker(object):
return 0.5 * tan(y_angle) return 0.5 * tan(y_angle)
def walking_direction(self, lr, d, hypo): def walking_direction(self, lr, d, hypo):
return (asin(0.5 / d) if hypo == 'bdist' else atan(0.2 / d)) * lr return (asin(0.40 / d) if hypo == 'bdist' else atan(0.2 / d)) * lr
def ball_tracking(self, soll=0, tol=0.15): def ball_tracking(self, soll=0, tol=0.15):
"""Track the ball using the feed from top and bottom camera. """Track the ball using the feed from top and bottom camera.
@@ -209,6 +210,7 @@ class Striker(object):
""" """
ball_locked = False ball_locked = False
tried_step_back = False
while not ball_locked: while not ball_locked:
# visibility check # visibility check
for i in range(3): for i in range(3):
@@ -228,7 +230,13 @@ class Striker(object):
# stop visibility check # stop visibility check
if not in_sight: if not in_sight:
self.scan_rotation() if not tried_step_back:
self.mover.move_to(-0.1, 0, 0)
self.mover.wait()
self.mover.stand_up()
tried_step_back = True
else:
self.scan_rotation()
continue continue
ball_locked = self.turn_to_ball(x, y, soll=soll, tol=tol) ball_locked = self.turn_to_ball(x, y, soll=soll, tol=tol)
@@ -282,7 +290,7 @@ class Striker(object):
if ball_angles is None: if ball_angles is None:
raise ValueError('No ball') raise ValueError('No ball')
x, y = ball_angles x, y = ball_angles
goal_x, goal_y = 0.095, 0.4 goal_x, goal_y = 0.088, 0.4
dx, dy = goal_x - x, goal_y - y dx, dy = goal_x - x, goal_y - y
dx = -dx * 0.2 if abs(dx) > 0.03 else 0 dx = -dx * 0.2 if abs(dx) > 0.03 else 0
@@ -320,11 +328,11 @@ class Striker(object):
if gcl > 0 > gcr: if gcl > 0 > gcr:
return True return True
if y > 0.35: if y > 0.38:
self.mover.move_to(-0.05, 0, 0) self.mover.move_to(-0.05, 0, 0)
self.mover.wait() self.mover.wait()
# return False # return False
elif y < 0.25: elif y < 0.28:
self.mover.move_to(0.05, 0, 0) self.mover.move_to(0.05, 0, 0)
self.mover.wait() self.mover.wait()
# return False # return False
@@ -342,9 +350,12 @@ class Striker(object):
def goal_search(self): def goal_search(self):
self.speak('Searching for goal') self.speak('Searching for goal')
print('Last goal:', self.last_goal)
goal_angles = None goal_angles = None
positions = [0, pi/6, pi/4, pi/3, pi/2] positions = [0, pi/6, pi/4, pi/3, pi/2]
direction = 1 if self.last_goal == 'right' else -1
angles = [-p for p in positions] + [p for p in positions][1:] angles = [-p for p in positions] + [p for p in positions][1:]
angles = [a * direction for a in angles]
for angle in angles: for angle in angles:
self.mover.set_head_angles(angle, -0.3) self.mover.set_head_angles(angle, -0.3)
@@ -358,6 +369,7 @@ class Striker(object):
goal_angles = tuple(gc + angle for gc in goal_angles) goal_angles = tuple(gc + angle for gc in goal_angles)
self.mover.set_head_angles(0, 0) self.mover.set_head_angles(0, 0)
print('Goal found:', str(goal_angles)) print('Goal found:', str(goal_angles))
self.last_goal = 'left' if goal_angles[2] > 0 else 'right'
return goal_angles return goal_angles
print('Goal not found at ', str(angle)) print('Goal not found at ', str(angle))