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