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

Robot died, started refactoring

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-29 15:15:34 +02:00
parent 3a92d8530d
commit 93ed282170
4 changed files with 135 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
pykick/move_robot.py
View File

@@ -1,25 +0,0 @@
from .movements import NaoMover
from .utils import read_config
if __name__ == "__main__":
cfg = read_config()
mover = NaoMover(cfg['ip'], cfg['port'])
mover.stand_up()
while True:
amount = float(raw_input('How much: '))
mover.move_to(0, amount, 0)
# mover.move_to(0, 0, 0.5 * amount)
# mover.wait()
# mover.move_to(0, -0.3 * amount, 0)
# mover.wait()
# mover.move_to(-0.1 * abs(amount), 0, 0)
# amount = float(raw_input('How much: '))
# if axis == 0:
# mover.move_to(amount, 0, 0)
# elif axis == 1:
# mover.move_to(0, amount, 0)
# elif axis == 2:
# mover.move_to(0, 0, amount)
# else:
# print('Axis out of range (0, 1, 2)')

55
pykick/movements.py
View File

@@ -1,8 +1,11 @@
# from time import sleep
import argparse
from math import radians
from naoqi import ALProxy
from .utils import read_config
class NaoMover(object):
@@ -89,7 +92,6 @@ class NaoMover(object):
joints.append(sides[side] + joint)
angles.append(radians(angle))
self.mp.angleInterpolation(joints, angles, wait, True)
# sleep(wait)
self.stand_up(0.5)
self.set_arm_stiffness()
@@ -174,8 +176,13 @@ class NaoMover(object):
if not self.ready_to_move:
self.mp.moveInit()
self.ready_to_move = True
self.mp.post.moveTo(front, side, rotation,[['MaxStepX', 0.07999999821186066], ['MaxStepY', 0.1599999964237213], ['MaxStepTheta', 0.5235987901687622], ['MaxStepFrequency', 1.0], ['StepHeight', 0.02]])
self.mp.post.moveTo(front, side, rotation, [
['MaxStepX', 0.07999999821186066],
['MaxStepY', 0.1599999964237213],
['MaxStepTheta', 0.5235987901687622],
['MaxStepFrequency', 1.0],
['StepHeight', 0.02]
])
def move_toward(self, front, side, rotation):
if not self.ready_to_move:
@@ -183,10 +190,50 @@ class NaoMover(object):
self.ready_to_move = True
self.mp.post.moveToward(front, side, rotation)
def wait(self):
self.mp.waitUntilMoveIsFinished()
def stop_moving(self):
print('STOOOP')
self.mp.stopMove()
if __name__ == '__main__':
cfg = read_config()
mover = NaoMover(cfg['ip'], cfg['port'])
parser = argparse.ArgumentParser()
actions = parser.add_mutually_exclusive_group()
actions.add_argument("-s", "--stand", action="store_true",
help="let the robot stand up")
actions.add_argument("-k", "--kick", action="store_true",
help="let the robot do a fancy kick")
actions.add_argument("-r", "--rest", action="store_true",
help="let the robot rest")
actions.add_argument("-m", "--move", action="store_true",
help="move around")
args = parser.parse_args()
if args.stand:
mover.stand_up()
elif args.rest:
mover.rest()
# perform a (fancy) kick
elif args.kick:
mover.stand_up()
mover.kick()
elif args.move:
mover.stand_up()
while True:
amount_x = float(raw_input('How much x: '))
amount_y = float(raw_input('How much y: '))
amount_z = float(raw_input('How much z: '))
print(amount_x, amount_y, amount_z)
mover.move_to(amount_x, amount_y, amount_z)
mover.wait()

4
pykick/nao_defaults.json
View File

