From 5bd50de06eb4826f7456c18a159821c10c3c3bf5 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Sun, 3 Jun 2018 16:08:39 +0200 Subject: [PATCH] From now on use `NaoMover` from `motion.py` Also use `read_config` from `utils.py` --- scripts/ball_approach.py | 87 ++++++ scripts/ball_hsv.json | 8 - scripts/colorpicker.py | 6 +- scripts/detection_demo.py | 7 +- scripts/imagereaders.py | 5 +- scripts/live_recognition_with_head.py | 106 ------- .../live_recognition_with_head_with_body.py | 265 ------------------ scripts/motion.py | 85 ++++++ scripts/motion_setter.py | 103 +------ scripts/move_robot.py | 137 ++------- scripts/photo_capture.py | 8 +- scripts/setangles.py | 49 +--- scripts/utils.py | 7 + 13 files changed, 229 insertions(+), 644 deletions(-) create mode 100644 scripts/ball_approach.py delete mode 100644 scripts/ball_hsv.json delete mode 100644 scripts/live_recognition_with_head.py delete mode 100644 scripts/live_recognition_with_head_with_body.py create mode 100644 scripts/motion.py create mode 100644 scripts/utils.py diff --git a/scripts/ball_approach.py b/scripts/ball_approach.py new file mode 100644 index 0000000..969d1ce --- /dev/null +++ b/scripts/ball_approach.py @@ -0,0 +1,87 @@ +from __future__ import print_function +from __future__ import division + +from utils import read_config + +from imagereaders import NaoImageReader +from finders import BallFinder +from motion import NaoMover + + +class BallFollower(object): + + def __init__(self, nao_ip, nao_port, res, hsv_lower, hsv_upper, + min_radius): + self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port) + self.mover.stand_up() + self.video_top = NaoImageReader(nao_ip, port=nao_port, res=res, + fps=30, cam_id=0) + self.video_bot = NaoImageReader(nao_ip, port=nao_port, res=res, + fps=30, cam_id=0) + self.finder = BallFinder(hsv_lower, hsv_upper, min_radius, None) + self.counter = 0 + + def update(self): + try: + (x, y), radius = self.finder.find_colored_ball( + self.video_top.get_frame() + ) + x, y = self.video_top.to_relative(x, y) + print('Top camera') + except TypeError: + try: + (x, y), radius = self.finder.find_colored_ball( + self.video_bot.get_frame() + ) + x, y = self.video_bot.to_relative(x, y) + print('Low camera') + except TypeError: + return + print(x, y) + self.process_coordinates(x, y) + + def process_coordinates(self, x, y): + x_diff = x - 0.5 + y_diff = y - 0.5 + print("x_diff: " + str(x_diff)) + print("y_diff: " + str(y_diff)) + + d_yaw, d_pitch = -x_diff / 2, 0 + + if abs(x_diff) > 0.1: + self.mover.change_head_angles(d_yaw, d_pitch, 0.3) + self.counter = 0 + else: + self.counter += 1 + print(self.counter) + if self.counter == 4: + self.counter = 0 + print('Going to rotate') + head_angle = self.mover.get_head_angles() + if abs(head_angle[0]) > 0.1: + self.mover.set_head_angles(0, 0, 0.05) + self.mover.move_to(0, 0, head_angle[0]) + self.mover.wait() + self.mover.move_to(0.3, 0, 0) + + def close(self): + self.video_top.close() + self.video_bot.close() + + +if __name__ == '__main__': + + cfg = read_config() + follower = BallFollower( + nao_ip=cfg['ip'], + nao_port=cfg['port'], + res=cfg['res'], + hsv_lower=tuple(map(cfg.get, ('low_h', 'low_s', 'low_v'))), + hsv_upper=tuple(map(cfg.get, ('high_h', 'high_s', 'high_v'))), + min_radius=cfg['min_radius'] + ) + try: + while True: + follower.update() + finally: + follower.close() diff --git a/scripts/ball_hsv.json b/scripts/ball_hsv.json deleted file mode 100644 index 77165ff..0000000 --- a/scripts/ball_hsv.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "low_s": 175, - "low_v": 100, - "high_h": 6, - "high_v": 255, - "low_h": 0, - "high_s": 255 -} \ No newline at end of file diff --git a/scripts/colorpicker.py b/scripts/colorpicker.py index e7ff280..e0bada5 100644 --- a/scripts/colorpicker.py +++ b/scripts/colorpicker.py @@ -5,7 +5,7 @@ import json import argparse import cv2 from imagereaders import VideoReader, NaoImageReader, PictureReader -# import imutils +from utils import read_config class Colorpicker(object): @@ -86,9 +86,7 @@ class Colorpicker(object): if __name__ == '__main__': - with open('nao_defaults.json') as f: - defaults = json.load(f) - + defaults = read_config() parser = argparse.ArgumentParser( epilog='When called without arguments specifying the video source, ' + 'will try to use the webcam' diff --git a/scripts/detection_demo.py b/scripts/detection_demo.py index bc2c940..695ee5d 100644 --- a/scripts/detection_demo.py +++ b/scripts/detection_demo.py @@ -1,18 +1,17 @@ from __future__ import print_function from __future__ import division -import json +from utils import read_config from imagereaders import NaoImageReader, VideoReader from finders import BallFinder if __name__ == '__main__': video = VideoReader(0, loop=True) - with open('ball_hsv.json') as f: - hsv = json.load(f) + hsv = read_config() hsv_lower = tuple(map(hsv.get, ('low_h', 'low_s', 'low_v'))) hsv_upper = tuple(map(hsv.get, ('high_h', 'high_s', 'high_v'))) - finder = BallFinder(hsv_lower, hsv_upper, 5, None) + finder = BallFinder(hsv_lower, hsv_upper, hsv['min_radius'], None) try: while True: frame = video.get_frame() diff --git a/scripts/imagereaders.py b/scripts/imagereaders.py index 8ca0941..70af785 100644 --- a/scripts/imagereaders.py +++ b/scripts/imagereaders.py @@ -2,10 +2,7 @@ from __future__ import division import numpy as np import cv2 -try: - from naoqi import ALProxy -except: - ALProxy = None +from naoqi import ALProxy class NaoImageReader(object): diff --git a/scripts/live_recognition_with_head.py b/scripts/live_recognition_with_head.py deleted file mode 100644 index b3322a2..0000000 --- a/scripts/live_recognition_with_head.py +++ /dev/null @@ -1,106 +0,0 @@ -from __future__ import print_function -from __future__ import division - -#import imutils -from naoqi import ALProxy -from imagereaders import NaoImageReader -from live_recognition import BallFinder -from live_recognition_with_head_with_body import move_to - - -# Nao configuration -nao_ip = '192.168.0.11' -nao_port = 9559 -#res = (3, (960, 1280)) # NAOQi code and acutal resolution -#res=(1,(240,320)) -res=1 -#res=(2,(480,640)) - -fps = 30 -cam_id = 0 # 0 := top, 1 := bottom - -# Recognition stuff -red_lower = (0, 185, 170) # HSV coded red interval -red_upper = (6, 255, 255) -min_radius = 5 -resized_width = None # Maybe we need it maybe don't (None if don't) - -current_value = 0 -counter = 0 - -def get_angle(): - robotIP="192.168.0.11" - PORT = 9559 - motionProxy = ALProxy("ALMotion", robotIP, PORT) - names=["HeadPitch","HeadYaw"] - useSensors=False - angle=motionProxy.getAngles(names,useSensors) - #print("angle_is"+str(angles)) - return angle - -def set_angle_new(x,y): - angle=get_angle() - robotIP="192.168.0.11" - PORT = 9559 - motionProxy = ALProxy("ALMotion", robotIP, PORT) - - # activiert gelenke - motionProxy.setStiffnesses("Head", 0.5) - names = ["HeadYaw", "HeadPitch"] - fractionMaxSpeed = 0.3 - x_diff=x-0.5 - y_diff=y-0.5 - print("x_diff="+str(x_diff)) - print("y_diff="+str(y_diff)) - - angles=[-x_diff / 2, 0] - - global counter - if abs(x_diff) > 0.1: - motionProxy.changeAngles(names, angles, fractionMaxSpeed) - counter = 0 - else: - counter += 1 - print(counter) - if counter == 4: - counter = 0 - print('Going to rotate') - angle = get_angle() - if abs(angle[1]) > 0.1: - motionProxy.setStiffnesses("Head", 0.7) - names = ["HeadYaw", "HeadPitch"] - fractionMaxSpeed = 0.05 - angles=[0, 0] - motionProxy.setAngles(names,angles,fractionMaxSpeed) - move_to(0, 0, angle[1]) - move_to(0.3, 0, 0) - - -if __name__ == '__main__': - video_top = NaoImageReader(nao_ip, port=nao_port, cam_id=0, res=res, - fps=fps) - video_low = NaoImageReader(nao_ip, port=nao_port, cam_id=1, res=res, - fps=fps) - finder = BallFinder(red_lower, red_upper, 5, None) - finder.load_hsv_config('ball_hsv.json') - try: - while True: - try: - (x, y), radius = finder.find_colored_ball(video_top.get_frame()) - x, y = video_top.to_relative(x, y) - print('Top camera') - except TypeError: - try: - (x, y), radius = finder.find_colored_ball( - video_low.get_frame() - ) - x, y = video_low.to_relative(x, y) - print('Low camera') - except TypeError: - continue - print(x, y) - print(x, y) - set_angle_new(x,y) - finally: - video_top.close() - video_low.close() diff --git a/scripts/live_recognition_with_head_with_body.py b/scripts/live_recognition_with_head_with_body.py deleted file mode 100644 index 8cc6473..0000000 --- a/scripts/live_recognition_with_head_with_body.py +++ /dev/null @@ -1,265 +0,0 @@ -from __future__ import print_function -from __future__ import division - -import cv2 -import numpy as np -#import imutils -from naoqi import ALProxy -from collections import deque -from imagereaders import NaoImageReader -from live_recognition import BallFinder -import time -import almath - -# Nao configuration -nao_ip = '192.168.0.11' -nao_port = 9559 -#res = (3, (960, 1280)) # NAOQi code and acutal resolution -#res=(1,(240,320)) -res=1 -#res=(2,(480,640)) - -fps = 30 -cam_id = 1 # 0 := top, 1 := bottom - -# Recognition stuff -red_lower = (0, 185, 170) # HSV coded red interval -red_upper = (6, 255, 255) -min_radius = 5 -resized_width = None # Maybe we need it maybe don't (None if don't) - -current_value = 0 -global counter -counter=0 - -def move_to(front, side, angle): - motionProxy = ALProxy("ALMotion", nao_ip, nao_port) - postureProxy = ALProxy("ALRobotPosture", nao_ip, nao_port) - - # Wake up robot - motionProxy.wakeUp() - - handStiffness=0.0; - # shoulder and elbow - armStiffness = 0.0; - # head - headStiffness = 0.9; - # hip - hipStiffness = 0.9; - # knee - kneeStiffness = 0.9; - # ankle - ankleStiffness = 0.9; - - - # set the stiffnes - motionProxy.setStiffnesses("Head", headStiffness) - motionProxy.setStiffnesses("RHand", handStiffness) - motionProxy.setStiffnesses("LHand", handStiffness) - motionProxy.setStiffnesses("LShoulderPitch", armStiffness) - motionProxy.setStiffnesses("LShoulderRoll", armStiffness) - motionProxy.setStiffnesses("LElbowRoll", armStiffness) - motionProxy.setStiffnesses("LWristYaw", handStiffness) - motionProxy.setStiffnesses("LElbowYaw", armStiffness) - motionProxy.setStiffnesses("LHipYawPitch", hipStiffness) - motionProxy.setStiffnesses("LHipPitch", hipStiffness) - motionProxy.setStiffnesses("LKneePitch", kneeStiffness) - motionProxy.setStiffnesses("LAnklePitch", ankleStiffness) - motionProxy.setStiffnesses("LHipRoll", hipStiffness) - motionProxy.setStiffnesses("LAnkleRoll", ankleStiffness) - - motionProxy.setStiffnesses("RShoulderPitch", armStiffness) - motionProxy.setStiffnesses("RShoulderRoll", armStiffness) - motionProxy.setStiffnesses("RElbowRoll", armStiffness) - motionProxy.setStiffnesses("RWristYaw", handStiffness) - motionProxy.setStiffnesses("RElbowYaw", armStiffness) - motionProxy.setStiffnesses("RHipYawPitch", hipStiffness) - motionProxy.setStiffnesses("RHipPitch", hipStiffness) - motionProxy.setStiffnesses("RKneePitch", kneeStiffness) - motionProxy.setStiffnesses("RAnklePitch", ankleStiffness) - motionProxy.setStiffnesses("RHipRoll", hipStiffness) - motionProxy.setStiffnesses("RAnkleRoll", ankleStiffness) - - - # Send robot to Stand Init - #postureProxy.goToPosture("StandInit", 0.5) - - # Initialize the move - motionProxy.moveInit() - - # First call of move API - # with post prefix to not be bloquing here. - # motionProxy.post.moveTo(0, 0.0, 0) - - # wait that the move process start running - # time.sleep(0.1) - - # get robotPosition and nextRobotPosition - # useSensors = False - # robotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensors)) - # nextRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition()) - - # get the first foot steps vector - # (footPosition, unChangeable and changeable steps) - - # footSteps1 = [] - #try: - # footSteps1 = motionProxy.getFootSteps() - #except Exception, errorMsg: - # print str(errorMsg) - # PLOT_ALLOW = False - - # Second call of move API - motionProxy.post.moveTo(front, side, angle) - # motionProxy.waitUntilMoveIsFinished() - - # get the second foot steps vector - # footSteps2 = [] - #try: - # footSteps2 = motionProxy.getFootSteps() - #except Exception, errorMsg: - #print str(errorMsg) - #PLOT_ALLOW = False - - -def get_angle(): - robotIP="192.168.0.11" - PORT = 9559 - motionProxy = ALProxy("ALMotion", robotIP, PORT) - names=["HeadPitch","HeadYaw"] - useSensors=False - angle=motionProxy.getAngles(names,useSensors) - #print("angle_is"+str(angles)) - return angle - -def set_angle_new(x,y): - angle=get_angle() - robotIP="192.168.0.11" - PORT = 9559 - motionProxy = ALProxy("ALMotion", robotIP, PORT) - - # activiert gelenke - motionProxy.setStiffnesses("Head", 0.5) - names = ["HeadYaw", "HeadPitch"] - fractionMaxSpeed = 0.3 - #x_mid=160 - #y_mid=120 - #x_diff=x-x_mid - #y_diff=y-y_mid - #print("x_diff="+str(x_diff)) - #print("y_diff="+str(y_diff)) - - videoProxy=ALProxy('ALVideoDevice', robotIP, PORT) - ball_angles=videoProxy.getAngularPositionFromImagePosition(1,[x/320,y/240]) - #print(ball_angles) - ball_angle_x=ball_angles[0] - ball_angle_y=ball_angles[1] - #print("ball_angle_x="+str(ball_angle_x)) - #print("ball_angle_y="+str(ball_angle_y)) - - #ball_angle_x_diff=ball_angle_x+ - #ball_angle_y_diff=ball_angle_y-99 - #print(ball_angle_x_diff*3.14/180) - #print(ball_angle_y_diff) - -# print(ball_angles-[-169,99]) - #[-169.53343200683594, 99.27782440185547] (x_mid,y_mid) - #if abs(ball_angle_x)>0.2 and abs(ball_angle_y)>0.01: - #angles=[ball_angle_x,0] - - #angles=[0.25*ball_angle_x,0.25*ball_angle_y] - angles=[0.25*ball_angle_x,0] - if abs(ball_angle_x)>0.1: - motionProxy.changeAngles(names, angles, fractionMaxSpeed) - - # if the head has the wright x coordinates - elif abs(ball_angle_x)<0.1: - #tts = ALProxy("ALTextToSpeech", "192.168.0.11", 9559) - #tts.setParameter("pitchShift", 1) - #tts.setParameter("speed", 50) - #tts.setVoice("Kenny22Enhanced") - - #tts.say("Ball found") - global counter - counter=counter+1 - #print(counter) - #print("Ball found") - if counter==5: - global counter - counter=0 - print(get_angle()) - a=get_angle() - if abs(a[1])>0.1: - move_to(a[0],a[1]) - - -def set_angle(direction): -#def main(robotIP,x,y): - robotIP="192.168.0.11" - PORT = 9559 - motionProxy = ALProxy("ALMotion", robotIP, PORT) - # activiert gelenke - motionProxy.setStiffnesses("Head", 1.0) - - - #names = "HeadYaw" - #useSensors = False - - #commandAngles = motionProxy.getAngles(names, useSensors) - - #type(commandAngles) - #type(float(commandAngles)) - #current_angle=float(commandAngles) - #print(current_angle) - #next_angle=float(commandAngles)-0.2 - #print("next_angle"+str(next_angle)) - #angles = [0,next_angle] - - - - #print("set_angle") - # Example showing how to set angles, using a fraction of max speed - names = ["HeadYaw", "HeadPitch"] - #global current_value - a=get_angle() - #print(a[0]) -# print(a) - - - #current_value=current_value-0.2 - if direction=="up": - angles = [a[1],a[0]-0.2] - elif direction=="down": - angles = [a[1], a[0]+0.2] - elif direction=="right": - angles= [a[1]-0.2,a[0]] - elif direction=="left": - angles=[a[1]+0.2,a[0]] - fractionMaxSpeed = 0.5 - - motionProxy.setAngles(names, angles, fractionMaxSpeed) - - -if __name__ == '__main__': - video = NaoImageReader(nao_ip, port=nao_port, cam_id=cam_id, res=res, - fps=fps) - finder = BallFinder(red_lower, red_upper, 5, None) - try: - while True: - try: - (x, y), radius = finder.find_colored_ball(video.get_frame()) - except TypeError: - continue - set_angle_new(x,y) - ''' - if 0y>200: - set_angle("down") - if 0x>220: - set_angle("right") - ''' - finally: - video.close() diff --git a/scripts/motion.py b/scripts/motion.py new file mode 100644 index 0000000..d300bb1 --- /dev/null +++ b/scripts/motion.py @@ -0,0 +1,85 @@ +from naoqi import ALProxy + + +class NaoMover(object): + + def __init__(self, nao_ip, nao_port=9559): + self.mp = ALProxy('ALMotion', nao_ip, nao_port) + self.pp = ALProxy('ALRobotPosture', nao_ip, nao_port) + ap = ALProxy("ALAutonomousLife", nao_ip, nao_port) + ap.setState('disabled') + self.set_head_stiffness() + self.set_hand_stiffness() + self.set_arm_stiffness() + self.set_hip_stiffness() + self.set_knee_stiffness() + self.set_ankle_stiffness() + self.ready_to_move = False + + def stand_up(self): + self.set_arm_stiffness(0.9) + self.set_hand_stiffness(0.9) + self.pp.goToPosture('StandInit', 1.0) + self.set_hand_stiffness() + self.set_arm_stiffness() + + def rest(self): + self.mp.rest() + self.ready_to_move = False + + def set_head_stiffness(self, stiffness=0.8): + self.mp.setStiffnesses("Head", stiffness) + + def set_hand_stiffness(self, stiffness=0.0): + self.mp.setStiffnesses("RHand", stiffness) + self.mp.setStiffnesses("LHand", stiffness) + self.mp.setStiffnesses("LWristYaw", stiffness) + self.mp.setStiffnesses("RWristYaw", stiffness) + + def set_arm_stiffness(self, stiffness=0.3): + self.mp.setStiffnesses("LShoulderPitch", stiffness) + self.mp.setStiffnesses("LShoulderRoll", stiffness) + self.mp.setStiffnesses("LElbowRoll", stiffness) + self.mp.setStiffnesses("LElbowYaw", stiffness) + self.mp.setStiffnesses("RShoulderPitch", stiffness) + self.mp.setStiffnesses("RShoulderRoll", stiffness) + self.mp.setStiffnesses("RElbowRoll", stiffness) + self.mp.setStiffnesses("RElbowYaw", stiffness) + + def set_hip_stiffness(self, stiffness=0.9): + self.mp.setStiffnesses("LHipYawPitch", stiffness) + self.mp.setStiffnesses("LHipPitch", stiffness) + self.mp.setStiffnesses("RHipYawPitch", stiffness) + self.mp.setStiffnesses("RHipPitch", stiffness) + self.mp.setStiffnesses("LHipRoll", stiffness) + self.mp.setStiffnesses("RHipRoll", stiffness) + + def set_ankle_stiffness(self, stiffness=0.9): + self.mp.setStiffnesses("LAnklePitch", stiffness) + self.mp.setStiffnesses("LAnkleRoll", stiffness) + self.mp.setStiffnesses("RAnklePitch", stiffness) + self.mp.setStiffnesses("RAnkleRoll", stiffness) + + def set_knee_stiffness(self, stiffness=0.9): + self.mp.setStiffnesses("LKneePitch", stiffness) + self.mp.setStiffnesses("RKneePitch", stiffness) + + def get_head_angles(self): + return self.mp.getAngles(('HeadYaw', 'HeadPitch'), useSensors=False) + + def change_head_angles(self, d_yaw, d_pitch, speed): + self.mp.changeAngles(('HeadYaw', 'HeadPitch'), + (d_yaw, d_pitch), speed) + + def set_head_angles(self, yaw, pitch, speed): + self.mp.setHeadAngles(('HeadYaw', 'HeadPitch'), + (yaw, pitch), speed) + + def move_to(self, front, side, rotation, wait=False): + if not self.ready_to_move: + self.mp.moveInit() + self.ready_to_move = True + self.mp.post.moveTo(front, side, rotation) + + def wait(self): + self.mp.waitUntilMoveIsFinished() diff --git a/scripts/motion_setter.py b/scripts/motion_setter.py index dde15c9..a702f58 100644 --- a/scripts/motion_setter.py +++ b/scripts/motion_setter.py @@ -1,95 +1,14 @@ -# -*- encoding: UTF-8 -*- - -''' PoseInit: Small example to make Nao go to an initial position. ''' -# syntax: python motion_setter.py 0 -> Robot stands up -# python motion_setter.py 1 -> Robot goes back to rest position - import argparse -from naoqi import ALProxy -import sys - -def main(robotIP, PORT,rest): -#def main(reset): - - motionProxy = ALProxy("ALMotion", robotIP, PORT) - postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) - AutonomousLifeProxy = ALProxy("ALAutonomousLife", robotIP,PORT) - - # Disable autonomous life - AutonomousLifeProxy.setState("disabled") - - # Wake up robot - # motionProxy.wakeUp() - - # stiffnessses - # hand and wrist - handStiffness = 0.0; - # shoulder and elbow - armStiffness = 0.0; - # head - headStiffness = 0.9; - # hip - hipStiffness = 0.9; - # knee - kneeStiffness = 0.9; - # ankle - ankleStiffness = 0.9; - - - # set the stiffnes - motionProxy.setStiffnesses("Head", headStiffness) - motionProxy.setStiffnesses("RHand", handStiffness) - motionProxy.setStiffnesses("LHand", handStiffness) - motionProxy.setStiffnesses("LShoulderPitch", armStiffness) - motionProxy.setStiffnesses("LShoulderRoll", armStiffness) - motionProxy.setStiffnesses("LElbowRoll", armStiffness) - motionProxy.setStiffnesses("LWristYaw", handStiffness) - motionProxy.setStiffnesses("LElbowYaw", armStiffness) - motionProxy.setStiffnesses("LHipYawPitch", hipStiffness) - motionProxy.setStiffnesses("LHipPitch", hipStiffness) - motionProxy.setStiffnesses("LKneePitch", kneeStiffness) - motionProxy.setStiffnesses("LAnklePitch", ankleStiffness) - motionProxy.setStiffnesses("LHipRoll", hipStiffness) - motionProxy.setStiffnesses("LAnkleRoll", ankleStiffness) - - motionProxy.setStiffnesses("RShoulderPitch", armStiffness) - motionProxy.setStiffnesses("RShoulderRoll", armStiffness) - motionProxy.setStiffnesses("RElbowRoll", armStiffness) - motionProxy.setStiffnesses("RWristYaw", handStiffness) - motionProxy.setStiffnesses("RElbowYaw", armStiffness) - motionProxy.setStiffnesses("RHipYawPitch", hipStiffness) - motionProxy.setStiffnesses("RHipPitch", hipStiffness) - motionProxy.setStiffnesses("RKneePitch", kneeStiffness) - motionProxy.setStiffnesses("RAnklePitch", ankleStiffness) - motionProxy.setStiffnesses("RHipRoll", hipStiffness) - motionProxy.setStiffnesses("RAnkleRoll", ankleStiffness) - - # Send robot to Stand Init - #postureProxy.goToPosture("StandInit", 0.5) - - # Go to rest position - #print("") - #if reset==1: - # motionProxy.rest() - rest=int(rest) - if rest==1: - motionProxy.rest() - else: - postureProxy.goToPosture("StandInit", 0.5) - - - +from motion import NaoMover +from utils import read_config if __name__ == "__main__": - #parser = argparse.ArgumentParser() - #parser.add_argument("--ip", type=str, default="192.168.0.11", - # help="Robot ip address") - #parser.add_argument("--port", type=int, default=9559, - # help="Robot port number") - - print(sys.argv[1]) - rest=sys.argv[1] - #args = parser.parse_args() -# main(args.ip, args.port,reset) - - main("192.168.0.11",9559,rest) + parser = argparse.ArgumentParser() + parser.add_argument('state', choices=('stand', 'rest')) + args = parser.parse_args() + cfg = read_config() + mover = NaoMover(cfg['ip'], cfg['port']) + if args.state == 'stand': + mover.stand_up() + elif args.state == 'rest': + mover.rest() diff --git a/scripts/move_robot.py b/scripts/move_robot.py index a1f5600..78f4115 100644 --- a/scripts/move_robot.py +++ b/scripts/move_robot.py @@ -1,124 +1,19 @@ -# -*- encoding: UTF-8 -*- +from motion import NaoMover +from utils import read_config -''' PoseInit: Small example to make Nao go to an initial position. ''' - -import argparse -from naoqi import ALProxy -import time -import math -import almath -#include - - -def main(robotIP, PORT=9559): - - motionProxy = ALProxy("ALMotion", robotIP, PORT) - postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) - - # Wake up robot - motionProxy.wakeUp() - - handStiffness=0.0; - # shoulder and elbow - armStiffness = 0.0; - # head - headStiffness = 0.9; - # hip - hipStiffness = 0.9; - # knee - kneeStiffness = 0.9; - # ankle - ankleStiffness = 0.9; - - - # set the stiffnes - motionProxy.setStiffnesses("Head", headStiffness) - motionProxy.setStiffnesses("RHand", handStiffness) - motionProxy.setStiffnesses("LHand", handStiffness) - motionProxy.setStiffnesses("LShoulderPitch", armStiffness) - motionProxy.setStiffnesses("LShoulderRoll", armStiffness) - motionProxy.setStiffnesses("LElbowRoll", armStiffness) - motionProxy.setStiffnesses("LWristYaw", handStiffness) - motionProxy.setStiffnesses("LElbowYaw", armStiffness) - motionProxy.setStiffnesses("LHipYawPitch", hipStiffness) - motionProxy.setStiffnesses("LHipPitch", hipStiffness) - motionProxy.setStiffnesses("LKneePitch", kneeStiffness) - motionProxy.setStiffnesses("LAnklePitch", ankleStiffness) - motionProxy.setStiffnesses("LHipRoll", hipStiffness) - motionProxy.setStiffnesses("LAnkleRoll", ankleStiffness) - - motionProxy.setStiffnesses("RShoulderPitch", armStiffness) - motionProxy.setStiffnesses("RShoulderRoll", armStiffness) - motionProxy.setStiffnesses("RElbowRoll", armStiffness) - motionProxy.setStiffnesses("RWristYaw", handStiffness) - motionProxy.setStiffnesses("RElbowYaw", armStiffness) - motionProxy.setStiffnesses("RHipYawPitch", hipStiffness) - motionProxy.setStiffnesses("RHipPitch", hipStiffness) - motionProxy.setStiffnesses("RKneePitch", kneeStiffness) - motionProxy.setStiffnesses("RAnklePitch", ankleStiffness) - motionProxy.setStiffnesses("RHipRoll", hipStiffness) - motionProxy.setStiffnesses("RAnkleRoll", ankleStiffness) - - - # Send robot to Stand Init - postureProxy.goToPosture("StandInit", 0.5) - - # Initialize the move - motionProxy.moveInit() - - # First call of move API - # with post prefix to not be bloquing here. - motionProxy.post.moveTo(0, 0.0, 0) - - while True: - ax = int(raw_input("Axis: ")) - mag = float(raw_input('How much: ')) - - if ax: - motionProxy.post.moveTo(0, mag, 0) - motionProxy.waitUntilMoveIsFinished() - else: - motionProxy.post.moveTo(mag, 0, 0) - - ''' - # Second call of move API - motionProxy.post.moveTo(1.5, 0, 0) - - # here we wait until the move process is over - motionProxy.waitUntilMoveIsFinished() - - - # Second call of move API - motionProxy.post.moveTo(0, -1.5, 0) - - # here we wait until the move process is over - motionProxy.waitUntilMoveIsFinished() - - # Second call of move API - motionProxy.post.moveTo(-1.5, 0, 0) - - # here we wait until the move process is over - motionProxy.waitUntilMoveIsFinished() - ''' - - # Second call of move API -# motionProxy.post.moveTo(0, 3, 0) - - # here we wait until the move process is over - # motionProxy.waitUntilMoveIsFinished() - - - - - # Go to rest position - #motionProxy.rest() if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("--ip", type=str, default="192.168.0.11", - help="Robot ip address") - parser.add_argument("--port", type=int, default=9559, - help="Robot port number") - - args = parser.parse_args() - main(args.ip, args.port) + cfg = read_config() + mover = NaoMover(cfg['ip'], cfg['port']) + mover.stand_up() + while True: + axis = int(raw_input('Axis: ')) + 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)') diff --git a/scripts/photo_capture.py b/scripts/photo_capture.py index 4f06fe9..02e99df 100644 --- a/scripts/photo_capture.py +++ b/scripts/photo_capture.py @@ -1,10 +1,12 @@ +import argparse import cv2 from datetime import datetime +from utils import read_config from imagereaders import NaoImageReader -import argparse if __name__ == '__main__': + cfg = read_config() parser = argparse.ArgumentParser() parser.add_argument('--res', type=int, choices=(1, 2, 3), default=3) @@ -12,11 +14,11 @@ if __name__ == '__main__': default=0) args = parser.parse_args() - video = NaoImageReader('192.168.0.11', res=args.res, cam_id=args.cam_id, + video = NaoImageReader(cfg['ip'], res=args.res, cam_id=args.cam_id, fps=1) frame = video.get_frame() video.close() now = datetime.now().strftime('%Y%m%d%H%M%S') - prefix = 'bottom' if args.cam_id else 'bottom' + prefix = 'bottom' if args.cam_id else 'top' cv2.imwrite(prefix + now + '.jpg', frame) diff --git a/scripts/setangles.py b/scripts/setangles.py index 8201dae..3b822fc 100644 --- a/scripts/setangles.py +++ b/scripts/setangles.py @@ -1,41 +1,16 @@ -# -*- encoding: UTF-8 -*- -# syntax -# python setangles.py x y - -import sys -from naoqi import ALProxy -import time - -def main(robotIP,x,y): - PORT = 9559 - - try: - motionProxy = ALProxy("ALMotion", robotIP, PORT) - except Exception,e: - print "Could not create proxy to ALMotion" - print "Error was: ",e - sys.exit(1) - - # activiert gelenke - motionProxy.setStiffnesses("Head", 1.0) - - # Example showing how to set angles, using a fraction of max speed - names = ["HeadYaw", "HeadPitch"] - angles = [x,y] - fractionMaxSpeed = 0.01 - print motionProxy.setAngles(names, angles, fractionMaxSpeed) - - #time.sleep(3.0) - #print motionProxy.setStiffnesses("Head", 0.0) +import argparse +from motion import NaoMover +from utils import read_config if __name__ == "__main__": - robotIp = "192.168.0.11" - #if len(sys.argv) <= 1: - # print "Usage python almotion_setangles.py robotIP (optional default: 127.0.0.1)" - #else: - # robotIp = sys.argv[1] - x=float(sys.argv[1]) - y=float(sys.argv[2]) - main(robotIp,x,y) + cfg = read_config() + parser = argparse.ArgumentParser() + parser.add_argument('yaw', type=float) + parser.add_argument('pitch', type=float) + parser.add_argument('speed', type=float, nargs='?', default=0.1) + args = parser.parse_args() + + mover = NaoMover(cfg['ip'], cfg['port']) + mover.set_head_angles(args.yaw, args.pitch, args.speed) diff --git a/scripts/utils.py b/scripts/utils.py new file mode 100644 index 0000000..1309631 --- /dev/null +++ b/scripts/utils.py @@ -0,0 +1,7 @@ +import json + + +def read_config(): + with open('nao_defaults.json') as f: + cfg = json.load(f) + return cfg