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

somewhat saner comments

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-23 10:58:00 +02:00
parent e0bf8e35cf
commit 5c8f66039f
2 changed files with 101 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
pykick/movements.py
View File

@@ -18,19 +18,18 @@ class NaoMover(object):
(1, 1, 'AnkleRoll', -9, 0.2)], (1, 1, 'AnkleRoll', -9, 0.2)],
1], 1],
# lift the feed using the knee joint and the ankle joint # lift the foot using the knee joint and the ankle joint
[[(1, 0, 'KneePitch', 90, 0.3), [[(1, 0, 'KneePitch', 90, 0.3),
(1, 0, 'AnklePitch', -40, 0.4)], (1, 0, 'AnklePitch', -40, 0.4)],
1.5,], 1.5,],
# move feed back using hip, knee and ankle joint # kick-it!
[[(1, 0, 'HipPitch', -45, 0.05), [[(1, 0, 'HipPitch', -45, 0.05),
(1, 0, 'KneePitch', 10, 0.8), (1, 0, 'KneePitch', 10, 0.8),
(1, 0, 'AnklePitch', 20, 0.7)], (1, 0, 'AnklePitch', 20, 0.7)],
1], 1],
# move feed forward using knee and ankle joint # prepare to return into standing position
# perform the kick
[[(1, 0, 'KneePitch', 40, 0.25), [[(1, 0, 'KneePitch', 40, 0.25),
(1, 0, 'AnklePitch', 10, 0.25)], (1, 0, 'AnklePitch', 10, 0.25)],
1,], 1,],

226
pykick/striker.py
View File

