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

Align to ball more intelligently

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-25 13:08:20 +02:00
parent b479805a45
commit d22d5f0e11
2 changed files with 77 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
pykick/movements.py
View File

@@ -170,6 +170,13 @@ class NaoMover(object):
self.ready_to_move = True self.ready_to_move = True
self.mp.post.moveTo(front, side, rotation) self.mp.post.moveTo(front, side, rotation)
def move_toward(self, front, side, rotation):
if not self.ready_to_move:
self.mp.moveInit()
self.ready_to_move = True
self.mp.post.moveToward(front, side, rotation)
def wait(self): def wait(self):
self.mp.waitUntilMoveIsFinished() self.mp.waitUntilMoveIsFinished()

94
pykick/striker.py
View File

@@ -36,6 +36,10 @@ class Striker(object):
self.tts_thread = None self.tts_thread = None
self.last_speak = None self.last_speak = None
self.rotating = False
self.rot_dir = 0
self.timer_start = 0
self.timer_stop = 0
def speak(self, text): def speak(self, text):
if ( if (
@@ -56,6 +60,8 @@ class Striker(object):
def ball_scan(self): def ball_scan(self):
"""Intelligently rotate the robot to search for stuff.""" """Intelligently rotate the robot to search for stuff."""
self.mover.stop_moving()
self.rotating = False
yaw = self.mover.get_head_angles()[0] yaw = self.mover.get_head_angles()[0]
mag = yaw mag = yaw
@@ -64,17 +70,19 @@ class Striker(object):
# the robot starts to move arround his z-Axis in the direction where his # 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 # head is aligned when the head yaw angle has reached his maximum
if mag > 2: if mag > 0.8:
self.mover.set_head_angles(-pi / 4, 0, 0.5) self.mover.set_head_angles(-pi / 8, 0, 0.5)
elif mag < -2: sleep(0.5)
self.mover.move_to(0, 0, -pi / 12) elif mag < -0.8:
self.mover.move_to(0, 0, -pi / 6)
self.mover.wait()
#self.speak("Where is the ball? I am searching for it") #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 left, if head yaw angle is equally zero or larger
# rotate head to the right, if head yaw angle is smaller than zero # rotate head to the right, if head yaw angle is smaller than zero
else: else:
#self.speak("I have found the ball") #self.speak("I have found the ball")
self.mover.change_head_angles(sign * pi / 4, 0, 0.5) self.mover.change_head_angles(sign * pi / 8, 0, 0.5)
sleep(0.1) sleep(0.3)
def get_ball_angles_from_camera(self, cam, mask=True): def get_ball_angles_from_camera(self, cam, mask=True):
"""Detect the ball and return its angles in camera coordinates.""" """Detect the ball and return its angles in camera coordinates."""
@@ -131,6 +139,11 @@ class Striker(object):
True if robot is nicely aligned to ball; else False. True if robot is nicely aligned to ball; else False.
""" """
ball_locked = False
while not ball_locked:
# visibility check
for i in range(3):
cams = [self.upper_camera, self.lower_camera] cams = [self.upper_camera, self.lower_camera]
in_sight = False in_sight = False
@@ -138,34 +151,26 @@ class Striker(object):
ball_angles = self.get_ball_angles_from_camera(cam) ball_angles = self.get_ball_angles_from_camera(cam)
if ball_angles is not None: if ball_angles is not None:
x, y = ball_angles x, y = ball_angles
self.timer_start = time()
in_sight = True in_sight = True
self.loss_counter = 0 self.loss_counter = 0
break break
if in_sight:
break
# stop visibility check
if not in_sight: if not in_sight:
print('No ball in sight')
self.loss_counter += 1
# if ball is not in sight for more than five consecutive frames,
# start a ball scan
if self.loss_counter > 5:
self.speak("Where is the ball? I am searching for it")
self.ball_scan() self.ball_scan()
return False continue
# turn to ball, if the angle between the ball and the robot is too big ball_locked = self.turn_to_ball(x, y, soll=soll)
if abs(x) > 0.15: print()
# self.speak('Align to the ball')
self.mover.stop_moving()
self.turn_to_ball(x, y, soll=soll)
return False
else:
return True return True
def run_to_ball(self): def run_to_ball(self):
self.mover.move_to(1, 0, 0) self.mover.move_to(1, 0, 0)
def turn_to_ball(self, ball_x, ball_y, tol=0.05, soll=0): def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0, m_delta=0.2):
"""Align robot to the ball. """Align robot to the ball.
If head is not centered at the ball (within tolerance), then If head is not centered at the ball (within tolerance), then
@@ -181,25 +186,39 @@ class Striker(object):
# center head at the ball # center head at the ball
if (abs(d_yaw) > 0.01): if (abs(d_yaw) > 0.01):
self.mover.change_head_angles(d_yaw, d_pitch, self.mover.change_head_angles(d_yaw, d_pitch,
abs(d_yaw) / 2) min(1, abs(d_yaw) * 1.25))
sleep(1) sleep(0.05)
self.mover.wait()
yaw = self.mover.get_head_angles()[0] yaw = self.mover.get_head_angles()[0]
print('head yaw', yaw) self.timer_stop = time()
print('Ball to head', self.timer_stop - self.timer_start)
print('head yaw', yaw, end=' ')
d_yaw = yaw - soll d_yaw = yaw - soll
print('head d_yaw', d_yaw) print('head d_yaw', d_yaw)
print('Rotating', self.rotating, end=' ')
print('Rotation direction', self.rot_dir, end=' ')
print('Allowed tolerance', tol)
# align body with the head # align body with the head
if not self.rotating:
if abs(d_yaw) > tol: if abs(d_yaw) > tol:
self.mover.stop_moving()
self.rotating = True
self.rot_dir = -1 if d_yaw > 0 else 1
print('Going to rotate') print('Going to rotate')
self.speak("Going to rotate") self.speak("Going to rotate")
self.mover.set_head_angles(soll, 0, 0.5) self.mover.move_toward(0, 0, -self.rot_dir)
self.mover.move_to(0, 0, d_yaw) sleep(0.8)
self.mover.wait() return False
else:
# def move_sideways(self, dy): print('Success')
# sign = 1 if dy > 0 else -1 self.speak('Ball locked')
return True
else:
if d_yaw * self.rot_dir > -tol - m_delta:
self.rotating = False
self.mover.stop_moving()
return False
def align_to_ball(self): def align_to_ball(self):
ball_angles = self.get_ball_angles_from_camera(self.lower_camera, ball_angles = self.get_ball_angles_from_camera(self.lower_camera,
@@ -301,7 +320,8 @@ class Striker(object):
# | | # | |
# | | | # | | |
# | | | # | | |
# | v | # | Ball in lower cam? | # | v |
# | Ball in lower cam? |
# | / \ | # | / \ |
# lost | yes / \ cannot do | # lost | yes / \ cannot do |
# ball | v v | # ball | v v |
@@ -377,7 +397,7 @@ if __name__ == '__main__':
elif args.rest: elif args.rest:
striker.mover.rest() striker.mover.rest()
# perform a fancy kick # perform a (fancy) kick
elif args.kick: elif args.kick:
striker.mover.stand_up() striker.mover.stand_up()
striker.mover.kick(fancy=True) striker.mover.kick(fancy=True)
@@ -387,7 +407,7 @@ if __name__ == '__main__':
else: else:
try: # Hit Ctrl-C to stop, cleanup and exit try: # Hit Ctrl-C to stop, cleanup and exit
state = 'tracking' state = 'tracking'
start_soll = -0.8 start_soll = 0.8
align_start = 0.29 align_start = 0.29
curve_start = -0.1 curve_start = -0.1
curve_stop = 0.1 curve_stop = 0.1
@@ -402,11 +422,11 @@ if __name__ == '__main__':
if state == 'tracking': if state == 'tracking':
# start ball approach when ball is visible # start ball approach when ball is visible
print('Soll angle', soll) print('Soll angle', soll)
striker.mover.set_head_angles(soll, 0, 0.5)
sleep(0.3)
if striker.ball_tracking(soll=soll): if striker.ball_tracking(soll=soll):
striker.speak("Ball approach") striker.speak("Ball approach")
state = 'ball_approach' state = 'ball_approach'
# striker.speak('I got the ball')
# sleep(10)
elif state == 'ball_approach': elif state == 'ball_approach':
bil = striker.get_ball_angles_from_camera( bil = striker.get_ball_angles_from_camera(