def __init__(self):
ip = '127.0.0.1'
port = 19997
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart(ip, port, True, True, -5000, 5)
if self.clientID == -1:
import sys
sys.exit('\nV-REP remote API server connection failed (' + ip +
':' + str(port) + '). Is V-REP running?')
print('Connected to Remote API Server')
with open(
'primitive_base/dance_primitive_library_interpole.json') as f:
self.dance_primitive_library = json.load(f)
self.Body = {}
get_first_handles(
self.clientID,
self.Body) #get first handles of Nao in the virtual environment
self.joint_actuator = Joint_Actuator(self.clientID, self.Body)
self.init_time = time.time()
self.time_sleep = 0.01
self.i = 0
def __init__(self):
self.ip = '127.0.0.1'
self.port = 19997
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart(self.ip, self.port, True, True, -5000, 5)
# Connect to V-REP
if self.clientID == -1:
import sys
sys.exit('\nV-REP remote API server connection failed (' +
self.ip + ':' + str(self.port) + '). Is V-REP running?')
print('Connected to Remote API Server') # show in the terminal
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
self.Body = {}
get_first_handles(self.clientID, self.Body)
def run(self):
print "================ Handles Initialization ================"
Head_Yaw = []
Head_Pitch = []
L_Hip_Yaw_Pitch = []
L_Hip_Roll = []
L_Hip_Pitch = []
L_Knee_Pitch = []
L_Ankle_Pitch = []
L_Ankle_Roll = []
R_Hip_Yaw_Pitch = []
R_Hip_Roll = []
R_Hip_Pitch = []
R_Knee_Pitch = []
R_Ankle_Pitch = []
R_Ankle_Roll = []
L_Shoulder_Pitch = []
L_Shoulder_Roll = []
L_Elbow_Yaw = []
L_Elbow_Roll = []
L_Wrist_Yaw = []
R_Shoulder_Pitch = []
R_Shoulder_Roll = []
R_Elbow_Yaw = []
R_Elbow_Roll = []
R_Wrist_Yaw = []
R_H = []
L_H = []
R_Hand = []
L_Hand = []
Body = [
Head_Yaw, Head_Pitch, L_Hip_Yaw_Pitch, L_Hip_Roll, L_Hip_Pitch,
L_Knee_Pitch, L_Ankle_Pitch, L_Ankle_Roll, R_Hip_Yaw_Pitch,
R_Hip_Roll, R_Hip_Pitch, R_Knee_Pitch, R_Ankle_Pitch, R_Ankle_Roll,
L_Shoulder_Pitch, L_Shoulder_Roll, L_Elbow_Yaw, L_Elbow_Roll,
L_Wrist_Yaw, R_Shoulder_Pitch, R_Shoulder_Roll, R_Elbow_Yaw,
R_Elbow_Roll, R_Wrist_Yaw, L_H, L_Hand, R_H, R_Hand
]
get_first_handles(clientID, Body)
while (vrep.simxGetConnectionId(clientID) != -1):
if self.exitflag.isSet():
print 'End of simulation'
break
JointControl(self.clientID, self.motionProxy, 0, Body)
def motion_actuator(queue_beat, queue_motion_from_selector):
#print('Process to actuate motion: %s' % os.getpid())
ip = '127.0.0.1'
port = 19997
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart(ip, port, True, True, -5000, 5)
# Connect to V-REP
if clientID == -1:
import sys
sys.exit('\nV-REP remote API server connection failed (' + ip + ':' +
str(port) + '). Is V-REP running?')
print('Connected to Remote API Server') # show in the terminal
Body = {}
get_first_handles(
clientID, Body) #get first handles of Nao in the virtual environment
motion_base_dir = 'MyNao\\motion_base\\motion_base.json'
with open(motion_base_dir, 'r') as f:
motion_base = json.load(f)
sc = Speed_Controller_and_Motion_Actuator(alpha=0.9,
motion_base=motion_base,
kp=2e-4,
ki=1e-6,
kd=5e-4) #2e-4, 1e-6, 1e-6
#time_init = time.time()
#time.sleep(5)
while (True):
if not queue_motion_from_selector.empty():
current_motion_idx = queue_motion_from_selector.get()
sc.set_current_motion(current_motion_idx)
current_motion = motion_base[str(current_motion_idx)]['frame']
for frame in range(len(current_motion)):
time_1 = time.time()
joint_actuate(clientID, Body, current_motion, frame)
time_transmit = time.time() - time_1
sc.set_current_frame(frame)
sc.control(queue_beat)
time_sleep = sc.get_time_sleep()
if time_sleep < 0.02:
time.sleep(0.033)
else:
time.sleep(max(0.02, time_sleep - time_transmit))
def __init__(self):
self.ip = '127.0.0.1'
self.port = 19997
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart(self.ip, self.port, True, True, -5000, 5)
# Connect to V-REP
if self.clientID == -1:
import sys
sys.exit('\nV-REP remote API server connection failed (' +
self.ip + ':' + str(self.port) + '). Is V-REP running?')
print('Connected to Remote API Server') # show in the terminal
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
self.Body = {}
get_first_handles(self.clientID, self.Body)
self.loader = inferenceDataLoader('./dance_unit_data.npy')
self.loader.load_encoded_library()
self.converter = coordinate2angle()
self.converter.set_bound(np.load('bound_range.npy'),
np.load('step.npy'), 32, 16)
self.init_time = time.time()
self.i = 0
self.time_sleep = 0.01
def run():
print '================ Program Started ================'
IP = '127.0.0.1'
PORT = '9559'
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.2', 19999, True, True, 5000, 5)
if clientID != -1:
print 'Connected to remote API server'
else:
print 'Connection non successful'
sys.exit('Could not connect')
print "================ Choregraphe's Initialization ================"
print 'NAO IP :' + IP
naoIP = IP
#naoIP = map(str,naoIP.split())
print ' NAO PORT:' + PORT
naoPort = PORT
naoPort = int(naoPort)
#naoPort = map(int,naoPort.split())
motionProxy = ALProxy("ALMotion", naoIP, naoPort)
postureProxy = ALProxy("ALRobotPosture", naoIP, naoPort)
#Go to the posture StandInitZero
posture = 'StandZero'
print 'Posture Initialization : ' + posture
motionProxy.stiffnessInterpolation('Body', 1.0, 1.0)
postureProxy.goToPosture(posture, 1.0)
Head_Yaw = []
Head_Pitch = []
L_Hip_Yaw_Pitch = []
L_Hip_Roll = []
L_Hip_Pitch = []
L_Knee_Pitch = []
L_Ankle_Pitch = []
L_Ankle_Roll = []
R_Hip_Yaw_Pitch = []
R_Hip_Roll = []
R_Hip_Pitch = []
R_Knee_Pitch = []
R_Ankle_Pitch = []
R_Ankle_Roll = []
L_Shoulder_Pitch = []
L_Shoulder_Roll = []
L_Elbow_Yaw = []
L_Elbow_Roll = []
L_Wrist_Yaw = []
R_Shoulder_Pitch = []
R_Shoulder_Roll = []
R_Elbow_Yaw = []
R_Elbow_Roll = []
R_Wrist_Yaw = []
R_H = []
L_H = []
R_Hand = []
L_Hand = []
Body = [
Head_Yaw, Head_Pitch, L_Hip_Yaw_Pitch, L_Hip_Roll, L_Hip_Pitch,
L_Knee_Pitch, L_Ankle_Pitch, L_Ankle_Roll, R_Hip_Yaw_Pitch, R_Hip_Roll,
R_Hip_Pitch, R_Knee_Pitch, R_Ankle_Pitch, R_Ankle_Roll,
L_Shoulder_Pitch, L_Shoulder_Roll, L_Elbow_Yaw, L_Elbow_Roll,
L_Wrist_Yaw, R_Shoulder_Pitch, R_Shoulder_Roll, R_Elbow_Yaw,
R_Elbow_Roll, R_Wrist_Yaw, L_H, L_Hand, R_H, R_Hand
]
get_first_handles(clientID, Body)
print "================ Handles Initialization ================"
commandAngles = motionProxy.getAngles('Body', False)
print '========== NAO is listening =========='
JointControl(clientID, motionProxy, 0, Body)
R_Ankle_Roll = []
L_Shoulder_Pitch = []
L_Shoulder_Roll = []
L_Elbow_Yaw = []
L_Elbow_Roll = []
L_Wrist_Yaw = []
R_Shoulder_Pitch = []
R_Shoulder_Roll = []
R_Elbow_Yaw = []
R_Elbow_Roll = []
R_Wrist_Yaw = []
R_H = []
L_H = []
R_Hand = []
L_Hand = []
Body = [
Head_Yaw, Head_Pitch, L_Hip_Yaw_Pitch, L_Hip_Roll, L_Hip_Pitch,
L_Knee_Pitch, L_Ankle_Pitch, L_Ankle_Roll, R_Hip_Yaw_Pitch, R_Hip_Roll,
R_Hip_Pitch, R_Knee_Pitch, R_Ankle_Pitch, R_Ankle_Roll, L_Shoulder_Pitch,
L_Shoulder_Roll, L_Elbow_Yaw, L_Elbow_Roll, L_Wrist_Yaw, R_Shoulder_Pitch,
R_Shoulder_Roll, R_Elbow_Yaw, R_Elbow_Roll, R_Wrist_Yaw, L_H, L_Hand, R_H,
R_Hand
]
get_first_handles(clientID, Body)
print "================ Handles Initialization ================"
commandAngles = motionProxy.getAngles('Body', False)
print '========== NAO is listening =========='
JointControl(clientID, motionProxy, 0, Body)
print 'Enter your NAO port'
naoPort = raw_input()
naoPort=int(naoPort)
#naoPort = map(int,naoPort.split())
motionProxy = ALProxy("ALMotion",naoIP, naoPort)
postureProxy = ALProxy("ALRobotPosture", naoIP, naoPort)
#Go to the posture StandInitZero
posture = 'StandZero'
print 'Posture Initialization : ' + posture
motionProxy.stiffnessInterpolation('Body', 1.0, 1.0)
postureProxy.goToPosture(posture,1.0)
Head_Yaw=[];Head_Pitch=[];
L_Hip_Yaw_Pitch=[];L_Hip_Roll=[];L_Hip_Pitch=[];L_Knee_Pitch=[];L_Ankle_Pitch=[];L_Ankle_Roll=[];
R_Hip_Yaw_Pitch=[];R_Hip_Roll=[];R_Hip_Pitch=[];R_Knee_Pitch=[];R_Ankle_Pitch=[];R_Ankle_Roll=[];
L_Shoulder_Pitch=[];L_Shoulder_Roll=[];L_Elbow_Yaw=[];L_Elbow_Roll=[];L_Wrist_Yaw=[]
R_Shoulder_Pitch=[];R_Shoulder_Roll=[];R_Elbow_Yaw=[];R_Elbow_Roll=[];R_Wrist_Yaw=[]
R_H=[];L_H=[];R_Hand=[];L_Hand=[];
Body = [Head_Yaw,Head_Pitch,L_Hip_Yaw_Pitch,L_Hip_Roll,L_Hip_Pitch,L_Knee_Pitch,L_Ankle_Pitch,L_Ankle_Roll,R_Hip_Yaw_Pitch,R_Hip_Roll,R_Hip_Pitch,R_Knee_Pitch,R_Ankle_Pitch,R_Ankle_Roll,L_Shoulder_Pitch,L_Shoulder_Roll,L_Elbow_Yaw,L_Elbow_Roll,L_Wrist_Yaw,R_Shoulder_Pitch,R_Shoulder_Roll,R_Elbow_Yaw,R_Elbow_Roll,R_Wrist_Yaw,L_H,L_Hand,R_H,R_Hand]
get_first_handles(clientID,Body)
print "================ Handles Initialization ================"
commandAngles = motionProxy.getAngles('Body', False)
print '========== NAO is listening =========='
JointControl(clientID,motionProxy,0,Body)
def runSimulation(cromossomos):
idx = 0
for cromossomo in cromossomos:
#commands.getoutput('~/Downloads/choregraphe-suite-2.1.4.13-linux64/bin/naoqi-bin -p 50004')
#subprocess.Popen('ls')
#subprocess.Popen('/home/gabriel/Downloads/choregraphe-suite-2.1.4.13-linux64/bin/naoqi-bin')
#subprocess.call('./test.sh&', shell=True)
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.2', 50003, True, True, 5000, 5)
if clientID != -1:
print 'Connected to remote API server'
# vrep.simxLoadScene(clientID,"/home/gabriel/MC906/trabalho/mc906/scenes/test.ttt",1,vrep.simx_opmode_oneshot_wait)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_streaming)
else:
print 'Connection was not successful'
sys.exit('Could not connect')
naoIP = "127.0.0.1"
naoPort = 50004
motionProxy = ALProxy("ALMotion", naoIP, naoPort)
postureProxy = ALProxy("ALRobotPosture", naoIP, naoPort)
# Go to the posture StandInitZero
posture = 'StandZero'
motionProxy.stiffnessInterpolation('Body', 1.0, 1.0)
Head_Yaw = []
Head_Pitch = []
L_Hip_Yaw_Pitch = []
L_Hip_Roll = []
L_Hip_Pitch = []
L_Knee_Pitch = []
L_Ankle_Pitch = []
L_Ankle_Roll = []
R_Hip_Yaw_Pitch = []
R_Hip_Roll = []
R_Hip_Pitch = []
R_Knee_Pitchi = []
R_Ankle_Pitch = []
R_Ankle_Roll = []
L_Shoulder_Pitch = []
L_Shoulder_Roll = []
L_Elbow_Yaw = []
L_Elbow_Roll = []
L_Wrist_Yaw = []
R_Shoulder_Pitch = []
R_Shoulder_Roll = []
R_Elbow_Yaw = []
R_Elbow_Roll = []
R_Wrist_Yaw = []
R_H = []
L_H = []
R_Hand = []
L_Hand = []
Body = [
Head_Yaw, Head_Pitch, L_Hip_Yaw_Pitch, L_Hip_Roll, L_Hip_Pitch,
L_Knee_Pitch, L_Ankle_Pitch, L_Ankle_Roll, R_Hip_Yaw_Pitch,
R_Hip_Roll, R_Hip_Pitch, R_Knee_Pitch, R_Ankle_Pitch, R_Ankle_Roll,
L_Shoulder_Pitch, L_Shoulder_Roll, L_Elbow_Yaw, L_Elbow_Roll,
L_Wrist_Yaw, R_Shoulder_Pitch, R_Shoulder_Roll, R_Elbow_Yaw,
R_Elbow_Roll, R_Wrist_Yaw, L_H, L_Hand, R_H, R_Hand
]
get_first_handles(clientID, Body)
# TARGET VELOCITY
X = 1.0
Y = 0.0
Theta = 0.0
Frequency = 1.0
# motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
postureProxy.applyPosture('StandZero', 0.1)
motionProxy.moveToward(
X, Y, Theta, [["Frequency", Frequency],
["MaxStepX", 0.01 + cromossomo[0] * 0.079],
["LeftStepHeight", 0.005 + cromossomo[1] * 0.035],
["RightStepHeight", 0.005 + cromossomo[1] * 0.035]])
# fitness.append((JointControl(clientID,motionProxy,postureProxy,0,Body),idx))
# idx += 1
return JointControl(clientID, motionProxy, postureProxy, 0, Body)
def runSimulation(cromossomos):
idx = 0
for cromossomo in cromossomos:
#commands.getoutput('~/Downloads/choregraphe-suite-2.1.4.13-linux64/bin/naoqi-bin -p 50004')
#subprocess.Popen('ls')
#subprocess.Popen('/home/gabriel/Downloads/choregraphe-suite-2.1.4.13-linux64/bin/naoqi-bin')
#subprocess.call('./test.sh&', shell=True)
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.2', 50003, True, True, 5000, 5)
if clientID != -1:
print 'Connected to remote API server'
# vrep.simxLoadScene(clientID,"/home/gabriel/MC906/trabalho/mc906/scenes/test.ttt",1,vrep.simx_opmode_oneshot_wait)
vrep.simxStartSimulation(clientID,vrep.simx_opmode_streaming)
else:
print 'Connection was not successful'
sys.exit('Could not connect')
naoIP = "127.0.0.1"
naoPort = 50004
motionProxy = ALProxy("ALMotion", naoIP, naoPort)
postureProxy = ALProxy("ALRobotPosture", naoIP, naoPort)
# Go to the posture StandInitZero
posture = 'StandZero'
motionProxy.stiffnessInterpolation('Body', 1.0, 1.0)
Head_Yaw = []
Head_Pitch = []
L_Hip_Yaw_Pitch = []
L_Hip_Roll = []
L_Hip_Pitch = []
L_Knee_Pitch = []
L_Ankle_Pitch = []
L_Ankle_Roll = []
R_Hip_Yaw_Pitch = []
R_Hip_Roll = []
R_Hip_Pitch = []
R_Knee_Pitchi = []
R_Ankle_Pitch = []
R_Ankle_Roll=[];
L_Shoulder_Pitch = []
L_Shoulder_Roll = []
L_Elbow_Yaw = []
L_Elbow_Roll = []
L_Wrist_Yaw = []
R_Shoulder_Pitch = []
R_Shoulder_Roll = []
R_Elbow_Yaw=[]
R_Elbow_Roll = []
R_Wrist_Yaw = []
R_H = []
L_H = []
R_Hand=[]
L_Hand=[]
Body = [Head_Yaw,
Head_Pitch,
L_Hip_Yaw_Pitch,
L_Hip_Roll,
L_Hip_Pitch,
L_Knee_Pitch,
L_Ankle_Pitch,
L_Ankle_Roll,
R_Hip_Yaw_Pitch,
R_Hip_Roll,
R_Hip_Pitch,
R_Knee_Pitch,
R_Ankle_Pitch,
R_Ankle_Roll,
L_Shoulder_Pitch,
L_Shoulder_Roll,
L_Elbow_Yaw,
L_Elbow_Roll,
L_Wrist_Yaw,
R_Shoulder_Pitch,
R_Shoulder_Roll,
R_Elbow_Yaw,
R_Elbow_Roll,
R_Wrist_Yaw,
L_H,
L_Hand,
R_H,
R_Hand]
get_first_handles(clientID, Body)
# TARGET VELOCITY
X = 1.0
Y = 0.0
Theta = 0.0
Frequency = 1.0
# motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
postureProxy.applyPosture('StandZero',0.1)
motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency],
["MaxStepX", 0.01 + cromossomo[0]*0.079],
["LeftStepHeight", 0.005 + cromossomo[1]*0.035],
["RightStepHeight", 0.005 + cromossomo[1]*0.035]])
# fitness.append((JointControl(clientID,motionProxy,postureProxy,0,Body),idx))
# idx += 1
return JointControl(clientID, motionProxy, postureProxy, 0, Body)
if __name__ == '__main__':
#print('Process to actuate motion: %s' % os.getpid())
ip = '127.0.0.1'
port = 19997
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart(ip, port, True, True, -5000, 5)
# Connect to V-REP
if clientID == -1:
sys.exit('\nV-REP remote API server connection failed (' + ip + ':' +
str(port) + '). Is V-REP running?')
print('Connected to Remote API Server') # show in the terminal
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
Body = {}
get_first_handles(
clientID, Body) #get first handles of Nao in the virtual environment
with open(
'C:\\Users\\lenovo\\Desktop\\AI-Project-Portfolio\\primitive_transfer_graph.json',
'r') as f:
primitive_transfer_graph = json.load(f)
cluster_id = 1
time1 = time.time()
while (True):
primitive_id = primitive_transfer_graph[str(cluster_id)]['include'][0]
print(primitive_id)
primitive = np.load(
'C:\\Users\\lenovo\\Desktop\\AI-Project-Portfolio\\danceprimitives\\'
+ primitive_id + '\\dance_motion_' + str(int(primitive_id)) +
