From now on use NaoMover from motion.py
Also use `read_config` from `utils.py`
This commit is contained in:
87
scripts/ball_approach.py
Normal file
87
scripts/ball_approach.py
Normal file
@@ -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()
|
||||
@@ -1,8 +0,0 @@
|
||||
{
|
||||
"low_s": 175,
|
||||
"low_v": 100,
|
||||
"high_h": 6,
|
||||
"high_v": 255,
|
||||
"low_h": 0,
|
||||
"high_s": 255
|
||||
}
|
||||
@@ -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'
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
85
scripts/motion.py
Normal file
85
scripts/motion.py
Normal file
@@ -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()
|
||||
@@ -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()
|
||||
|
||||
@@ -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()
|
||||
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)')
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
7
scripts/utils.py
Normal file
7
scripts/utils.py
Normal file
@@ -0,0 +1,7 @@
|
||||
import json
|
||||
|
||||
|
||||
def read_config():
|
||||
with open('nao_defaults.json') as f:
|
||||
cfg = json.load(f)
|
||||
return cfg
|
||||
Reference in New Issue
Block a user