From now on use NaoMover from motion.py

Also use `read_config` from `utils.py`

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()

scripts/ball_hsv.json

@@ -1,8 +0,0 @@
-{
-    "low_s": 175, 
-    "low_v": 100, 
-    "high_h": 6, 
-    "high_v": 255, 
-    "low_h": 0, 
-    "high_s": 255
-}

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 = read_config()
-        defaults = json.load(f)
-
     parser = argparse.ArgumentParser(
         epilog='When called without arguments specifying the video source, ' +
         'will try to use the webcam'

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 = read_config()
-        hsv = json.load(f)
     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()

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
 
 
 class NaoImageReader(object):

scripts/live_recognition_with_head.py

@@ -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()

scripts/live_recognition_with_head_with_body.py

@@ -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 0<y<100:
-                set_angle("up")
-            elif 240>y>200:
-                set_angle("down")
-            if 0<x<100:  # maybe do simultaneous x-y setting
-                set_angle("left")
-            elif 320>x>220:
-                set_angle("right")
-  	    '''
-    finally:
-        video.close()

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()

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
+from motion import NaoMover
-import sys
+from utils import read_config
-
-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)
-
-    
-	    
 
 if __name__ == "__main__":
-    #parser = argparse.ArgumentParser()
+    parser = argparse.ArgumentParser()
-    #parser.add_argument("--ip", type=str, default="192.168.0.11",
+    parser.add_argument('state', choices=('stand', 'rest'))
-    #                    help="Robot ip address")
+    args = parser.parse_args()
-    #parser.add_argument("--port", type=int, default=9559,
+    cfg = read_config()
-    #                    help="Robot port number")
+    mover = NaoMover(cfg['ip'], cfg['port'])
-
+    if args.state == 'stand':
-    print(sys.argv[1])
+        mover.stand_up()
-    rest=sys.argv[1]
+    elif args.state == 'rest':
-    #args = parser.parse_args()
+        mover.rest()
-#    main(args.ip, args.port,reset)
-    
-    main("192.168.0.11",9559,rest)

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 <alproxies/alnavigationproxy.h>
-
-
-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()
+    cfg = read_config()
-    parser.add_argument("--ip", type=str, default="192.168.0.11",
+    mover = NaoMover(cfg['ip'], cfg['port'])
-                        help="Robot ip address")
+    mover.stand_up()
-    parser.add_argument("--port", type=int, default=9559,
+    while True:
-                        help="Robot port number")
+        axis = int(raw_input('Axis: '))
-
+        amount = float(raw_input('How much: '))
-    args = parser.parse_args()
+        if axis == 0:
-    main(args.ip, args.port)
+            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)')

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)

scripts/setangles.py

@@ -1,41 +1,16 @@
-# -*- encoding: UTF-8 -*-
+import argparse
-# syntax
+from motion import NaoMover
-# python setangles.py x y
+from utils import read_config
-
-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)
 
 
 if __name__ == "__main__":
-    robotIp = "192.168.0.11"
 
-    #if len(sys.argv) <= 1:
+    cfg = read_config()
-    #    print "Usage python almotion_setangles.py robotIP (optional default: 127.0.0.1)"
+    parser = argparse.ArgumentParser()
-    #else:
+    parser.add_argument('yaw', type=float)
-    #    robotIp = sys.argv[1]
+    parser.add_argument('pitch', type=float)
-    x=float(sys.argv[1])
+    parser.add_argument('speed', type=float, nargs='?', default=0.1)
-    y=float(sys.argv[2])
+    args = parser.parse_args()
-    main(robotIp,x,y)
+
+    mover = NaoMover(cfg['ip'], cfg['port'])
+    mover.set_head_angles(args.yaw, args.pitch, args.speed)

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