@@ -10,16 +10,17 @@ from .finders import BallFinder, GoalFinder
from .movements import NaoMover from .movements import NaoMover
import argparse import argparse
class Striker(object): class Striker(object):
def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv,
ball_min_radius, run_after): ball_min_radius, run_after):
self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port) self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port)
self.mover.stand_up() self.mover.stand_up()
self.video_top = NaoImageReader(nao_ip, port=nao_port, res=res, self.upper_camera = NaoImageReader(nao_ip, port=nao_port, res=res,
fps=30, cam_id=0) fps=30, cam_id=0)
self.video_bot = NaoImageReader(nao_ip, port=nao_port, res=res, self.lower_camera = NaoImageReader(nao_ip, port=nao_port, res=res,
fps=30, cam_id=1) fps=30, cam_id=1)
self.ball_finder = BallFinder(tuple(ball_hsv[0]), tuple(ball_hsv[1]), self.ball_finder = BallFinder(tuple(ball_hsv[0]), tuple(ball_hsv[1]),
ball_min_radius) ball_min_radius)
self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1])) self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1]))
@@ -28,18 +29,16 @@ class Striker(object):
self.run_after = run_after self.run_after = run_after
self.in_move = False self.in_move = False
# this function will scan for the ball, if it is not in sight
def ball_scan(self): def ball_scan(self):
"""Intelligently rotate the robot to search for stuff."""
# determine current head angle
yaw = self.mover.get_head_angles()[0] yaw = self.mover.get_head_angles()[0]
mag = abs(yaw) mag = abs(yaw)
# determine direction of head rotation # determine direction of head rotation
sign = 1 if yaw >= 0 else -1 sign = 1 if yaw >= 0 else -1
# the robot starts to move arround his z-Axis in the direction where his head is aligned # the robot starts to move arround his z-Axis in the direction where his
# 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 > 2:
self.mover.move_to(0, 0, sign * pi / 12) self.mover.move_to(0, 0, sign * pi / 12)
@@ -48,9 +47,8 @@ class Striker(object):
else: else:
self.mover.change_head_angles(sign * pi / 4, 0, 0.5) self.mover.change_head_angles(sign * pi / 4, 0, 0.5)
# this function detects the ball in the view of a given camera view
# and returns the angles of the ball to the camera
def get_ball_angles_from_camera(self, cam): def get_ball_angles_from_camera(self, cam):
"""Detect the ball and return its angles in camera coordinates."""
ball = self.ball_finder.find_colored_ball( ball = self.ball_finder.find_colored_ball(
cam.get_frame() cam.get_frame()
) )
@@ -65,50 +63,39 @@ class Striker(object):
def distance_to_ball(self): def distance_to_ball(self):
return 0.5 return 0.5
# this function tracks the ball in the camera views
def ball_tracking(self): def ball_tracking(self):
"""Track the ball using the feed from top and bottom camera.
# get video streams from both cameras Returns
cams = [self.video_top, self.video_bot] -------
bool
True if robot is nicely aligned to ball; else False.
"""
cams = [self.upper_camera, self.lower_camera]
in_sight = False in_sight = False
# try to determine the angle of the ball to the cameras in both streams
for cam in cams: for cam in cams:
ball_angles = self.get_ball_angles_from_camera(cam) ball_angles = self.get_ball_angles_from_camera(cam)
# check if the ball angles could be determined
if ball_angles is not None: if ball_angles is not None:
# safe ball angles in x,y
x, y = ball_angles x, y = ball_angles
# ball is in view -> set in_sight variable to true
in_sight = True in_sight = True
# reset ball loss counter
self.loss_counter = 0 self.loss_counter = 0
break break
# actions, if the ball is not in sight
if not in_sight: if not in_sight:
print('No ball in sight') print('No ball in sight')
# increase ball loss counter
self.loss_counter += 1 self.loss_counter += 1
# if ball is not in view for more than five times, # if ball is not in sight for more than five consecutive frames,
# start a ball scan # start a ball scan
if self.loss_counter > 5: if self.loss_counter > 5:
self.ball_scan() self.ball_scan()
return False return False
# check if the x angle between the robot and the ball is above a specific threshold # turn to ball, if the angle between the ball and the robot is too big
if abs(x) > 0.15: if abs(x) > 0.15:
# stop the robot, if the angle is to large
self.mover.stop_moving() self.mover.stop_moving()
# align the robot to the ball
self.turn_to_ball(x, y) self.turn_to_ball(x, y)
return False return False
else: else:
@@ -117,41 +104,39 @@ class Striker(object):
def run_to_ball(self): def run_to_ball(self):
self.mover.move_to(1, 0, 0) self.mover.move_to(1, 0, 0)
# this function aligns the robot to the ball
def turn_to_ball(self, ball_x, ball_y): def turn_to_ball(self, ball_x, ball_y):
"""Align robot to the ball.
# only the x ball angle is relevant for the rotation If head is not centered at the ball (within tolerance), then
turn head to ball. If after that the angle of head to body
becomes too big, rotate the body by the head angle and
simultaneously rotate head into 0 position to achieve alignment.
"""
# only the x ball angle is relevant for alignment
d_yaw, d_pitch = ball_x, 0 d_yaw, d_pitch = ball_x, 0
print('ball yaw:', d_yaw) print('ball yaw:', d_yaw)
# check if the angle between the robot and the ball is above a specific threshold # center head at the ball
if (abs(d_yaw) > 0.01): if (abs(d_yaw) > 0.01):
# try to align camera with the ball
self.mover.change_head_angles(d_yaw, d_pitch, self.mover.change_head_angles(d_yaw, d_pitch,
abs(d_yaw) / 2) abs(d_yaw) / 2)
sleep(1) sleep(1)
self.mover.wait() self.mover.wait()
# determine current head angle to estimate
# how far the robot has to rotate arround the z-Axis
yaw = self.mover.get_head_angles()[0] yaw = self.mover.get_head_angles()[0]
print('head yaw', yaw) print('head yaw', yaw)
# determine if the angle of the head to the body is above a specific threshold # align body with the head
if abs(yaw) > 0.05: if abs(yaw) > 0.05:
print('Going to rotate') print('Going to rotate')
# change the head angles to 0 0
self.mover.set_head_angles(0, 0, 0.5) self.mover.set_head_angles(0, 0, 0.5)
# rotate robot arround the z-Axis for the estimated angle
self.mover.move_to(0, 0, yaw) self.mover.move_to(0, 0, yaw)
self.mover.wait() self.mover.wait()
def align_to_ball(self): def align_to_ball(self):
ball_angles = self.get_ball_angles_from_camera(self.video_bot) ball_angles = self.get_ball_angles_from_camera(self.lower_camera)
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
@@ -169,7 +154,7 @@ class Striker(object):
return False return False
def align_to_goal(self): def align_to_goal(self):
ball_angles = self.get_ball_angles_from_camera(self.video_bot) ball_angles = self.get_ball_angles_from_camera(self.lower_camera)
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
@@ -189,11 +174,11 @@ class Striker(object):
return False return False
goal_contour = self.goal_finder.find_goal_contour( goal_contour = self.goal_finder.find_goal_contour(
self.video_top.get_frame() self.upper_camera.get_frame()
) )
if goal_contour is not None: if goal_contour is not None:
goal_center_x = self.goal_finder.goal_center(goal_contour) goal_center_x = self.goal_finder.goal_center(goal_contour)
gcx_rel, _ = self.video_top.to_relative(goal_center_x, 0) gcx_rel, _ = self.upper_camera.to_relative(goal_center_x, 0)
if abs(gcx_rel - 0.5) > 0.1: if abs(gcx_rel - 0.5) > 0.1:
increment = 0.1 if gcx_rel > 0.5 else -0.1 increment = 0.1 if gcx_rel > 0.5 else -0.1
else: else:
@@ -209,8 +194,8 @@ class Striker(object):
def close(self): def close(self):
self.mover.rest() self.mover.rest()
self.video_top.close() self.upper_camera.close()
self.video_bot.close() self.lower_camera.close()
# ____________________ STRIKER __________________________ # ____________________ STRIKER __________________________
# #
@@ -272,7 +257,6 @@ if __name__ == '__main__':
run_after=False run_after=False
) )
# allow additional arguments when running the function like # allow additional arguments when running the function like
# stand # stand
# rest # rest
@@ -281,19 +265,17 @@ if __name__ == '__main__':
parser = argparse.ArgumentParser() parser = argparse.ArgumentParser()
parser.add_argument("-s", "--stand", action="store_true", parser.add_argument("-s", "--stand", action="store_true",
help="let the robot stand up") help="let the robot stand up")
parser.add_argument("-k", "--kick", action="store_true", parser.add_argument("-k", "--kick", action="store_true",
help="let the robot do a fancy kick") help="let the robot do a fancy kick")
parser.add_argument("-r", "--rest", action="store_true", parser.add_argument("-r", "--rest", action="store_true",
help="let the robot rest") help="let the robot rest")
args = parser.parse_args() args = parser.parse_args()
# bring robot in stand_up position
if args.stand: if args.stand:
striker.mover.stand_up() striker.mover.stand_up()
# bring robot in rest postion
elif args.rest: elif args.rest:
striker.mover.rest() striker.mover.rest()
@@ -305,91 +287,63 @@ if __name__ == '__main__':
# perform normal state-machine if no input argument is given # perform normal state-machine if no input argument is given
# (see diagram above)
else: else:
try: # Hit Ctrl-C to stop, cleanup and exit
state = 'tracking'
while True:
# meassure time for debbuging
loop_start = time()
print('State:', state)
try: if state == 'tracking':
# start with ball tracking first # start ball approach when ball is visible
state = 'tracking' if striker.ball_tracking():
state = 'ball_approach'
# state machine of the striker elif state == 'ball_approach':
while True: ball_in_lower = striker.get_ball_angles_from_camera(
striker.lower_camera
)
print(ball_in_lower)
if (ball_in_lower is not None and ball_in_lower[1] > 0.28):
print('Ball is close enough, stop approach')
#striker.mover.stop_moving()
#state = 'align'
state='simple_kick'
else:
print('Continue running')
striker.run_to_ball()
state = 'tracking'
# start time meassure for debbuging elif state == 'simple_kick':
loop_start = time() #striker.mover.set_head_angles(0,0.25,0.3)
print('Doing the simple kick')
# print the current state of the state machine # just walk a short distance forward, ball should be near
print('State:', state) # and it will probably be kicked in the right direction
striker.mover.move_to(0.3,0,0)
striker.mover.wait()
state = 'tracking'
# actions in the tracking state elif state == 'align':
if state == 'tracking': striker.mover.set_head_angles(0, 0.25, 0.3)
sleep(0.5)
try:
success = striker.align_to_ball()
sleep(0.3)
if success:
state = 'kick'
except ValueError:
striker.mover.set_head_angles(0, 0, 0.3)
state = 'tracking'
# start ball approach when ball is visible elif state == 'kick':
if striker.ball_tracking(): print('KICK!')
state = 'ball_approach' striker.mover.kick()
break
# actions in the ball_approach state loop_end = time()
elif state == 'ball_approach': print('Loop time:', loop_end - loop_start)
finally:
# get the angle of the ball in the picture of the lower camera striker.close()
ball_in_lower = striker.get_ball_angles_from_camera(
striker.video_bot
)
# print the angle of the ball in the lower camera
print(ball_in_lower)
# check if the ball is in the lower camera
# and the angle is above a specific threshold (ball is close enough)
if (ball_in_lower is not None
and ball_in_lower[1] > 0.28):
print('Ball is in lower camera, go to align')
#striker.mover.stop_moving()
#state = 'align'
# perform a simple kick
state='simple_kick'
# continue moving, if the ball is not close enough
# or not in the view of the lower camera
else:
print('Continue running')
striker.run_to_ball()
# go back to the tracking state
state = 'tracking'
# actions in the simple_kick state
elif state == 'simple_kick':
#striker.mover.set_head_angles(0,0.25,0.3)
print('Doing the simple kick')
# just walk a short distance straight forward,
# as the ball should be straight ahead in a small distance
striker.mover.move_to(0.3,0,0)
striker.mover.wait()
# go back to the tracking state after the simple_kick
state = 'tracking'
elif state == 'align':
striker.mover.set_head_angles(0, 0.25, 0.3)
sleep(0.5)
try:
success = striker.align_to_ball()
sleep(0.3)
if success:
state = 'kick'
except ValueError:
striker.mover.set_head_angles(0, 0, 0.3)
state = 'tracking'
elif state == 'kick':
print('KICK!')
striker.mover.kick()
break
# stop time meassuring for debbuging and print the time of the loop
loop_end = time()
print('Loop time:', loop_end - loop_start)
finally:
striker.close()