@@ -11,7 +11,6 @@
255
]
],
"cam": 1,
"goal": [
[
0,
@@ -24,6 +23,7 @@
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
}

199
pykick/striker.py
View File

@@ -1,7 +1,6 @@
from __future__ import print_function
from __future__ import division
import argparse
from threading import Thread
from math import pi,tan,asin
from time import sleep, time
@@ -157,7 +156,6 @@ class Striker(object):
y_angle=angles[1]
y_angle=pi/2-y_angle-15*pi/180
distance = 0.5 * tan(y_angle)
# vertical distance to ball at the end
vert_dist=0.3
@@ -183,7 +181,7 @@ class Striker(object):
print()
def run_to_ball(self):
self.mover.move_toward(1, 3 * self.dy, 0)
self.mover.move_toward(1, min(2 * self.dy, 1), 0)
def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0, fancy=False,
m_delta=0.2):
@@ -313,9 +311,12 @@ class Striker(object):
def close(self):
self.is_over = True
self.tts_thread.join()
if self.tts_thread.isAlive():
self.tts_thread.join()
self.upper_camera.close()
self.lower_camera.close()
self.mover.stop_moving()
# self.mover.stand_up()
def goal_search(self):
goal_center_x = None
@@ -389,124 +390,88 @@ class Striker(object):
if __name__ == '__main__':
cfg = read_config()
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'],
)
try: # Hit Ctrl-C to stop, cleanup and exit
cfg = read_config()
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'],
)
# allow additional arguments when running the function like
# stand
# rest
# kick
# if no argument is given the state machine is run
state = 'tracking'
init_soll = 0.0
align_start = 0.15
curve_start = -0.1
curve_stop = 0.1
soll = init_soll
while True:
# meassure time for debbuging
loop_start = time()
print('State:', state)
parser = argparse.ArgumentParser()
parser.add_argument("-s", "--stand", action="store_true",
help="let the robot stand up")
parser.add_argument("-k", "--kick", action="store_true",
help="let the robot do a fancy kick")
parser.add_argument("-r", "--rest", action="store_true",
help="let the robot rest")
if state == 'tracking':
# start ball approach when ball is visible
print('Soll angle')
striker.ball_tracking(tol=0.05)
state = 'ball_approach'
args = parser.parse_args()
elif state == 'ball_approach':
# bil = striker.get_ball_angles_from_camera(
# striker.lower_camera
# ) # Ball in lower
if args.stand:
striker.mover.stand_up()
striker.close()
# print('Ball in lower', bil)
print('Continue running')
striker.run_to_ball()
# state = 'tracking'
elif args.rest:
striker.mover.rest()
striker.close()
elif state == 'goal_align':
try:
if striker.align_to_goal():
striker.speak('I am aligning to ball')
state = "align"
except ValueError:
state = 'tracking'
# perform a (fancy) kick
elif args.kick:
striker.mover.stand_up()
striker.mover.kick(fancy=True)
striker.close()
# perform normal state-machine if no input argument is given
# (see diagram above)
else:
try: # Hit Ctrl-C to stop, cleanup and exit
state = 'tracking'
# init_soll = 0.0
# align_start = 0.15
# curve_start = -0.1
# curve_stop = 0.1
# soll = init_soll
while True:
# meassure time for debbuging
loop_start = time()
print('State:', state)
if state == 'tracking':
# start ball approach when ball is visible
print('Soll angle')
striker.ball_tracking(tol=0.05)
state = 'ball_approach'
elif state == 'ball_approach':
# bil = striker.get_ball_angles_from_camera(
# striker.lower_camera
# ) # Ball in lower
# print('Ball in lower', bil)
print('Continue running')
striker.run_to_ball()
# state = 'tracking'
elif state == 'goal_align':
try:
if striker.align_to_goal():
striker.speak('I am aligning to ball')
state = "align"
except ValueError:
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:
striker.speak('I am going. To kick ass')
state = 'kick'
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
# state = 'tracking'
elif state == 'kick':
print('KICK!')
striker.mover.stand_up()
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)
striker.mover.kick(fancy=True, foot='L')
break
if success:
striker.speak('I am going. To kick ass')
state = 'kick'
except ValueError:
pass
# striker.mover.set_head_angles(0, 0, 0.3)
# state = 'tracking'
loop_end = time()
print('Loop time:', loop_end - loop_start)
print('\n\n')
finally:
striker.close()
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
# state = 'tracking'
elif state == 'kick':
print('KICK!')
striker.mover.stand_up()
sleep(0.3)
striker.mover.kick(fancy=True, foot='L')
break
loop_end = time()
print('Loop time:', loop_end - loop_start)
print('\n\n')
finally:
striker.close()
striker.mover.rest()