├── Vrep-Scene └── NAO.ttt ├── Scripts ├── remoteApi.dll ├── remoteApi.so ├── single_nao_control.py ├── vision_sensor.py ├── multi_nao_control.py ├── manage_joints.py ├── vrepConst.py └── vrep.py ├── Example └── test_multi_nao.py ├── README.md └── READMEFR.md /Vrep-Scene/NAO.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PierreJac/Project-NAO-Control/HEAD/Vrep-Scene/NAO.ttt -------------------------------------------------------------------------------- /Scripts/remoteApi.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PierreJac/Project-NAO-Control/HEAD/Scripts/remoteApi.dll -------------------------------------------------------------------------------- /Scripts/remoteApi.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PierreJac/Project-NAO-Control/HEAD/Scripts/remoteApi.so -------------------------------------------------------------------------------- /Example/test_multi_nao.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Jul 6 14:00:59 2015 4 | 5 | @author: Pierre Jacquot 6 | """ 7 | #This code give three different orders to three NAOs located at three different ports. 8 | #You may want to change the IP's adresses and the ports as well (or even the number of NAO) 9 | from naoqi import ALProxy 10 | from threading import Thread 11 | 12 | def StiffnessOn(proxy): 13 | pNames = "Body" 14 | pStiffnessLists = 1.0 15 | pTimeLists = 1.0 16 | proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists) 17 | 18 | 19 | def main(robotIP,port,postureName,speed=1.0): 20 | ''' Example showing a path of two positions 21 | Warning: Needs a PoseInit before executing 22 | ''' 23 | # Init proxies. 24 | try: 25 | motionProxy = ALProxy("ALMotion", robotIP, port) 26 | except Exception, e: 27 | print "Could not create proxy to ALMotion" 28 | print "Error was: ", e 29 | 30 | try: 31 | postureProxy = ALProxy("ALRobotPosture", robotIP, port) 32 | print robotIP 33 | print port 34 | 35 | # Set NAO in Stiffness On 36 | StiffnessOn(motionProxy) 37 | 38 | # Send NAO to postureName 39 | postureProxy.goToPosture(postureName,speed) 40 | 41 | except Exception, e: 42 | print "Could not create proxy to ALRobotPosture" 43 | print "Error was: ", e 44 | 45 | if __name__== "__main__": 46 | Thread(target = main, args= ('172.0.0.1',9559,'Crouch',0.5)).start() 47 | Thread(target = main, args= ('172.0.0.1',9558,'Sit',0.5)).start() 48 | Thread(target = main, args= ('172.0.0.1',9557,'SitRelax',0.5)).start() 49 | 50 | -------------------------------------------------------------------------------- /Scripts/single_nao_control.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | @author: Pierre Jacquot 4 | """ 5 | #For more informations please check : http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm 6 | import vrep,sys 7 | from naoqi import ALProxy 8 | from manage_joints import get_first_handles,JointControl 9 | 10 | print '================ Program Sarted ================' 11 | 12 | vrep.simxFinish(-1) 13 | clientID=vrep.simxStart('127.0.0.2',19999,True,True,5000,5) 14 | if clientID!=-1: 15 | print 'Connected to remote API server' 16 | 17 | else: 18 | print 'Connection non successful' 19 | sys.exit('Could not connect') 20 | 21 | 22 | print "================ Choregraphe's Initialization ================" 23 | print 'Enter your NAO IP' 24 | naoIP = raw_input() 25 | #naoIP = map(str,naoIP.split()) 26 | print 'Enter your NAO port' 27 | naoPort = raw_input() 28 | naoPort=int(naoPort) 29 | #naoPort = map(int,naoPort.split()) 30 | 31 | motionProxy = ALProxy("ALMotion",naoIP, naoPort) 32 | postureProxy = ALProxy("ALRobotPosture", naoIP, naoPort) 33 | 34 | #Go to the posture StandInitZero 35 | posture = 'StandZero' 36 | print 'Posture Initialization : ' + posture 37 | motionProxy.stiffnessInterpolation('Body', 1.0, 1.0) 38 | postureProxy.goToPosture(posture,1.0,1.0) 39 | 40 | Head_Yaw=[];Head_Pitch=[]; 41 | L_Hip_Yaw_Pitch=[];L_Hip_Roll=[];L_Hip_Pitch=[];L_Knee_Pitch=[];L_Ankle_Pitch=[];L_Ankle_Roll=[]; 42 | R_Hip_Yaw_Pitch=[];R_Hip_Roll=[];R_Hip_Pitch=[];R_Knee_Pitch=[];R_Ankle_Pitch=[];R_Ankle_Roll=[]; 43 | L_Shoulder_Pitch=[];L_Shoulder_Roll=[];L_Elbow_Yaw=[];L_Elbow_Roll=[];L_Wrist_Yaw=[] 44 | R_Shoulder_Pitch=[];R_Shoulder_Roll=[];R_Elbow_Yaw=[];R_Elbow_Roll=[];R_Wrist_Yaw=[] 45 | R_H=[];L_H=[];R_Hand=[];L_Hand=[]; 46 | 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] 47 | 48 | get_first_handles(clientID,Body) 49 | print "================ Handles Initialization ================" 50 | commandAngles = motionProxy.getAngles('Body', False) 51 | print '========== NAO is listening ==========' 52 | 53 | JointControl(clientID,motionProxy,0,Body) 54 | 55 | 56 | -------------------------------------------------------------------------------- /Scripts/vision_sensor.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thu Jul 9 15:30:54 2015 4 | 5 | @author: Pierre Jacquot 6 | """ 7 | 8 | import vrep,time,sys 9 | import matplotlib.pyplot as plt 10 | from PIL import Image as I 11 | import array 12 | 13 | def streamVisionSensor(visionSensorName,clientID,pause=0.0001): 14 | #Get the handle of the vision sensor 15 | res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait) 16 | #Get the image 17 | res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming) 18 | #Allow the display to be refreshed 19 | plt.ion() 20 | #Initialiazation of the figure 21 | time.sleep(0.5) 22 | res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) 23 | im = I.new("RGB", (resolution[0], resolution[1]), "white") 24 | #Give a title to the figure 25 | fig = plt.figure(1) 26 | fig.canvas.set_window_title(visionSensorName) 27 | #inverse the picture 28 | plotimg = plt.imshow(im,origin='lower') 29 | #Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash 30 | time.sleep(1) 31 | while (vrep.simxGetConnectionId(clientID)!=-1): 32 | #Get the image of the vision sensor 33 | res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) 34 | #Transform the image so it can be displayed using pyplot 35 | image_byte_array = array.array('b',image) 36 | im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1) 37 | #Update the image 38 | plotimg.set_data(im) 39 | #Refresh the display 40 | plt.draw() 41 | #The mandatory pause ! (or it'll not work) 42 | plt.pause(pause) 43 | print 'End of Simulation' 44 | 45 | def getVisionSensor(visionSensorName,clientID): 46 | #Get the handle of the vision sensor 47 | res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait) 48 | #Get the image 49 | res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming) 50 | time.sleep(1) 51 | while (vrep.simxGetConnectionId(clientID)!=-1): 52 | #Get the image of the vision sensor 53 | res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) 54 | print resolution 55 | print 'End of Simulation' 56 | 57 | if __name__ == '__main__': 58 | vrep.simxFinish(-1) 59 | clientID=vrep.simxStart('127.0.0.2',19999,True,True,5000,5) 60 | if clientID!=-1: 61 | print 'Connected to remote API server' 62 | #Get and display the pictures from the camera 63 | streamVisionSensor('NAO_vision1',clientID,0.0001) 64 | #Only get the image 65 | #getVisionSensor('NAO_vision1',clientID) 66 | 67 | else: 68 | print 'Connection non successful' 69 | sys.exit('Could not connect') 70 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Project NAO Control 2 | This project goal is to simulate a NAO in v-rep. 3 | The main idea is to be able to test a script in a virtual environment before implementing it on a real NAO. 4 | In addition to v-rep we will use the Choregraphe suite and the Python NAOqi SDK from Aldebaran. 5 | 6 | ### Requirements 7 | - [v-rep] : A mostly free and awsome robot simulator. 8 | - [Python NAOqi-SDK] : Contain all the function you need to manipulate your NAO (virtual or not) using python. 9 | - [Choregraphe Suite] : This will allow you to manipulate your virtual robot easier and launch a virtual NAO on your computer. 10 | - [Spyder] : This is not mandatory, but it's a good MATLAB-like development environment for python 11 | 12 | > N.B : To download the Aldebaran related softwares you must pocess a NAO or join their [developper program] 13 | 14 | ### Quickstart guide 15 | - Launch v-rep and load the scene contained in the *Vrep-scene* folder 16 | - Go to your choregraphe suite folder, then in the bin folder and launch *naoqi-bin* to create a virtual NAO on your computer 17 | - You can launch several virtual NAO on different ports using : 18 | ```sh 19 | $ ./naoqi-bin -p [Port Number] & 20 | ``` 21 | - Add pynaoqi to your python path or if your using Spyder goto to *->Tools->PYTHONPATH manager* and add a path to the folder containing pynaoqi 22 | - *Optional* : You can launch choregraphe to visualize your virtual NAO or check its IP and Port using the connect button ![Alt Text][id1] 23 | - Launch the v-rep simulation ![AltText][id2]. (or the scripts won't work) 24 | - Launch the multi_nao_control.py script if you have several NAO or the single_nao_control.py if you have just one NAO to control 25 | - Give all the informations needed (IPs and Ports) and wait until *NAO is listening* 26 | - You can try to make your NAO move in v-rep using choregraphe or a script you've made 27 | - Enjoy ! 28 | 29 | ### How to retrieve the video from NAO's vision sensors in v-rep : 30 | You can retrieve images from the cameras of your virtual NAO in v-rep just by using our script vision_sensor.py. This script will stream the camera in a independent display. You can also import the function in another script. 31 | The function getVisionSensor will just retrieve the image and not display it. 32 | 33 | ### How to configre your own v-rep scene : 34 | If you want to create your very own v-rep scene containing a NAO, you'll need to configure it so the remote API could connect to it. To do so please follow the official v-rep documentation : 35 | - [Enable remote API client side] 36 | - [Enable remote API server side] 37 | - Or if you prefer you can follow this -> [video] <- (many thanks to Nikolai K. for his really good tutorial) 38 | In order to get the camera and the fingers working you'll also need a few more steps : 39 | - For the cameras 40 | - In the properties of the cameras untick "Explicit Handling" 41 | - For the fingers 42 | - In each joint properties tick "Motion Handling of all joints enabled" 43 | - In each model properties of each joints groupement check that everything is untick 44 | 45 | Finally, disable the child scripts automatically generated with the NAO. 46 | 47 | 48 | [v-rep]:http://www.coppeliarobotics.com/downloads.html 49 | [Python NAOqi-SDK]:https://community.aldebaran.com/en/resources/software 50 | [Choregraphe Suite]:https://community.aldebaran.com/en/resources/software 51 | [developper program]:https://community.aldebaran.com/en/developerprogram#section3 52 | [Spyder]:https://pypi.python.org/pypi/spyder 53 | [id1]:http://doc.aldebaran.com/2-1/_images/connect-to_button.png 54 | [id2]:http://www.coppeliarobotics.com/helpFiles/en/images/simulation1.jpg 55 | [Enable remote API client side]:http://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm 56 | [Enable remote API server side]:http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm 57 | [video]:https://www.youtube.com/watch?v=SQont-mTnfM&list=PLhEaSBRJaAcyCDyLWYvtOte0RuoovBU2t&index=3 -------------------------------------------------------------------------------- /READMEFR.md: -------------------------------------------------------------------------------- 1 | # Projet NAO Control 2 | Le but de ce projet est de simuler un NAO sous v-rep. 3 | L'idée est d'être capable de tester un script dans un environnement virtuel avant de l'implémenter dans un NAO réel. 4 | En plus de v-rep, nous allons utiliser la suite de logiciels Choregraphe et le SDK NAOqi en Python. 5 | 6 | ### Prérequis 7 | - [v-rep] : Un super simulateur de robotique majoritairement gratuit 8 | - [Python NAOqi-SDK] : Contient toutes les fonctions dont vous avez besoin pour manipuler votre NAO (virtuel ou non) utilisant python. 9 | - [Suite de logiciels Choregraphe] : Cela vous permettra de manipuler votre robot virtuel plus aisément et de lancer un NAO virtuel sur votre ordinateur. 10 | - [Spyder] : Ce n'est pas obligatoire, mais c'est un bon environnement de développement proche de MATLAB mais pour le langage python. 11 | 12 | > N.B : Pour télécharger les logiciels liés à Aldebaran , vous devez posséder un NAO ou rejoindre leur [programme de développement] 13 | 14 | ### Guide pour démarrer rapidement 15 | - Lancez v-rep et chargez la scène contenue dans le dossier *Vrep-scene* 16 | - Allez dans votre répertoire contenant la suite de logiciels Choregraphe, puis dans le dossier bin et lancez *naoqi-bin* afin de créer un NAO virtuel sur votre ordinateur 17 | - Vous pouvez lancer plusieurs NAO virtuels sur différents Ports en utilisant : 18 | ```sh 19 | $ ./naoqi-bin -p [Numéro de Port] & 20 | ``` 21 | - Ajoutez pynaoqi dans votre chemin python (PYTHONPATH) ou si vous utilisez Spyder allez dans *->Outils->Gestionnaire de PYTHONPATH* et ajoutez le chemin du dossier contenant pynaoqi 22 | - *Optionnel* : Vous pouvez lancer Choregraphe pour visualiser votre NAO virtuel ou vérifier son adresse IP et son Port en utilisant le bouton de connexion ![Alt Text][id1] 23 | - Lancez la simulation v-rep ![AltText][id2] (sinon les scripts ne fonctionneront pas) 24 | - Lancez le script multi_nao_control.py si vous avez plusieurs NAO ou single_nao_control.py si vous avez seulement un NAO à contrôler 25 | - Donnez toutes les informations nécessaires (adresses IP et Ports) et attendez jusqu'à voir apparaître *NAO is listening* 26 | - Vous pouvez essayer de faire bouger votre NAO dans v-rep en utilisant Choregraphe ou un script que vous avez fait. 27 | - Enjoy ! 28 | 29 | ### Comment recevoir les images des caméras du NAO virtuel de v-rep 30 | Vous pouvez récupérer les images des caméras du NAO virtuel dans v-rep juste en utilisant notre script vision_sensor.py. Ce script diffusera en streaming la caméra dans une nouvelle fenêtre. Vous pouvez aussi importer la fonction dans un autre script. La fonction getVisionSensor récupérera simplement l'image mais ne l'affichera pas. 31 | ### Comment configurer votre propre scène v-rep: 32 | Si vous voulez créer votre scène v-rep personnelle contenant un NAO, vous devrez la configurer afin que l'API distante puisse s'y connecter. Pour le faire, suivez la documentation officielle de v-rep: 33 | - [Activer l'API distante du côté client] 34 | - [Activer l'API distante du côté serveur] 35 | - Ou si vous préférez, vous pouvez suivre cette -> [vidéo] <- 36 | Afin d'avoir les caméras et les doigts du NAO fonctionnels, vous allez également avoir besoin de suivre d'autres étapes: 37 | - Pour les caméras 38 | - Dans les propriétés des caméras décochez "Explicit Handling" 39 | - Pour les doigts 40 | - Dans les propriétés de chaque articulation cochez "Motion Handling of all joints enabled" 41 | - Dans chaque "model properties" de chaque groupement d'articulations vérifiez que tout est décoché 42 | 43 | Finalement, désactivez le script généré automatiquement avec le NAO. 44 | 45 | [v-rep]:http://www.coppeliarobotics.com/downloads.html 46 | [Python NAOqi-SDK]:https://community.aldebaran.com/en/resources/software 47 | [Suite de logiciels Choregraphe]:https://community.aldebaran.com/en/resources/software 48 | [programme de développement]:https://community.aldebaran.com/en/developerprogram#section3 49 | [Spyder]:https://pypi.python.org/pypi/spyder 50 | [id1]:http://doc.aldebaran.com/2-1/_images/connect-to_button.png 51 | [id2]:http://www.coppeliarobotics.com/helpFiles/en/images/simulation1.jpg 52 | [Activer l'API distante du côté client]:http://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm 53 | [Activer l'API distante du côté serveur]:http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm 54 | [vidéo]:https://www.youtube.com/watch?v=SQont-mTnfM&list=PLhEaSBRJaAcyCDyLWYvtOte0RuoovBU2t&index=3 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /Scripts/multi_nao_control.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Jul 6 14:00:59 2015 4 | 5 | @author: Pierre Jacquot 6 | """ 7 | 8 | import sys,vrep,time 9 | from manage_joints import get_new_nao_handles,get_first_handles,JointControl 10 | from naoqi import ALProxy 11 | from threading import Thread 12 | 13 | print 'Enter the number of Nao which are connected on your network : ' 14 | nbrOfNao= raw_input() 15 | nbrOfNao = int(nbrOfNao) 16 | print 'Enter their IP adresses (separated with one space)' 17 | naoIP = raw_input() 18 | naoIP = map(str,naoIP.split()) 19 | print 'Enter their ports (respectively to the order of the IPs and separated with one space)' 20 | naoPort = raw_input() 21 | naoPort = map(int,naoPort.split()) 22 | 23 | #Connexion to VRep 24 | print '================ Connexion to VRep ================' 25 | vrep.simxFinish(-1) 26 | clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) 27 | if clientID!=-1: 28 | print 'Connected to remote API server' 29 | 30 | else: 31 | print 'Connection non successful' 32 | sys.exit('Could not connect') 33 | 34 | print '================ Initialization of the first NAO ================' 35 | #Handles first initialization 36 | Head_Yaw=[];Head_Pitch=[]; 37 | L_Hip_Yaw_Pitch=[];L_Hip_Roll=[];L_Hip_Pitch=[];L_Knee_Pitch=[];L_Ankle_Pitch=[];L_Ankle_Roll=[]; 38 | R_Hip_Yaw_Pitch=[];R_Hip_Roll=[];R_Hip_Pitch=[];R_Knee_Pitch=[];R_Ankle_Pitch=[];R_Ankle_Roll=[]; 39 | L_Shoulder_Pitch=[];L_Shoulder_Roll=[];L_Elbow_Yaw=[];L_Elbow_Roll=[];L_Wrist_Yaw=[] 40 | R_Shoulder_Pitch=[];R_Shoulder_Roll=[];R_Elbow_Yaw=[];R_Elbow_Roll=[];R_Wrist_Yaw=[] 41 | R_H=[];L_H=[];R_Hand=[];L_Hand=[]; 42 | 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] 43 | #Proxy creation 44 | motionProxy=[];postureProxy=[] 45 | 46 | get_first_handles(clientID,Body) 47 | #Get the Handle of the whole NAO in VRep 48 | NAO_Handle = vrep.simxGetObjectHandle(clientID,'NAO',vrep.simx_opmode_oneshot_wait) 49 | #Get the absolute position of the first NAO 50 | NAO_pos = vrep.simxGetObjectPosition(clientID,NAO_Handle[1],-1,vrep.simx_opmode_streaming)[1] 51 | 52 | print 'You have ' + str(nbrOfNao) + ' NAO connected' 53 | 54 | 55 | #Proxy initialization 56 | if nbrOfNao == 1: 57 | motionProxy.append(ALProxy('ALMotion',naoIP[0],naoPort[0])) 58 | postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0])) 59 | else: 60 | motionProxy.append(ALProxy('ALMotion',naoIP[0],naoPort[0])) 61 | postureProxy.append(ALProxy('ALRobotPosture', naoIP[0], naoPort[0])) 62 | y=0.8 63 | #Pause the simulation to avoid collision problems 64 | vrep.simxPauseSimulation(clientID,vrep.simx_opmode_oneshot) 65 | time.sleep(2) 66 | for i in range(0,nbrOfNao-1): 67 | motionProxy.append(ALProxy('ALMotion',naoIP[i+1],naoPort[i+1])) 68 | postureProxy.append(ALProxy('ALRobotPosture', naoIP[i+1], naoPort[i+1])) 69 | #Create new NAo in VRep 70 | vrep.simxCopyPasteObjects(clientID,NAO_Handle,vrep.simx_opmode_oneshot_wait) 71 | #Get the handle of the new NAO 72 | copyNAO = vrep.simxGetObjectHandle(clientID,'NAO#'+str(i),vrep.simx_opmode_oneshot_wait) 73 | #Change the position of the new NAO so it won't collide with others 74 | vrep.simxSetObjectPosition(clientID,copyNAO[1],-1,[0,y,0.3518],vrep.simx_opmode_oneshot ) 75 | y+=0.8 76 | #Restart the simulation 77 | vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot) 78 | 79 | print '================ Posture Initialization ================' 80 | #Go to the posture StandZero 81 | print 'Posture Initialization : StandZero' 82 | for i in range(0,nbrOfNao): 83 | motionProxy[i].stiffnessInterpolation('Body', 1.0, 1.0) 84 | postureProxy[i].goToPosture('StandZero',1.0,1.0) 85 | 86 | print '================ Handle Creation for the new NAO ================' 87 | a=vrep.simxGetObjectGroupData(clientID,vrep.sim_object_joint_type,0,vrep.simx_opmode_oneshot_wait) 88 | get_new_nao_handles(nbrOfNao,clientID,Body) 89 | thread=[] 90 | for i in range(0,nbrOfNao): 91 | thread.append(Thread(target = JointControl, args=(clientID,motionProxy[i],i,Body))) 92 | thread[i].start() 93 | print 'Nao number ' + str(i+1) + ' initialized' 94 | print '================ All the Nao are listening ================' -------------------------------------------------------------------------------- /Scripts/manage_joints.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jul 7 14:09:03 2015 4 | 5 | @author: Pierre Jacquot 6 | """ 7 | 8 | import vrep 9 | 10 | #Get the handles of all NAO in the scene 11 | def get_all_handles(nbrOfNao,clientID,Body): 12 | print '-> Head for NAO : '+ str(1) 13 | Body[0].append(vrep.simxGetObjectHandle(clientID,'HeadYaw#',vrep.simx_opmode_oneshot_wait)[1]) 14 | Body[1].append(vrep.simxGetObjectHandle(clientID,'HeadPitch#',vrep.simx_opmode_oneshot_wait)[1]) 15 | #Left Leg 16 | print '-> Left Leg for NAO : ' + str(1) 17 | Body[2].append(vrep.simxGetObjectHandle(clientID,'LHipYawPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 18 | Body[3].append(vrep.simxGetObjectHandle(clientID,'LHipRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 19 | Body[4].append(vrep.simxGetObjectHandle(clientID,'LHipPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 20 | Body[5].append(vrep.simxGetObjectHandle(clientID,'LKneePitch3#',vrep.simx_opmode_oneshot_wait)[1]) 21 | Body[6].append(vrep.simxGetObjectHandle(clientID,'LAnklePitch3#',vrep.simx_opmode_oneshot_wait)[1]) 22 | Body[7].append(vrep.simxGetObjectHandle(clientID,'LAnkleRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 23 | #Right Leg 24 | print '-> Right Leg for NAO : ' + str(1) 25 | Body[8].append(vrep.simxGetObjectHandle(clientID,'RHipYawPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 26 | Body[9].append(vrep.simxGetObjectHandle(clientID,'RHipRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 27 | Body[10].append(vrep.simxGetObjectHandle(clientID,'RHipPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 28 | Body[11].append(vrep.simxGetObjectHandle(clientID,'RKneePitch3#',vrep.simx_opmode_oneshot_wait)[1]) 29 | Body[12].append(vrep.simxGetObjectHandle(clientID,'RAnklePitch3#',vrep.simx_opmode_oneshot_wait)[1]) 30 | Body[13].append(vrep.simxGetObjectHandle(clientID,'RAnkleRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 31 | #Left Arm 32 | print '-> Left Arm for NAO : ' + str(1) 33 | Body[14].append(vrep.simxGetObjectHandle(clientID,'LShoulderPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 34 | Body[15].append(vrep.simxGetObjectHandle(clientID,'LShoulderRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 35 | Body[16].append(vrep.simxGetObjectHandle(clientID,'LElbowYaw3#',vrep.simx_opmode_oneshot_wait)[1]) 36 | Body[17].append(vrep.simxGetObjectHandle(clientID,'LElbowRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 37 | Body[18].append(vrep.simxGetObjectHandle(clientID,'LWristYaw3#',vrep.simx_opmode_oneshot_wait)[1]) 38 | #Right Arm 39 | print '-> Right Arm for NAO : ' + str(1) 40 | Body[19].append(vrep.simxGetObjectHandle(clientID,'RShoulderPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 41 | Body[20].append(vrep.simxGetObjectHandle(clientID,'RShoulderRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 42 | Body[21].append(vrep.simxGetObjectHandle(clientID,'RElbowYaw3#',vrep.simx_opmode_oneshot_wait)[1]) 43 | Body[22].append(vrep.simxGetObjectHandle(clientID,'RElbowRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 44 | Body[23].append(vrep.simxGetObjectHandle(clientID,'RWristYaw3#',vrep.simx_opmode_oneshot_wait)[1]) 45 | #Left fingers 46 | print '-> Left Fingers for NAO : ' + str(1) 47 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LThumbBase#',vrep.simx_opmode_oneshot_wait)[1]) 48 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint8#',vrep.simx_opmode_oneshot_wait)[1]) 49 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LLFingerBase#',vrep.simx_opmode_oneshot_wait)[1]) 50 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint12#',vrep.simx_opmode_oneshot_wait)[1]) 51 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint14#',vrep.simx_opmode_oneshot_wait)[1]) 52 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LRFinger_Base#',vrep.simx_opmode_oneshot_wait)[1]) 53 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint11#',vrep.simx_opmode_oneshot_wait)[1]) 54 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint13#',vrep.simx_opmode_oneshot_wait)[1]) 55 | Body[25].append(Body[24][0:8]) 56 | #Right Fingers 57 | print '-> Right Fingers for NAO : ' + str(1) 58 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RThumbBase#',vrep.simx_opmode_oneshot_wait)[1]) 59 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint0#',vrep.simx_opmode_oneshot_wait)[1]) 60 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RLFingerBase#',vrep.simx_opmode_oneshot_wait)[1]) 61 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint5#',vrep.simx_opmode_oneshot_wait)[1]) 62 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint6#',vrep.simx_opmode_oneshot_wait)[1]) 63 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RRFinger_Base#',vrep.simx_opmode_oneshot_wait)[1]) 64 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint2#',vrep.simx_opmode_oneshot_wait)[1]) 65 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint3#',vrep.simx_opmode_oneshot_wait)[1]) 66 | Body[27].append(Body[26][0:8]) 67 | for i in range(0, nbrOfNao-1): 68 | print '-> Head for NAO : '+ str(i+2) 69 | Body[0].append(vrep.simxGetObjectHandle(clientID,'HeadYaw#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 70 | Body[1].append(vrep.simxGetObjectHandle(clientID,'HeadPitch#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 71 | #Left Leg 72 | print '-> Left Leg for NAO : ' + str(i+2) 73 | Body[2].append(vrep.simxGetObjectHandle(clientID,'LHipYawPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 74 | Body[3].append(vrep.simxGetObjectHandle(clientID,'LHipRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 75 | Body[4].append(vrep.simxGetObjectHandle(clientID,'LHipPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 76 | Body[5].append(vrep.simxGetObjectHandle(clientID,'LKneePitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 77 | Body[6].append(vrep.simxGetObjectHandle(clientID,'LAnklePitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 78 | Body[7].append(vrep.simxGetObjectHandle(clientID,'LAnkleRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 79 | #Right Leg 80 | print '-> Right Leg for NAO : ' + str(i+2) 81 | Body[8].append(vrep.simxGetObjectHandle(clientID,'RHipYawPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 82 | Body[9].append(vrep.simxGetObjectHandle(clientID,'RHipRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 83 | Body[10].append(vrep.simxGetObjectHandle(clientID,'RHipPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 84 | Body[11].append(vrep.simxGetObjectHandle(clientID,'RKneePitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 85 | Body[12].append(vrep.simxGetObjectHandle(clientID,'RAnklePitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 86 | Body[13].append(vrep.simxGetObjectHandle(clientID,'RAnkleRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 87 | #Left Arm 88 | print '-> Left Arm for NAO : ' + str(i+2) 89 | Body[14].append(vrep.simxGetObjectHandle(clientID,'LShoulderPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 90 | Body[15].append(vrep.simxGetObjectHandle(clientID,'LShoulderRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 91 | Body[16].append(vrep.simxGetObjectHandle(clientID,'LElbowYaw3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 92 | Body[17].append(vrep.simxGetObjectHandle(clientID,'LElbowRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 93 | Body[18].append(vrep.simxGetObjectHandle(clientID,'LWristYaw3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 94 | #Right Arm 95 | print '-> Right Arm for NAO : ' + str(i+2) 96 | Body[19].append(vrep.simxGetObjectHandle(clientID,'RShoulderPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 97 | Body[20].append(vrep.simxGetObjectHandle(clientID,'RShoulderRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 98 | Body[21].append(vrep.simxGetObjectHandle(clientID,'RElbowYaw3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 99 | Body[22].append(vrep.simxGetObjectHandle(clientID,'RElbowRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 100 | Body[23].append(vrep.simxGetObjectHandle(clientID,'RWristYaw3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 101 | #Left fingers 102 | print '-> Left Fingers for NAO : ' + str(i+2) 103 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LThumbBase#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 104 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint8#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 105 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LLFingerBase#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 106 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint12#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 107 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint14#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 108 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LRFinger_Base#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 109 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint11#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 110 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint13#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 111 | Body[25].append(Body[24][i*8:(i+1)*8]) 112 | #Right Fingers 113 | print '-> Right Fingers for NAO : ' + str(i+2) 114 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RThumbBase#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 115 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint0#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 116 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RLFingerBase#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 117 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint5#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 118 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint6#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 119 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RRFinger_Base#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 120 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint2#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 121 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 122 | Body[27].append(Body[26][i*8:(i+1)*8]) 123 | 124 | #Get the Handles of all NAOs except the first one 125 | def get_new_nao_handles(nbrOfNao,clientID,Body): 126 | for i in range(0, nbrOfNao-1): 127 | print '-> Head for NAO : '+ str(i+2) 128 | Body[0].append(vrep.simxGetObjectHandle(clientID,'HeadYaw#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 129 | Body[1].append(vrep.simxGetObjectHandle(clientID,'HeadPitch#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 130 | #Left Leg 131 | print '-> Left Leg for NAO : ' + str(i+2) 132 | Body[2].append(vrep.simxGetObjectHandle(clientID,'LHipYawPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 133 | Body[3].append(vrep.simxGetObjectHandle(clientID,'LHipRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 134 | Body[4].append(vrep.simxGetObjectHandle(clientID,'LHipPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 135 | Body[5].append(vrep.simxGetObjectHandle(clientID,'LKneePitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 136 | Body[6].append(vrep.simxGetObjectHandle(clientID,'LAnklePitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 137 | Body[7].append(vrep.simxGetObjectHandle(clientID,'LAnkleRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 138 | #Right Leg 139 | print '-> Right Leg for NAO : ' + str(i+2) 140 | Body[8].append(vrep.simxGetObjectHandle(clientID,'RHipYawPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 141 | Body[9].append(vrep.simxGetObjectHandle(clientID,'RHipRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 142 | Body[10].append(vrep.simxGetObjectHandle(clientID,'RHipPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 143 | Body[11].append(vrep.simxGetObjectHandle(clientID,'RKneePitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 144 | Body[12].append(vrep.simxGetObjectHandle(clientID,'RAnklePitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 145 | Body[13].append(vrep.simxGetObjectHandle(clientID,'RAnkleRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 146 | #Left Arm 147 | print '-> Left Arm for NAO : ' + str(i+2) 148 | Body[14].append(vrep.simxGetObjectHandle(clientID,'LShoulderPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 149 | Body[15].append(vrep.simxGetObjectHandle(clientID,'LShoulderRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 150 | Body[16].append(vrep.simxGetObjectHandle(clientID,'LElbowYaw3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 151 | Body[17].append(vrep.simxGetObjectHandle(clientID,'LElbowRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 152 | Body[18].append(vrep.simxGetObjectHandle(clientID,'LWristYaw3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 153 | #Right Arm 154 | print '-> Right Arm for NAO : ' + str(i+2) 155 | Body[19].append(vrep.simxGetObjectHandle(clientID,'RShoulderPitch3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 156 | Body[20].append(vrep.simxGetObjectHandle(clientID,'RShoulderRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 157 | Body[21].append(vrep.simxGetObjectHandle(clientID,'RElbowYaw3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 158 | Body[22].append(vrep.simxGetObjectHandle(clientID,'RElbowRoll3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 159 | Body[23].append(vrep.simxGetObjectHandle(clientID,'RWristYaw3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 160 | #Left fingers 161 | print '-> Left Fingers for NAO : ' + str(i+2) 162 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LThumbBase#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 163 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint8#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 164 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LLFingerBase#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 165 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint12#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 166 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint14#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 167 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LRFinger_Base#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 168 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint11#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 169 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint13#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 170 | Body[25].append(Body[24][i*8:(i+1)*8]) 171 | #Right Fingers 172 | print '-> Right Fingers for NAO : ' + str(i+2) 173 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RThumbBase#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 174 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint0#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 175 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RLFingerBase#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 176 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint5#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 177 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint6#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 178 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RRFinger_Base#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 179 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint2#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 180 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint3#'+str(i),vrep.simx_opmode_oneshot_wait)[1]) 181 | Body[27].append(Body[26][(i+1)*8:(i+2)*8]) 182 | #Allow the joint to move in the VRep Simulation 183 | def JointControl(clientID,motionProxy,i,Body): 184 | #Head 185 | while(vrep.simxGetConnectionId(clientID)!=-1): 186 | #Getting joint's angles from Choregraphe (please check your robot's IP) 187 | commandAngles = motionProxy.getAngles('Body', False) 188 | #Allow the robot to move in VRep using choregraphe's angles 189 | 190 | vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming) 191 | vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming) 192 | #Left Leg 193 | vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming) 194 | vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming) 195 | vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming) 196 | vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming) 197 | vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming) 198 | vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming) 199 | #Right Leg 200 | vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming) 201 | vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming) 202 | vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming) 203 | vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming) 204 | vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming) 205 | vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming) 206 | #Left Arm 207 | vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming) 208 | vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming) 209 | vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming) 210 | vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming) 211 | vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming) 212 | #Right Arm 213 | vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming) 214 | vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming) 215 | vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming) 216 | vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming) 217 | vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming) 218 | #Left Fingers 219 | vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming) 220 | vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming) 221 | vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming) 222 | vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming) 223 | vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming) 224 | vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming) 225 | vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming) 226 | vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming) 227 | #Right Fingers 228 | vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming) 229 | vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming) 230 | vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming) 231 | vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming) 232 | vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming) 233 | vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming) 234 | vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming) 235 | vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming) 236 | print 'End of simulation' 237 | 238 | #Get the Handle of only one NAO 239 | def get_first_handles(clientID,Body): 240 | print '-> Head for NAO : '+ str(1) 241 | Body[0].append(vrep.simxGetObjectHandle(clientID,'HeadYaw#',vrep.simx_opmode_oneshot_wait)[1]) 242 | Body[1].append(vrep.simxGetObjectHandle(clientID,'HeadPitch#',vrep.simx_opmode_oneshot_wait)[1]) 243 | #Left Leg 244 | print '-> Left Leg for NAO : ' + str(1) 245 | Body[2].append(vrep.simxGetObjectHandle(clientID,'LHipYawPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 246 | Body[3].append(vrep.simxGetObjectHandle(clientID,'LHipRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 247 | Body[4].append(vrep.simxGetObjectHandle(clientID,'LHipPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 248 | Body[5].append(vrep.simxGetObjectHandle(clientID,'LKneePitch3#',vrep.simx_opmode_oneshot_wait)[1]) 249 | Body[6].append(vrep.simxGetObjectHandle(clientID,'LAnklePitch3#',vrep.simx_opmode_oneshot_wait)[1]) 250 | Body[7].append(vrep.simxGetObjectHandle(clientID,'LAnkleRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 251 | #Right Leg 252 | print '-> Right Leg for NAO : ' + str(1) 253 | Body[8].append(vrep.simxGetObjectHandle(clientID,'RHipYawPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 254 | Body[9].append(vrep.simxGetObjectHandle(clientID,'RHipRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 255 | Body[10].append(vrep.simxGetObjectHandle(clientID,'RHipPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 256 | Body[11].append(vrep.simxGetObjectHandle(clientID,'RKneePitch3#',vrep.simx_opmode_oneshot_wait)[1]) 257 | Body[12].append(vrep.simxGetObjectHandle(clientID,'RAnklePitch3#',vrep.simx_opmode_oneshot_wait)[1]) 258 | Body[13].append(vrep.simxGetObjectHandle(clientID,'RAnkleRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 259 | #Left Arm 260 | print '-> Left Arm for NAO : ' + str(1) 261 | Body[14].append(vrep.simxGetObjectHandle(clientID,'LShoulderPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 262 | Body[15].append(vrep.simxGetObjectHandle(clientID,'LShoulderRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 263 | Body[16].append(vrep.simxGetObjectHandle(clientID,'LElbowYaw3#',vrep.simx_opmode_oneshot_wait)[1]) 264 | Body[17].append(vrep.simxGetObjectHandle(clientID,'LElbowRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 265 | Body[18].append(vrep.simxGetObjectHandle(clientID,'LWristYaw3#',vrep.simx_opmode_oneshot_wait)[1]) 266 | #Right Arm 267 | print '-> Right Arm for NAO : ' + str(1) 268 | Body[19].append(vrep.simxGetObjectHandle(clientID,'RShoulderPitch3#',vrep.simx_opmode_oneshot_wait)[1]) 269 | Body[20].append(vrep.simxGetObjectHandle(clientID,'RShoulderRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 270 | Body[21].append(vrep.simxGetObjectHandle(clientID,'RElbowYaw3#',vrep.simx_opmode_oneshot_wait)[1]) 271 | Body[22].append(vrep.simxGetObjectHandle(clientID,'RElbowRoll3#',vrep.simx_opmode_oneshot_wait)[1]) 272 | Body[23].append(vrep.simxGetObjectHandle(clientID,'RWristYaw3#',vrep.simx_opmode_oneshot_wait)[1]) 273 | #Left fingers 274 | print '-> Left Fingers for NAO : ' + str(1) 275 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LThumbBase#',vrep.simx_opmode_oneshot_wait)[1]) 276 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint8#',vrep.simx_opmode_oneshot_wait)[1]) 277 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LLFingerBase#',vrep.simx_opmode_oneshot_wait)[1]) 278 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint12#',vrep.simx_opmode_oneshot_wait)[1]) 279 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint14#',vrep.simx_opmode_oneshot_wait)[1]) 280 | Body[24].append(vrep.simxGetObjectHandle(clientID,'NAO_LRFinger_Base#',vrep.simx_opmode_oneshot_wait)[1]) 281 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint11#',vrep.simx_opmode_oneshot_wait)[1]) 282 | Body[24].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint13#',vrep.simx_opmode_oneshot_wait)[1]) 283 | Body[25].append(Body[24][0:8]) 284 | #Right Fingers 285 | print '-> Right Fingers for NAO : ' + str(1) 286 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RThumbBase#',vrep.simx_opmode_oneshot_wait)[1]) 287 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint0#',vrep.simx_opmode_oneshot_wait)[1]) 288 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RLFingerBase#',vrep.simx_opmode_oneshot_wait)[1]) 289 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint5#',vrep.simx_opmode_oneshot_wait)[1]) 290 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint6#',vrep.simx_opmode_oneshot_wait)[1]) 291 | Body[26].append(vrep.simxGetObjectHandle(clientID,'NAO_RRFinger_Base#',vrep.simx_opmode_oneshot_wait)[1]) 292 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint2#',vrep.simx_opmode_oneshot_wait)[1]) 293 | Body[26].append(vrep.simxGetObjectHandle(clientID,'Revolute_joint3#',vrep.simx_opmode_oneshot_wait)[1]) 294 | Body[27].append(Body[26][0:8]) -------------------------------------------------------------------------------- /Scripts/vrepConst.py: -------------------------------------------------------------------------------- 1 | # This file is part of the REMOTE API 2 | # 3 | # Copyright 2006-2015 Coppelia Robotics GmbH. All rights reserved. 4 | # marc@coppeliarobotics.com 5 | # www.coppeliarobotics.com 6 | # 7 | # The REMOTE API is licensed under the terms of GNU GPL: 8 | # 9 | # ------------------------------------------------------------------- 10 | # The REMOTE API is free software: you can redistribute it and/or modify 11 | # it under the terms of the GNU General Public License as published by 12 | # the Free Software Foundation, either version 3 of the License, or 13 | # (at your option) any later version. 14 | # 15 | # THE REMOTE API IS DISTRIBUTED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED 16 | # WARRANTY. THE USER WILL USE IT AT HIS/HER OWN RISK. THE ORIGINAL 17 | # AUTHORS AND COPPELIA ROBOTICS GMBH WILL NOT BE LIABLE FOR DATA LOSS, 18 | # DAMAGES, LOSS OF PROFITS OR ANY OTHER KIND OF LOSS WHILE USING OR 19 | # MISUSING THIS SOFTWARE. 20 | # 21 | # See the GNU General Public License for more details. 22 | # 23 | # You should have received a copy of the GNU General Public License 24 | # along with the REMOTE API. If not, see . 25 | # ------------------------------------------------------------------- 26 | # 27 | # This file was automatically created for V-REP release V3.2.1 on May 3rd 2015 28 | 29 | #constants 30 | #Scene object types. Values are serialized 31 | sim_object_shape_type =0 32 | sim_object_joint_type =1 33 | sim_object_graph_type =2 34 | sim_object_camera_type =3 35 | sim_object_dummy_type =4 36 | sim_object_proximitysensor_type =5 37 | sim_object_reserved1 =6 38 | sim_object_reserved2 =7 39 | sim_object_path_type =8 40 | sim_object_visionsensor_type =9 41 | sim_object_volume_type =10 42 | sim_object_mill_type =11 43 | sim_object_forcesensor_type =12 44 | sim_object_light_type =13 45 | sim_object_mirror_type =14 46 | 47 | #General object types. Values are serialized 48 | sim_appobj_object_type =109 49 | sim_appobj_collision_type =110 50 | sim_appobj_distance_type =111 51 | sim_appobj_simulation_type =112 52 | sim_appobj_ik_type =113 53 | sim_appobj_constraintsolver_type=114 54 | sim_appobj_collection_type =115 55 | sim_appobj_ui_type =116 56 | sim_appobj_script_type =117 57 | sim_appobj_pathplanning_type =118 58 | sim_appobj_RESERVED_type =119 59 | sim_appobj_texture_type =120 60 | 61 | # Ik calculation methods. Values are serialized 62 | sim_ik_pseudo_inverse_method =0 63 | sim_ik_damped_least_squares_method =1 64 | sim_ik_jacobian_transpose_method =2 65 | 66 | # Ik constraints. Values are serialized 67 | sim_ik_x_constraint =1 68 | sim_ik_y_constraint =2 69 | sim_ik_z_constraint =4 70 | sim_ik_alpha_beta_constraint=8 71 | sim_ik_gamma_constraint =16 72 | sim_ik_avoidance_constraint =64 73 | 74 | # Ik calculation results 75 | sim_ikresult_not_performed =0 76 | sim_ikresult_success =1 77 | sim_ikresult_fail =2 78 | 79 | # Scene object sub-types. Values are serialized 80 | # Light sub-types 81 | sim_light_omnidirectional_subtype =1 82 | sim_light_spot_subtype =2 83 | sim_light_directional_subtype =3 84 | # Joint sub-types 85 | sim_joint_revolute_subtype =10 86 | sim_joint_prismatic_subtype =11 87 | sim_joint_spherical_subtype =12 88 | # Shape sub-types 89 | sim_shape_simpleshape_subtype =20 90 | sim_shape_multishape_subtype =21 91 | # Proximity sensor sub-types 92 | sim_proximitysensor_pyramid_subtype =30 93 | sim_proximitysensor_cylinder_subtype=31 94 | sim_proximitysensor_disc_subtype =32 95 | sim_proximitysensor_cone_subtype =33 96 | sim_proximitysensor_ray_subtype =34 97 | # Mill sub-types 98 | sim_mill_pyramid_subtype =40 99 | sim_mill_cylinder_subtype =41 100 | sim_mill_disc_subtype =42 101 | sim_mill_cone_subtype =42 102 | # No sub-type 103 | sim_object_no_subtype =200 104 | 105 | 106 | #Scene object main properties (serialized) 107 | sim_objectspecialproperty_collidable =0x0001 108 | sim_objectspecialproperty_measurable =0x0002 109 | #reserved =0x0004 110 | #reserved =0x0008 111 | sim_objectspecialproperty_detectable_ultrasonic =0x0010 112 | sim_objectspecialproperty_detectable_infrared =0x0020 113 | sim_objectspecialproperty_detectable_laser =0x0040 114 | sim_objectspecialproperty_detectable_inductive =0x0080 115 | sim_objectspecialproperty_detectable_capacitive =0x0100 116 | sim_objectspecialproperty_renderable =0x0200 117 | sim_objectspecialproperty_detectable_all =sim_objectspecialproperty_detectable_ultrasonic|sim_objectspecialproperty_detectable_infrared|sim_objectspecialproperty_detectable_laser|sim_objectspecialproperty_detectable_inductive|sim_objectspecialproperty_detectable_capacitive 118 | sim_objectspecialproperty_cuttable =0x0400 119 | sim_objectspecialproperty_pathplanning_ignored =0x0800 120 | 121 | # Model properties (serialized) 122 | sim_modelproperty_not_collidable =0x0001 123 | sim_modelproperty_not_measurable =0x0002 124 | sim_modelproperty_not_renderable =0x0004 125 | sim_modelproperty_not_detectable =0x0008 126 | sim_modelproperty_not_cuttable =0x0010 127 | sim_modelproperty_not_dynamic =0x0020 128 | sim_modelproperty_not_respondable =0x0040 # cannot be selected if sim_modelproperty_not_dynamic is not selected 129 | sim_modelproperty_not_reset =0x0080 # Model is not reset at simulation end. This flag is cleared at simulation end 130 | sim_modelproperty_not_visible =0x0100 # Whole model is invisible independent of local visibility settings 131 | sim_modelproperty_not_model =0xf000 # object is not a model 132 | 133 | 134 | # Check the documentation instead of comments below!! 135 | # Following messages are dispatched to the Lua-message container 136 | sim_message_ui_button_state_change =0 # a UI button slider etc. changed (due to a user's action). aux[0]=UI handle aux[1]=button handle aux[2]=button attributes aux[3]=slider position (if slider) 137 | sim_message_reserved9 =1 # Do not use 138 | sim_message_object_selection_changed=2 139 | sim_message_reserved10 =3 # do not use 140 | sim_message_model_loaded =4 141 | sim_message_reserved11 =5 # do not use 142 | sim_message_keypress =6 # a key was pressed while the focus was on a page (aux[0]=key aux[1]=ctrl and shift key state) 143 | sim_message_bannerclicked =7 # a banner was clicked (aux[0]=banner ID) 144 | 145 | 146 | # Following messages are dispatched only to the C-API (not available from Lua) 147 | sim_message_for_c_api_only_start =0x100 # Do not use 148 | sim_message_reserved1 =0x101 # Do not use 149 | sim_message_reserved2 =0x102 # Do not use 150 | sim_message_reserved3 =0x103 # Do not use 151 | sim_message_eventcallback_scenesave =0x104 # about to save a scene 152 | sim_message_eventcallback_modelsave =0x105 # about to save a model (current selection will be saved) 153 | sim_message_eventcallback_moduleopen =0x106 # called when simOpenModule in Lua is called 154 | sim_message_eventcallback_modulehandle =0x107 # called when simHandleModule in Lua is called with argument false 155 | sim_message_eventcallback_moduleclose =0x108 # called when simCloseModule in Lua is called 156 | sim_message_reserved4 =0x109 # Do not use 157 | sim_message_reserved5 =0x10a # Do not use 158 | sim_message_reserved6 =0x10b # Do not use 159 | sim_message_reserved7 =0x10c # Do not use 160 | sim_message_eventcallback_instancepass =0x10d # Called once every main application loop pass. auxiliaryData[0] contains event flags of events that happened since last time 161 | sim_message_eventcallback_broadcast =0x10e 162 | sim_message_eventcallback_imagefilter_enumreset =0x10f 163 | sim_message_eventcallback_imagefilter_enumerate =0x110 164 | sim_message_eventcallback_imagefilter_adjustparams =0x111 165 | sim_message_eventcallback_imagefilter_reserved =0x112 166 | sim_message_eventcallback_imagefilter_process =0x113 167 | sim_message_eventcallback_reserved1 =0x114 # do not use 168 | sim_message_eventcallback_reserved2 =0x115 # do not use 169 | sim_message_eventcallback_reserved3 =0x116 # do not use 170 | sim_message_eventcallback_reserved4 =0x117 # do not use 171 | sim_message_eventcallback_abouttoundo =0x118 # the undo button was hit and a previous state is about to be restored 172 | sim_message_eventcallback_undoperformed =0x119 # the undo button was hit and a previous state restored 173 | sim_message_eventcallback_abouttoredo =0x11a # the redo button was hit and a future state is about to be restored 174 | sim_message_eventcallback_redoperformed =0x11b # the redo button was hit and a future state restored 175 | sim_message_eventcallback_scripticondblclick =0x11c # scipt icon was double clicked. (aux[0]=object handle associated with script set replyData[0] to 1 if script should not be opened) 176 | sim_message_eventcallback_simulationabouttostart =0x11d 177 | sim_message_eventcallback_simulationended =0x11e 178 | sim_message_eventcallback_reserved5 =0x11f # do not use 179 | sim_message_eventcallback_keypress =0x120 # a key was pressed while the focus was on a page (aux[0]=key aux[1]=ctrl and shift key state) 180 | sim_message_eventcallback_modulehandleinsensingpart =0x121 # called when simHandleModule in Lua is called with argument true 181 | sim_message_eventcallback_renderingpass =0x122 # called just before the scene is rendered 182 | sim_message_eventcallback_bannerclicked =0x123 # called when a banner was clicked (aux[0]=banner ID) 183 | sim_message_eventcallback_menuitemselected =0x124 # auxiliaryData[0] indicates the handle of the item auxiliaryData[1] indicates the state of the item 184 | sim_message_eventcallback_refreshdialogs =0x125 # aux[0]=refresh degree (0=light 1=medium 2=full) 185 | sim_message_eventcallback_sceneloaded =0x126 186 | sim_message_eventcallback_modelloaded =0x127 187 | sim_message_eventcallback_instanceswitch =0x128 188 | sim_message_eventcallback_guipass =0x129 189 | sim_message_eventcallback_mainscriptabouttobecalled =0x12a 190 | sim_message_eventcallback_rmlposition =0x12b #the command simRMLPosition was called. The appropriate plugin should handle the call 191 | sim_message_eventcallback_rmlvelocity =0x12c # the command simRMLVelocity was called. The appropriate plugin should handle the call 192 | sim_message_simulation_start_resume_request =0x1000 193 | sim_message_simulation_pause_request =0x1001 194 | sim_message_simulation_stop_request =0x1002 195 | 196 | # Scene object properties. Combine with the | operator 197 | sim_objectproperty_reserved1 =0x0000 198 | sim_objectproperty_reserved2 =0x0001 199 | sim_objectproperty_reserved3 =0x0002 200 | sim_objectproperty_reserved4 =0x0003 201 | sim_objectproperty_reserved5 =0x0004 # formely sim_objectproperty_visible 202 | sim_objectproperty_reserved6 =0x0008 # formely sim_objectproperty_wireframe 203 | sim_objectproperty_collapsed =0x0010 204 | sim_objectproperty_selectable =0x0020 205 | sim_objectproperty_reserved7 =0x0040 206 | sim_objectproperty_selectmodelbaseinstead =0x0080 207 | sim_objectproperty_dontshowasinsidemodel =0x0100 208 | # reserved =0x0200 209 | sim_objectproperty_canupdatedna =0x0400 210 | sim_objectproperty_selectinvisible =0x0800 211 | sim_objectproperty_depthinvisible =0x1000 212 | 213 | 214 | # type of arguments (input and output) for custom lua commands 215 | sim_lua_arg_nil =0 216 | sim_lua_arg_bool =1 217 | sim_lua_arg_int =2 218 | sim_lua_arg_float =3 219 | sim_lua_arg_string =4 220 | sim_lua_arg_invalid =5 221 | sim_lua_arg_table =8 222 | 223 | # custom user interface properties. Values are serialized. 224 | sim_ui_property_visible =0x0001 225 | sim_ui_property_visibleduringsimulationonly =0x0002 226 | sim_ui_property_moveable =0x0004 227 | sim_ui_property_relativetoleftborder =0x0008 228 | sim_ui_property_relativetotopborder =0x0010 229 | sim_ui_property_fixedwidthfont =0x0020 230 | sim_ui_property_systemblock =0x0040 231 | sim_ui_property_settocenter =0x0080 232 | sim_ui_property_rolledup =0x0100 233 | sim_ui_property_selectassociatedobject =0x0200 234 | sim_ui_property_visiblewhenobjectselected =0x0400 235 | 236 | 237 | # button properties. Values are serialized. 238 | sim_buttonproperty_button =0x0000 239 | sim_buttonproperty_label =0x0001 240 | sim_buttonproperty_slider =0x0002 241 | sim_buttonproperty_editbox =0x0003 242 | sim_buttonproperty_staydown =0x0008 243 | sim_buttonproperty_enabled =0x0010 244 | sim_buttonproperty_borderless =0x0020 245 | sim_buttonproperty_horizontallycentered =0x0040 246 | sim_buttonproperty_ignoremouse =0x0080 247 | sim_buttonproperty_isdown =0x0100 248 | sim_buttonproperty_transparent =0x0200 249 | sim_buttonproperty_nobackgroundcolor =0x0400 250 | sim_buttonproperty_rollupaction =0x0800 251 | sim_buttonproperty_closeaction =0x1000 252 | sim_buttonproperty_verticallycentered =0x2000 253 | sim_buttonproperty_downupevent =0x4000 254 | 255 | 256 | # Simulation status 257 | sim_simulation_stopped =0x00 # Simulation is stopped 258 | sim_simulation_paused =0x08 # Simulation is paused 259 | sim_simulation_advancing =0x10 # Simulation is advancing 260 | sim_simulation_advancing_firstafterstop =sim_simulation_advancing|0x00 # First simulation pass (1x) 261 | sim_simulation_advancing_running =sim_simulation_advancing|0x01 # Normal simulation pass (>=1x) 262 | # reserved =sim_simulation_advancing|0x02 263 | sim_simulation_advancing_lastbeforepause =sim_simulation_advancing|0x03 # Last simulation pass before pause (1x) 264 | sim_simulation_advancing_firstafterpause =sim_simulation_advancing|0x04 # First simulation pass after pause (1x) 265 | sim_simulation_advancing_abouttostop =sim_simulation_advancing|0x05 # "Trying to stop" simulation pass (>=1x) 266 | sim_simulation_advancing_lastbeforestop =sim_simulation_advancing|0x06 # Last simulation pass (1x) 267 | 268 | 269 | # Script execution result (first return value) 270 | sim_script_no_error =0 271 | sim_script_main_script_nonexistent =1 272 | sim_script_main_script_not_called =2 273 | sim_script_reentrance_error =4 274 | sim_script_lua_error =8 275 | sim_script_call_error =16 276 | 277 | 278 | # Script types (serialized!) 279 | sim_scripttype_mainscript =0 280 | sim_scripttype_childscript =1 281 | sim_scripttype_pluginscript =2 282 | sim_scripttype_threaded =0x00f0 # Combine with one of above's type values 283 | 284 | # API call error messages 285 | sim_api_errormessage_ignore =0 # does not memorize nor output errors 286 | sim_api_errormessage_report =1 # memorizes errors (default for C-API calls) 287 | sim_api_errormessage_output =2 # memorizes and outputs errors (default for Lua-API calls) 288 | 289 | 290 | # special argument of some functions 291 | sim_handle_all =-2 292 | sim_handle_all_except_explicit =-3 293 | sim_handle_self =-4 294 | sim_handle_main_script =-5 295 | sim_handle_tree =-6 296 | sim_handle_chain =-7 297 | sim_handle_single =-8 298 | sim_handle_default =-9 299 | sim_handle_all_except_self =-10 300 | sim_handle_parent =-11 301 | 302 | 303 | # special handle flags 304 | sim_handleflag_assembly =0x400000 305 | sim_handleflag_model =0x800000 306 | 307 | 308 | # distance calculation methods (serialized) 309 | sim_distcalcmethod_dl =0 310 | sim_distcalcmethod_dac =1 311 | sim_distcalcmethod_max_dl_dac =2 312 | sim_distcalcmethod_dl_and_dac =3 313 | sim_distcalcmethod_sqrt_dl2_and_dac2=4 314 | sim_distcalcmethod_dl_if_nonzero =5 315 | sim_distcalcmethod_dac_if_nonzero =6 316 | 317 | 318 | # Generic dialog styles 319 | sim_dlgstyle_message =0 320 | sim_dlgstyle_input =1 321 | sim_dlgstyle_ok =2 322 | sim_dlgstyle_ok_cancel =3 323 | sim_dlgstyle_yes_no =4 324 | sim_dlgstyle_dont_center =32# can be combined with one of above values. Only with this flag can the position of the related UI be set just after dialog creation 325 | 326 | # Generic dialog return values 327 | sim_dlgret_still_open =0 328 | sim_dlgret_ok =1 329 | sim_dlgret_cancel =2 330 | sim_dlgret_yes =3 331 | sim_dlgret_no =4 332 | 333 | 334 | # Path properties 335 | sim_pathproperty_show_line =0x0001 336 | sim_pathproperty_show_orientation =0x0002 337 | sim_pathproperty_closed_path =0x0004 338 | sim_pathproperty_automatic_orientation =0x0008 339 | sim_pathproperty_invert_velocity =0x0010 340 | sim_pathproperty_infinite_acceleration =0x0020 341 | sim_pathproperty_flat_path =0x0040 342 | sim_pathproperty_show_position =0x0080 343 | sim_pathproperty_auto_velocity_profile_translation =0x0100 344 | sim_pathproperty_auto_velocity_profile_rotation =0x0200 345 | sim_pathproperty_endpoints_at_zero =0x0400 346 | sim_pathproperty_keep_x_up =0x0800 347 | 348 | 349 | # drawing objects 350 | # following are mutually exclusive 351 | sim_drawing_points =0 # 3 values per point (point size in pixels) 352 | sim_drawing_lines =1 # 6 values per line (line size in pixels) 353 | sim_drawing_triangles =2 # 9 values per triangle 354 | sim_drawing_trianglepoints =3 # 6 values per point (3 for triangle position 3 for triangle normal vector) (triangle size in meters) 355 | sim_drawing_quadpoints =4 # 6 values per point (3 for quad position 3 for quad normal vector) (quad size in meters) 356 | sim_drawing_discpoints =5 # 6 values per point (3 for disc position 3 for disc normal vector) (disc size in meters) 357 | sim_drawing_cubepoints =6 # 6 values per point (3 for cube position 3 for cube normal vector) (cube size in meters) 358 | sim_drawing_spherepoints =7 # 3 values per point (sphere size in meters) 359 | 360 | # following can be or-combined 361 | sim_drawing_itemcolors =0x00020 # +3 values per item (each item has its own ambient color (rgb values)). 362 | # Mutually exclusive with sim_drawing_vertexcolors 363 | sim_drawing_vertexcolors =0x00040 # +3 values per vertex (each vertex has its own ambient color (rgb values). Only for sim_drawing_lines (+6) and for sim_drawing_triangles(+9)). Mutually exclusive with sim_drawing_itemcolors 364 | sim_drawing_itemsizes =0x00080 # +1 value per item (each item has its own size). Not for sim_drawing_triangles 365 | sim_drawing_backfaceculling =0x00100 # back faces are not displayed for all items 366 | sim_drawing_wireframe =0x00200 # all items displayed in wireframe 367 | sim_drawing_painttag =0x00400 # all items are tagged as paint (for additinal processing at a later stage) 368 | sim_drawing_followparentvisibility =0x00800 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible 369 | sim_drawing_cyclic =0x01000 # if the max item count was reached then the first items are overwritten. 370 | sim_drawing_50percenttransparency =0x02000 # the drawing object will be 50% transparent 371 | sim_drawing_25percenttransparency =0x04000 # the drawing object will be 25% transparent 372 | sim_drawing_12percenttransparency =0x08000 # the drawing object will be 12.5% transparent 373 | sim_drawing_emissioncolor =0x10000 # When used in combination with sim_drawing_itemcolors or sim_drawing_vertexcolors then the specified colors will be for the emissive component 374 | sim_drawing_facingcamera =0x20000 # Only for trianglepoints quadpoints discpoints and cubepoints. If specified the normal verctor is calculated to face the camera (each item data requires 3 values less) 375 | sim_drawing_overlay =0x40000 # When specified objects are always drawn on top of "regular objects" 376 | sim_drawing_itemtransparency =0x80000 # +1 value per item (each item has its own transparency value (0-1)). Not compatible with sim_drawing_vertexcolors 377 | 378 | # banner values 379 | # following can be or-combined 380 | sim_banner_left =0x00001 # Banners display on the left of the specified point 381 | sim_banner_right =0x00002 # Banners display on the right of the specified point 382 | sim_banner_nobackground =0x00004 # Banners have no background rectangle 383 | sim_banner_overlay =0x00008 # When specified banners are always drawn on top of "regular objects" 384 | sim_banner_followparentvisibility =0x00010 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible 385 | sim_banner_clickselectsparent =0x00020 # if the object is associated with a scene object then clicking the banner will select the scene object 386 | sim_banner_clicktriggersevent =0x00040 # if the banner is clicked an event is triggered (sim_message_eventcallback_bannerclicked and sim_message_bannerclicked are generated) 387 | sim_banner_facingcamera =0x00080 # If specified the banner will always face the camera by rotating around the banner's vertical axis (y-axis) 388 | sim_banner_fullyfacingcamera =0x00100 # If specified the banner will always fully face the camera (the banner's orientation is same as the camera looking at it) 389 | sim_banner_backfaceculling =0x00200 # If specified the banner will only be visible from one side 390 | sim_banner_keepsamesize =0x00400 # If specified the banner will always appear in the same size. In that case size represents the character height in pixels 391 | sim_banner_bitmapfont =0x00800 # If specified a fixed-size bitmap font is used. The text will also always fully face the camera and be right 392 | # to the specified position. Bitmap fonts are not clickable 393 | 394 | 395 | # particle objects following are mutually exclusive 396 | sim_particle_points1 =0 # 6 values per point (pt1 and pt2. Pt1 is start position pt2-pt1 is the initial velocity vector). i 397 | #Point is 1 pixel big. Only appearance is a point internally handled as a perfect sphere 398 | sim_particle_points2 =1 # 6 values per point. Point is 2 pixel big. Only appearance is a point internally handled as a perfect sphere 399 | sim_particle_points4 =2 # 6 values per point. Point is 4 pixel big. Only appearance is a point internally handled as a perfect sphere 400 | sim_particle_roughspheres =3 # 6 values per sphere. Only appearance is rough. Internally a perfect sphere 401 | sim_particle_spheres =4 # 6 values per sphere. Internally a perfect sphere 402 | 403 | 404 | 405 | 406 | # following can be or-combined 407 | sim_particle_respondable1to4 =0x0020 # the particles are respondable against shapes (against all objects that have at least one bit 1-4 activated in the global respondable mask) 408 | sim_particle_respondable5to8 =0x0040 # the particles are respondable against shapes (against all objects that have at least one bit 5-8 activated in the global respondable mask) 409 | sim_particle_particlerespondable =0x0080 # the particles are respondable against each other 410 | sim_particle_ignoresgravity =0x0100 # the particles ignore the effect of gravity. Not compatible with sim_particle_water 411 | sim_particle_invisible =0x0200 # the particles are invisible 412 | sim_particle_itemsizes =0x0400 # +1 value per particle (each particle can have a different size) 413 | sim_particle_itemdensities =0x0800 # +1 value per particle (each particle can have a different density) 414 | sim_particle_itemcolors =0x1000 # +3 values per particle (each particle can have a different color) 415 | sim_particle_cyclic =0x2000 # if the max item count was reached then the first items are overwritten. 416 | sim_particle_emissioncolor =0x4000 # When used in combination with sim_particle_itemcolors then the specified colors will be for the emissive component 417 | sim_particle_water =0x8000 # the particles are water particles (no weight in the water (i.e. when z<0)). Not compatible with sim_particle_ignoresgravity 418 | sim_particle_painttag =0x10000 # The particles can be seen by vision sensors (sim_particle_invisible must not be set) 419 | 420 | 421 | 422 | 423 | # custom user interface menu attributes 424 | sim_ui_menu_title =1 425 | sim_ui_menu_minimize =2 426 | sim_ui_menu_close =4 427 | sim_ui_menu_systemblock =8 428 | 429 | 430 | 431 | # Boolean parameters 432 | sim_boolparam_hierarchy_visible =0 433 | sim_boolparam_console_visible =1 434 | sim_boolparam_collision_handling_enabled =2 435 | sim_boolparam_distance_handling_enabled =3 436 | sim_boolparam_ik_handling_enabled =4 437 | sim_boolparam_gcs_handling_enabled =5 438 | sim_boolparam_dynamics_handling_enabled =6 439 | sim_boolparam_joint_motion_handling_enabled =7 440 | sim_boolparam_path_motion_handling_enabled =8 441 | sim_boolparam_proximity_sensor_handling_enabled =9 442 | sim_boolparam_vision_sensor_handling_enabled =10 443 | sim_boolparam_mill_handling_enabled =11 444 | sim_boolparam_browser_visible =12 445 | sim_boolparam_scene_and_model_load_messages =13 446 | sim_reserved0 =14 447 | sim_boolparam_shape_textures_are_visible =15 448 | sim_boolparam_display_enabled =16 449 | sim_boolparam_infotext_visible =17 450 | sim_boolparam_statustext_open =18 451 | sim_boolparam_fog_enabled =19 452 | sim_boolparam_rml2_available =20 453 | sim_boolparam_rml4_available =21 454 | sim_boolparam_mirrors_enabled =22 455 | sim_boolparam_aux_clip_planes_enabled =23 456 | sim_boolparam_full_model_copy_from_api =24 457 | sim_boolparam_realtime_simulation =25 458 | sim_boolparam_force_show_wireless_emission =27 459 | sim_boolparam_force_show_wireless_reception =28 460 | sim_boolparam_video_recording_triggered =29 461 | sim_boolparam_threaded_rendering_enabled =32 462 | sim_boolparam_fullscreen =33 463 | sim_boolparam_headless =34 464 | sim_boolparam_hierarchy_toolbarbutton_enabled =35 465 | sim_boolparam_browser_toolbarbutton_enabled =36 466 | sim_boolparam_objectshift_toolbarbutton_enabled =37 467 | sim_boolparam_objectrotate_toolbarbutton_enabled=38 468 | sim_boolparam_force_calcstruct_all_visible =39 469 | sim_boolparam_force_calcstruct_all =40 470 | sim_boolparam_exit_request =41 471 | sim_boolparam_play_toolbarbutton_enabled =42 472 | sim_boolparam_pause_toolbarbutton_enabled =43 473 | sim_boolparam_stop_toolbarbutton_enabled =44 474 | 475 | 476 | # Integer parameters 477 | sim_intparam_error_report_mode =0 # Check sim_api_errormessage_... constants above for valid values 478 | sim_intparam_program_version =1 # e.g Version 2.1.4 --> 20104. Can only be read 479 | sim_intparam_instance_count =2 # do not use anymore (always returns 1 since V-REP 2.5.11) 480 | sim_intparam_custom_cmd_start_id =3 # can only be read 481 | sim_intparam_compilation_version =4 # 0=evaluation version 1=full version 2=player version. Can only be read 482 | sim_intparam_current_page =5 483 | sim_intparam_flymode_camera_handle =6 # can only be read 484 | sim_intparam_dynamic_step_divider =7 # can only be read 485 | sim_intparam_dynamic_engine =8 # 0=Bullet 1=ODE. 2=Vortex. 486 | sim_intparam_server_port_start =9 # can only be read 487 | sim_intparam_server_port_range =10 # can only be read 488 | sim_intparam_visible_layers =11 489 | sim_intparam_infotext_style =12 490 | sim_intparam_settings =13 491 | sim_intparam_edit_mode_type =14 # can only be read 492 | sim_intparam_server_port_next =15 # is initialized at sim_intparam_server_port_start 493 | sim_intparam_qt_version =16 # version of the used Qt framework 494 | sim_intparam_event_flags_read =17 # can only be read 495 | sim_intparam_event_flags_read_clear =18 # can only be read 496 | sim_intparam_platform =19 # can only be read 497 | sim_intparam_scene_unique_id =20 # can only be read 498 | sim_intparam_work_thread_count =21 499 | sim_intparam_mouse_x =22 500 | sim_intparam_mouse_y =23 501 | sim_intparam_core_count =24 502 | sim_intparam_work_thread_calc_time_ms =25 503 | sim_intparam_idle_fps =26 504 | sim_intparam_prox_sensor_select_down =27 505 | sim_intparam_prox_sensor_select_up =28 506 | sim_intparam_stop_request_counter =29 507 | sim_intparam_program_revision =30 508 | sim_intparam_mouse_buttons =31 509 | sim_intparam_dynamic_warning_disabled_mask =32 510 | sim_intparam_simulation_warning_disabled_mask =33 511 | sim_intparam_scene_index =34 512 | sim_intparam_motionplanning_seed =35 513 | 514 | # Float parameters 515 | sim_floatparam_rand=0 # random value (0.0-1.0) 516 | sim_floatparam_simulation_time_step =1 517 | sim_floatparam_stereo_distance =2 518 | 519 | # String parameters 520 | sim_stringparam_application_path=0 # path of V-REP's executable 521 | sim_stringparam_video_filename=1 522 | sim_stringparam_app_arg1 =2 523 | sim_stringparam_app_arg2 =3 524 | sim_stringparam_app_arg3 =4 525 | sim_stringparam_app_arg4 =5 526 | sim_stringparam_app_arg5 =6 527 | sim_stringparam_app_arg6 =7 528 | sim_stringparam_app_arg7 =8 529 | sim_stringparam_app_arg8 =9 530 | sim_stringparam_app_arg9 =10 531 | sim_stringparam_scene_path_and_name =13 532 | 533 | # Array parameters 534 | sim_arrayparam_gravity =0 535 | sim_arrayparam_fog =1 536 | sim_arrayparam_fog_color =2 537 | sim_arrayparam_background_color1=3 538 | sim_arrayparam_background_color2=4 539 | sim_arrayparam_ambient_light =5 540 | sim_arrayparam_random_euler =6 541 | 542 | 543 | # User interface elements 544 | sim_gui_menubar =0x0001 545 | sim_gui_popups =0x0002 546 | sim_gui_toolbar1 =0x0004 547 | sim_gui_toolbar2 =0x0008 548 | sim_gui_hierarchy =0x0010 549 | sim_gui_infobar =0x0020 550 | sim_gui_statusbar =0x0040 551 | sim_gui_scripteditor =0x0080 552 | sim_gui_scriptsimulationparameters =0x0100 553 | sim_gui_dialogs =0x0200 554 | sim_gui_browser =0x0400 555 | sim_gui_all =0xffff 556 | 557 | 558 | # Joint modes 559 | sim_jointmode_passive =0 560 | sim_jointmode_motion =1 561 | sim_jointmode_ik =2 562 | sim_jointmode_ikdependent =3 563 | sim_jointmode_dependent =4 564 | sim_jointmode_force =5 565 | 566 | 567 | # Navigation and selection modes with the mouse. Lower byte values are mutually exclusive upper byte bits can be combined 568 | sim_navigation_passive =0x0000 569 | sim_navigation_camerashift =0x0001 570 | sim_navigation_camerarotate =0x0002 571 | sim_navigation_camerazoom =0x0003 572 | sim_navigation_cameratilt =0x0004 573 | sim_navigation_cameraangle =0x0005 574 | sim_navigation_camerafly =0x0006 575 | sim_navigation_objectshift =0x0007 576 | sim_navigation_objectrotate =0x0008 577 | sim_navigation_reserved2 =0x0009 578 | sim_navigation_reserved3 =0x000A 579 | sim_navigation_jointpathtest =0x000B 580 | sim_navigation_ikmanip =0x000C 581 | sim_navigation_objectmultipleselection =0x000D 582 | # Bit-combine following values and add them to one of above's values for a valid navigation mode 583 | sim_navigation_reserved4 =0x0100 584 | sim_navigation_clickselection =0x0200 585 | sim_navigation_ctrlselection =0x0400 586 | sim_navigation_shiftselection =0x0800 587 | sim_navigation_camerazoomwheel =0x1000 588 | sim_navigation_camerarotaterightbutton =0x2000 589 | 590 | 591 | 592 | #Remote API constants 593 | SIMX_VERSION =0 594 | # Remote API message header structure 595 | SIMX_HEADER_SIZE =18 596 | simx_headeroffset_crc =0 # 1 simxUShort. Generated by the client or server. The CRC for the message 597 | simx_headeroffset_version =2 # 1 byte. Generated by the client or server. The version of the remote API software 598 | simx_headeroffset_message_id =3 # 1 simxInt. Generated by the client (and used in a reply by the server) 599 | simx_headeroffset_client_time =7 # 1 simxInt. Client time stamp generated by the client (and sent back by the server) 600 | simx_headeroffset_server_time =11 # 1 simxInt. Generated by the server when a reply is generated. The server timestamp 601 | simx_headeroffset_scene_id =15 # 1 simxUShort. Generated by the server. A unique ID identifying the scene currently displayed 602 | simx_headeroffset_server_state =17 # 1 byte. Generated by the server. Bit coded 0 set --> simulation not stopped 1 set --> simulation paused 2 set --> real-time switch on 3-5 edit mode type (0=no edit mode 1=triangle 2=vertex 3=edge 4=path 5=UI) 603 | 604 | # Remote API command header 605 | SIMX_SUBHEADER_SIZE =26 606 | simx_cmdheaderoffset_mem_size =0 # 1 simxInt. Generated by the client or server. The buffer size of the command. 607 | simx_cmdheaderoffset_full_mem_size =4 # 1 simxInt. Generated by the client or server. The full buffer size of the command (applies to split chunks). 608 | simx_cmdheaderoffset_pdata_offset0 =8 # 1 simxUShort. Generated by the client or server. The amount of data that is part of the command identification. 609 | simx_cmdheaderoffset_pdata_offset1 =10 # 1 simxInt. Generated by the client or server. The amount of shift of the pure data buffer (applies to split chunks). 610 | simx_cmdheaderoffset_cmd=14 # 1 simxInt. Generated by the client (and used in a reply by the server). The command combined with the operation mode of the command. 611 | simx_cmdheaderoffset_delay_or_split =18 # 1 simxUShort. Generated by the client or server. The amount of delay in ms of a continuous command or the max. pure data size to send at once (applies to split commands). 612 | simx_cmdheaderoffset_sim_time =20 # 1 simxInt. Generated by the server. The simulation time (in ms) when the command was executed (or 0 if simulation is not running) 613 | simx_cmdheaderoffset_status =24 # 1 byte. Generated by the server. (1 bit 0 is set --> error in function execution on server side). The client writes bit 1 if command cannot be overwritten 614 | simx_cmdheaderoffset_reserved =25 # 1 byte. Not yet used 615 | 616 | 617 | 618 | 619 | 620 | # Regular operation modes 621 | simx_opmode_oneshot =0x000000 # sends command as one chunk. Reply will also come as one chunk. Doesn't wait for the reply. 622 | simx_opmode_oneshot_wait =0x010000 # sends command as one chunk. Reply will also come as one chunk. Waits for the reply (_REPLY_WAIT_TIMEOUT_IN_MS is the timeout). 623 | simx_opmode_continuous =0x020000 624 | simx_opmode_streaming =0x020000 # sends command as one chunk. Command will be stored on the server and always executed 625 | #(every x ms (as far as possible) where x can be 0-65535. just add x to opmode_continuous). 626 | # A reply will be sent continuously each time as one chunk. Doesn't wait for the reply. 627 | 628 | # Operation modes for heavy data 629 | simx_opmode_oneshot_split =0x030000 # sends command as several chunks (max chunk size is x bytes where x can be _MIN_SPLIT_AMOUNT_IN_BYTES-65535. Just add x to opmode_oneshot_split). Reply will also come as several chunks. Doesn't wait for the reply. 630 | simx_opmode_continuous_split =0x040000 631 | simx_opmode_streaming_split =0x040000 # sends command as several chunks (max chunk size is x bytes where x can be _MIN_SPLIT_AMOUNT_IN_BYTES-65535. Just add x to opmode_continuous_split). Command will be stored on the server and always executed. A reply will be sent continuously each time as several chunks. Doesn't wait for the reply. 632 | 633 | # Special operation modes 634 | simx_opmode_discontinue =0x050000 # removes and cancels all commands stored on the client or server side (also continuous commands) 635 | simx_opmode_buffer =0x060000 # doesn't send anything but checks if a reply for the given command is available in the input buffer (i.e. previously received from the server) 636 | simx_opmode_remove =0x070000 # doesn't send anything and doesn't return any specific value. It just erases a similar command reply in the inbox (to free some memory) 637 | 638 | 639 | # Command return codes 640 | simx_return_ok =0x000000 641 | simx_return_novalue_flag =0x000001 # input buffer doesn't contain the specified command 642 | simx_return_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode 643 | simx_return_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode 644 | simx_return_remote_error_flag =0x000008 # command caused an error on the server side 645 | simx_return_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) 646 | simx_return_local_error_flag =0x000020 # command caused an error on the client side 647 | simx_return_initialize_error_flag =0x000040 # simxStart was not yet called 648 | 649 | # Following for backward compatibility (same as above) 650 | simx_error_noerror =0x000000 651 | simx_error_novalue_flag =0x000001 # input buffer doesn't contain the specified command 652 | simx_error_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode 653 | simx_error_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode 654 | simx_error_remote_error_flag =0x000008 # command caused an error on the server side 655 | simx_error_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) 656 | simx_error_local_error_flag =0x000020 # command caused an error on the client side 657 | simx_error_initialize_error_flag =0x000040 # simxStart was not yet called 658 | 659 | 660 | -------------------------------------------------------------------------------- /Scripts/vrep.py: -------------------------------------------------------------------------------- 1 | # This file is part of the REMOTE API 2 | # 3 | # Copyright 2006-2015 Coppelia Robotics GmbH. All rights reserved. 4 | # marc@coppeliarobotics.com 5 | # www.coppeliarobotics.com 6 | # 7 | # The REMOTE API is licensed under the terms of GNU GPL: 8 | # 9 | # ------------------------------------------------------------------- 10 | # The REMOTE API is free software: you can redistribute it and/or modify 11 | # it under the terms of the GNU General Public License as published by 12 | # the Free Software Foundation, either version 3 of the License, or 13 | # (at your option) any later version. 14 | # 15 | # THE REMOTE API IS DISTRIBUTED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED 16 | # WARRANTY. THE USER WILL USE IT AT HIS/HER OWN RISK. THE ORIGINAL 17 | # AUTHORS AND COPPELIA ROBOTICS GMBH WILL NOT BE LIABLE FOR DATA LOSS, 18 | # DAMAGES, LOSS OF PROFITS OR ANY OTHER KIND OF LOSS WHILE USING OR 19 | # MISUSING THIS SOFTWARE. 20 | # 21 | # See the GNU General Public License for more details. 22 | # 23 | # You should have received a copy of the GNU General Public License 24 | # along with the REMOTE API. If not, see . 25 | # ------------------------------------------------------------------- 26 | # 27 | # This file was automatically created for V-REP release V3.2.1 on May 3rd 2015 28 | 29 | import platform 30 | import struct 31 | from ctypes import * 32 | from vrepConst import * 33 | 34 | #load library 35 | libsimx = None 36 | try: 37 | if platform.system() =='Windows': 38 | libsimx = CDLL("./remoteApi.dll") 39 | elif platform.system() == 'Darwin': 40 | libsimx = CDLL("./remoteApi.dylib") 41 | else: 42 | libsimx = CDLL("./remoteApi.so") 43 | except: 44 | print ('----------------------------------------------------') 45 | print ('The remoteApi library could not be loaded. Make sure') 46 | print ('it is located in the same folder as "vrep.py", or') 47 | print ('appropriately adjust the file "vrep.py"') 48 | print ('----------------------------------------------------') 49 | print ('') 50 | 51 | #ctypes wrapper prototypes 52 | c_GetJointPosition = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetJointPosition", libsimx)) 53 | c_SetJointPosition = CFUNCTYPE(c_int32,c_int32, c_int32, c_float, c_int32)(("simxSetJointPosition", libsimx)) 54 | c_GetJointMatrix = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetJointMatrix", libsimx)) 55 | c_SetSphericalJointMatrix = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxSetSphericalJointMatrix", libsimx)) 56 | c_SetJointTargetVelocity = CFUNCTYPE(c_int32,c_int32, c_int32, c_float, c_int32)(("simxSetJointTargetVelocity", libsimx)) 57 | c_SetJointTargetPosition = CFUNCTYPE(c_int32,c_int32, c_int32, c_float, c_int32)(("simxSetJointTargetPosition", libsimx)) 58 | c_GetJointForce = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetJointForce", libsimx)) 59 | c_SetJointForce = CFUNCTYPE(c_int32,c_int32, c_int32, c_float, c_int32)(("simxSetJointForce", libsimx)) 60 | c_ReadForceSensor = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_ubyte), POINTER(c_float), POINTER(c_float), c_int32)(("simxReadForceSensor", libsimx)) 61 | c_BreakForceSensor = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxBreakForceSensor", libsimx)) 62 | c_ReadVisionSensor = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_ubyte), POINTER(POINTER(c_float)), POINTER(POINTER(c_int32)), c_int32)(("simxReadVisionSensor", libsimx)) 63 | c_GetObjectHandle = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_int32), c_int32)(("simxGetObjectHandle", libsimx)) 64 | c_GetVisionSensorImage = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), POINTER(POINTER(c_byte)), c_ubyte, c_int32)(("simxGetVisionSensorImage", libsimx)) 65 | c_SetVisionSensorImage = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_byte), c_int32, c_ubyte, c_int32)(("simxSetVisionSensorImage", libsimx)) 66 | c_GetVisionSensorDepthBuffer= CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), POINTER(POINTER(c_float)), c_int32)(("simxGetVisionSensorDepthBuffer", libsimx)) 67 | c_GetObjectChild = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetObjectChild", libsimx)) 68 | c_GetObjectParent = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetObjectParent", libsimx)) 69 | c_ReadProximitySensor = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_ubyte), POINTER(c_float), POINTER(c_int32), POINTER(c_float), c_int32)(("simxReadProximitySensor", libsimx)) 70 | c_LoadModel = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_ubyte, POINTER(c_int32), c_int32)(("simxLoadModel", libsimx)) 71 | c_LoadUI = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_ubyte, POINTER(c_int32), POINTER(POINTER(c_int32)), c_int32)(("simxLoadUI", libsimx)) 72 | c_LoadScene = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_ubyte, c_int32)(("simxLoadScene", libsimx)) 73 | c_StartSimulation = CFUNCTYPE(c_int32,c_int32, c_int32)(("simxStartSimulation", libsimx)) 74 | c_PauseSimulation = CFUNCTYPE(c_int32,c_int32, c_int32)(("simxPauseSimulation", libsimx)) 75 | c_StopSimulation = CFUNCTYPE(c_int32,c_int32, c_int32)(("simxStopSimulation", libsimx)) 76 | c_GetUIHandle = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_int32), c_int32)(("simxGetUIHandle", libsimx)) 77 | c_GetUISlider = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetUISlider", libsimx)) 78 | c_SetUISlider = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_int32, c_int32)(("simxSetUISlider", libsimx)) 79 | c_GetUIEventButton = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), POINTER(c_int32), c_int32)(("simxGetUIEventButton", libsimx)) 80 | c_GetUIButtonProperty = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetUIButtonProperty", libsimx)) 81 | c_SetUIButtonProperty = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_int32, c_int32)(("simxSetUIButtonProperty", libsimx)) 82 | c_AddStatusbarMessage = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32)(("simxAddStatusbarMessage", libsimx)) 83 | c_AuxiliaryConsoleOpen = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32, c_int32, POINTER(c_int32), POINTER(c_int32), POINTER(c_float), POINTER(c_float), POINTER(c_int32), c_int32)(("simxAuxiliaryConsoleOpen", libsimx)) 84 | c_AuxiliaryConsoleClose = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxAuxiliaryConsoleClose", libsimx)) 85 | c_AuxiliaryConsolePrint = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_char), c_int32)(("simxAuxiliaryConsolePrint", libsimx)) 86 | c_AuxiliaryConsoleShow = CFUNCTYPE(c_int32,c_int32, c_int32, c_ubyte, c_int32)(("simxAuxiliaryConsoleShow", libsimx)) 87 | c_GetObjectOrientation = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetObjectOrientation", libsimx)) 88 | c_GetObjectPosition = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetObjectPosition", libsimx)) 89 | c_SetObjectOrientation = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_float), c_int32)(("simxSetObjectOrientation", libsimx)) 90 | c_SetObjectPosition = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_float), c_int32)(("simxSetObjectPosition", libsimx)) 91 | c_SetObjectParent = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_ubyte, c_int32)(("simxSetObjectParent", libsimx)) 92 | c_SetUIButtonLabel = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_char), POINTER(c_char), c_int32)(("simxSetUIButtonLabel", libsimx)) 93 | c_GetLastErrors = CFUNCTYPE(c_int32,c_int32, POINTER(c_int32), POINTER(POINTER(c_char)), c_int32)(("simxGetLastErrors", libsimx)) 94 | c_GetArrayParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetArrayParameter", libsimx)) 95 | c_SetArrayParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxSetArrayParameter", libsimx)) 96 | c_GetBooleanParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_ubyte), c_int32)(("simxGetBooleanParameter", libsimx)) 97 | c_SetBooleanParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_ubyte, c_int32)(("simxSetBooleanParameter", libsimx)) 98 | c_GetIntegerParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetIntegerParameter", libsimx)) 99 | c_SetIntegerParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_int32)(("simxSetIntegerParameter", libsimx)) 100 | c_GetFloatingParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetFloatingParameter", libsimx)) 101 | c_SetFloatingParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_float, c_int32)(("simxSetFloatingParameter", libsimx)) 102 | c_GetStringParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(POINTER(c_char)), c_int32)(("simxGetStringParameter", libsimx)) 103 | c_GetCollisionHandle = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_int32), c_int32)(("simxGetCollisionHandle", libsimx)) 104 | c_GetDistanceHandle = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_int32), c_int32)(("simxGetDistanceHandle", libsimx)) 105 | c_ReadCollision = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_ubyte), c_int32)(("simxReadCollision", libsimx)) 106 | c_ReadDistance = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxReadDistance", libsimx)) 107 | c_RemoveObject = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxRemoveObject", libsimx)) 108 | c_RemoveModel = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxRemoveModel", libsimx)) 109 | c_RemoveUI = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxRemoveUI", libsimx)) 110 | c_CloseScene = CFUNCTYPE(c_int32,c_int32, c_int32)(("simxCloseScene", libsimx)) 111 | c_GetObjects = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), POINTER(POINTER(c_int32)), c_int32)(("simxGetObjects", libsimx)) 112 | c_DisplayDialog = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_char), c_int32, POINTER(c_char), POINTER(c_float), POINTER(c_float), POINTER(c_int32), POINTER(c_int32), c_int32)(("simxDisplayDialog", libsimx)) 113 | c_EndDialog = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxEndDialog", libsimx)) 114 | c_GetDialogInput = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(POINTER(c_char)), c_int32)(("simxGetDialogInput", libsimx)) 115 | c_GetDialogResult = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetDialogResult", libsimx)) 116 | c_CopyPasteObjects = CFUNCTYPE(c_int32,c_int32, POINTER(c_int32), c_int32, POINTER(POINTER(c_int32)), POINTER(c_int32), c_int32)(("simxCopyPasteObjects", libsimx)) 117 | c_GetObjectSelection = CFUNCTYPE(c_int32,c_int32, POINTER(POINTER(c_int32)), POINTER(c_int32), c_int32)(("simxGetObjectSelection", libsimx)) 118 | c_SetObjectSelection = CFUNCTYPE(c_int32,c_int32, POINTER(c_int32), c_int32, c_int32)(("simxSetObjectSelection", libsimx)) 119 | c_ClearFloatSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32)(("simxClearFloatSignal", libsimx)) 120 | c_ClearIntegerSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32)(("simxClearIntegerSignal", libsimx)) 121 | c_ClearStringSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32)(("simxClearStringSignal", libsimx)) 122 | c_GetFloatSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_float), c_int32)(("simxGetFloatSignal", libsimx)) 123 | c_GetIntegerSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_int32), c_int32)(("simxGetIntegerSignal", libsimx)) 124 | c_GetStringSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(POINTER(c_ubyte)), POINTER(c_int32), c_int32)(("simxGetStringSignal", libsimx)) 125 | c_SetFloatSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_float, c_int32)(("simxSetFloatSignal", libsimx)) 126 | c_SetIntegerSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32, c_int32)(("simxSetIntegerSignal", libsimx)) 127 | c_SetStringSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_ubyte), c_int32, c_int32)(("simxSetStringSignal", libsimx)) 128 | c_AppendStringSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_ubyte), c_int32, c_int32)(("simxAppendStringSignal", libsimx)) 129 | c_WriteStringStream = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_ubyte), c_int32, c_int32)(("simxWriteStringStream", libsimx)) 130 | c_GetObjectFloatParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetObjectFloatParameter", libsimx)) 131 | c_SetObjectFloatParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_float, c_int32)(("simxSetObjectFloatParameter", libsimx)) 132 | c_GetObjectIntParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetObjectIntParameter", libsimx)) 133 | c_SetObjectIntParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_int32, c_int32)(("simxSetObjectIntParameter", libsimx)) 134 | c_GetModelProperty = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetModelProperty", libsimx)) 135 | c_SetModelProperty = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_int32)(("simxSetModelProperty", libsimx)) 136 | c_Start = CFUNCTYPE(c_int32,POINTER(c_char), c_int32, c_ubyte, c_ubyte, c_int32, c_int32)(("simxStart", libsimx)) 137 | c_Finish = CFUNCTYPE(None, c_int32)(("simxFinish", libsimx)) 138 | c_GetPingTime = CFUNCTYPE(c_int32,c_int32, POINTER(c_int32))(("simxGetPingTime", libsimx)) 139 | c_GetLastCmdTime = CFUNCTYPE(c_int32,c_int32)(("simxGetLastCmdTime", libsimx)) 140 | c_SynchronousTrigger = CFUNCTYPE(c_int32,c_int32)(("simxSynchronousTrigger", libsimx)) 141 | c_Synchronous = CFUNCTYPE(c_int32,c_int32, c_ubyte)(("simxSynchronous", libsimx)) 142 | c_PauseCommunication = CFUNCTYPE(c_int32,c_int32, c_ubyte)(("simxPauseCommunication", libsimx)) 143 | c_GetInMessageInfo = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32))(("simxGetInMessageInfo", libsimx)) 144 | c_GetOutMessageInfo = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32))(("simxGetOutMessageInfo", libsimx)) 145 | c_GetConnectionId = CFUNCTYPE(c_int32,c_int32)(("simxGetConnectionId", libsimx)) 146 | c_CreateBuffer = CFUNCTYPE(POINTER(c_ubyte), c_int32)(("simxCreateBuffer", libsimx)) 147 | c_ReleaseBuffer = CFUNCTYPE(None, c_void_p)(("simxReleaseBuffer", libsimx)) 148 | c_TransferFile = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_char), c_int32, c_int32)(("simxTransferFile", libsimx)) 149 | c_EraseFile = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32)(("simxEraseFile", libsimx)) 150 | c_GetAndClearStringSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(POINTER(c_ubyte)), POINTER(c_int32), c_int32)(("simxGetAndClearStringSignal", libsimx)) 151 | c_ReadStringStream = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(POINTER(c_ubyte)), POINTER(c_int32), c_int32)(("simxReadStringStream", libsimx)) 152 | c_CreateDummy = CFUNCTYPE(c_int32,c_int32, c_float, POINTER(c_ubyte), POINTER(c_int32), c_int32)(("simxCreateDummy", libsimx)) 153 | c_Query = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_ubyte), c_int32, POINTER(c_char), POINTER(POINTER(c_ubyte)), POINTER(c_int32), c_int32)(("simxQuery", libsimx)) 154 | c_GetObjectGroupData = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_int32), POINTER(POINTER(c_int32)), POINTER(c_int32), POINTER(POINTER(c_int32)), POINTER(c_int32), POINTER(POINTER(c_float)), POINTER(c_int32), POINTER(POINTER(c_char)), c_int32)(("simxGetObjectGroupData", libsimx)) 155 | c_GetObjectVelocity = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), POINTER(c_float), c_int32)(("simxGetObjectVelocity", libsimx)) 156 | 157 | #API functions 158 | def simxGetJointPosition(clientID, jointHandle, operationMode): 159 | ''' 160 | Please have a look at the function description/documentation in the V-REP user manual 161 | ''' 162 | position = c_float() 163 | return c_GetJointPosition(clientID, jointHandle, byref(position), operationMode), position.value 164 | 165 | def simxSetJointPosition(clientID, jointHandle, position, operationMode): 166 | ''' 167 | Please have a look at the function description/documentation in the V-REP user manual 168 | ''' 169 | 170 | return c_SetJointPosition(clientID, jointHandle, position, operationMode) 171 | 172 | def simxGetJointMatrix(clientID, jointHandle, operationMode): 173 | ''' 174 | Please have a look at the function description/documentation in the V-REP user manual 175 | ''' 176 | matrix = (c_float*12)() 177 | ret = c_GetJointMatrix(clientID, jointHandle, matrix, operationMode) 178 | arr = [] 179 | for i in range(12): 180 | arr.append(matrix[i]) 181 | return ret, arr 182 | 183 | def simxSetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode): 184 | ''' 185 | Please have a look at the function description/documentation in the V-REP user manual 186 | ''' 187 | matrix = (c_float*12)(*matrix) 188 | return c_SetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode) 189 | 190 | def simxSetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode): 191 | ''' 192 | Please have a look at the function description/documentation in the V-REP user manual 193 | ''' 194 | 195 | return c_SetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode) 196 | 197 | def simxSetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode): 198 | ''' 199 | Please have a look at the function description/documentation in the V-REP user manual 200 | ''' 201 | 202 | return c_SetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode) 203 | 204 | def simxJointGetForce(clientID, jointHandle, operationMode): 205 | ''' 206 | Please have a look at the function description/documentation in the V-REP user manual 207 | ''' 208 | force = c_float() 209 | return c_GetJointForce(clientID, jointHandle, byref(force), operationMode), force.value 210 | 211 | def simxGetJointForce(clientID, jointHandle, operationMode): 212 | ''' 213 | Please have a look at the function description/documentation in the V-REP user manual 214 | ''' 215 | force = c_float() 216 | return c_GetJointForce(clientID, jointHandle, byref(force), operationMode), force.value 217 | 218 | def simxSetJointForce(clientID, jointHandle, force, operationMode): 219 | ''' 220 | Please have a look at the function description/documentation in the V-REP user manual 221 | ''' 222 | return c_SetJointForce(clientID, jointHandle, force, operationMode) 223 | 224 | def simxReadForceSensor(clientID, forceSensorHandle, operationMode): 225 | ''' 226 | Please have a look at the function description/documentation in the V-REP user manual 227 | ''' 228 | state = c_ubyte() 229 | forceVector = (c_float*3)() 230 | torqueVector = (c_float*3)() 231 | ret = c_ReadForceSensor(clientID, forceSensorHandle, byref(state), forceVector, torqueVector, operationMode) 232 | arr1 = [] 233 | for i in range(3): 234 | arr1.append(forceVector[i]) 235 | arr2 = [] 236 | for i in range(3): 237 | arr2.append(torqueVector[i]) 238 | return ret, ord(state.value), arr1, arr2 239 | 240 | def simxBreakForceSensor(clientID, forceSensorHandle, operationMode): 241 | ''' 242 | Please have a look at the function description/documentation in the V-REP user manual 243 | ''' 244 | return c_BreakForceSensor(clientID, forceSensorHandle, operationMode) 245 | 246 | def simxReadVisionSensor(clientID, sensorHandle, operationMode): 247 | ''' 248 | Please have a look at the function description/documentation in the V-REP user manual 249 | ''' 250 | 251 | detectionState = c_ubyte() 252 | auxValues = pointer(c_float()) 253 | auxValuesCount = pointer(c_int()) 254 | ret = c_ReadVisionSensor(clientID, sensorHandle, byref(detectionState), byref(auxValues), byref(auxValuesCount), operationMode) 255 | 256 | auxValues2 = [] 257 | if ret == 0: 258 | s = 0 259 | for i in range(auxValuesCount[0]): 260 | auxValues2.append(auxValues[s:s+auxValuesCount[i+1]]) 261 | s += auxValuesCount[i+1] 262 | 263 | #free C buffers 264 | c_ReleaseBuffer(auxValues) 265 | c_ReleaseBuffer(auxValuesCount) 266 | 267 | return ret, bool(detectionState.value!=0), auxValues2 268 | 269 | def simxGetObjectHandle(clientID, objectName, operationMode): 270 | ''' 271 | Please have a look at the function description/documentation in the V-REP user manual 272 | ''' 273 | handle = c_int() 274 | return c_GetObjectHandle(clientID, objectName, byref(handle), operationMode), handle.value 275 | 276 | def simxGetVisionSensorImage(clientID, sensorHandle, options, operationMode): 277 | ''' 278 | Please have a look at the function description/documentation in the V-REP user manual 279 | ''' 280 | 281 | resolution = (c_int*2)() 282 | c_image = pointer(c_byte()) 283 | bytesPerPixel = 3 284 | if (options and 1) != 0: 285 | bytesPerPixel = 1 286 | ret = c_GetVisionSensorImage(clientID, sensorHandle, resolution, byref(c_image), options, operationMode) 287 | 288 | reso = [] 289 | image = [] 290 | if (ret == 0): 291 | image = [None]*resolution[0]*resolution[1]*bytesPerPixel 292 | for i in range(resolution[0] * resolution[1] * bytesPerPixel): 293 | image[i] = c_image[i] 294 | for i in range(2): 295 | reso.append(resolution[i]) 296 | return ret, reso, image 297 | 298 | def simxSetVisionSensorImage(clientID, sensorHandle, image, options, operationMode): 299 | ''' 300 | Please have a look at the function description/documentation in the V-REP user manual 301 | ''' 302 | size = len(image) 303 | image_bytes = (c_byte*size)(*image) 304 | return c_SetVisionSensorImage(clientID, sensorHandle, image_bytes, size, options, operationMode) 305 | 306 | def simxGetVisionSensorDepthBuffer(clientID, sensorHandle, operationMode): 307 | ''' 308 | Please have a look at the function description/documentation in the V-REP user manual 309 | ''' 310 | c_buffer = pointer(c_float()) 311 | resolution = (c_int*2)() 312 | ret = c_GetVisionSensorDepthBuffer(clientID, sensorHandle, resolution, byref(c_buffer), operationMode) 313 | reso = [] 314 | buffer = [] 315 | if (ret == 0): 316 | buffer = [None]*resolution[0]*resolution[1] 317 | for i in range(resolution[0] * resolution[1]): 318 | buffer[i] = c_buffer[i] 319 | for i in range(2): 320 | reso.append(resolution[i]) 321 | return ret, reso, buffer 322 | 323 | def simxGetObjectChild(clientID, parentObjectHandle, childIndex, operationMode): 324 | ''' 325 | Please have a look at the function description/documentation in the V-REP user manual 326 | ''' 327 | childObjectHandle = c_int() 328 | return c_GetObjectChild(clientID, parentObjectHandle, childIndex, byref(childObjectHandle), operationMode), childObjectHandle.value 329 | 330 | def simxGetObjectParent(clientID, childObjectHandle, operationMode): 331 | ''' 332 | Please have a look at the function description/documentation in the V-REP user manual 333 | ''' 334 | 335 | parentObjectHandle = c_int() 336 | return c_GetObjectParent(clientID, childObjectHandle, byref(parentObjectHandle), operationMode), parentObjectHandle.value 337 | 338 | def simxReadProximitySensor(clientID, sensorHandle, operationMode): 339 | ''' 340 | Please have a look at the function description/documentation in the V-REP user manual 341 | ''' 342 | 343 | detectionState = c_ubyte() 344 | detectedObjectHandle = c_int() 345 | detectedPoint = (c_float*3)() 346 | detectedSurfaceNormalVector = (c_float*3)() 347 | ret = c_ReadProximitySensor(clientID, sensorHandle, byref(detectionState), detectedPoint, byref(detectedObjectHandle), detectedSurfaceNormalVector, operationMode) 348 | arr1 = [] 349 | for i in range(3): 350 | arr1.append(detectedPoint[i]) 351 | arr2 = [] 352 | for i in range(3): 353 | arr2.append(detectedSurfaceNormalVector[i]) 354 | return ret, bool(detectionState.value!=0), arr1, detectedObjectHandle.value, arr2 355 | 356 | def simxLoadModel(clientID, modelPathAndName, options, operationMode): 357 | ''' 358 | Please have a look at the function description/documentation in the V-REP user manual 359 | ''' 360 | baseHandle = c_int() 361 | return c_LoadModel(clientID, modelPathAndName, options, byref(baseHandle), operationMode), baseHandle.value 362 | 363 | def simxLoadUI(clientID, uiPathAndName, options, operationMode): 364 | ''' 365 | Please have a look at the function description/documentation in the V-REP user manual 366 | ''' 367 | 368 | count = c_int() 369 | uiHandles = pointer(c_int()) 370 | ret = c_LoadUI(clientID, uiPathAndName, options, byref(count), byref(uiHandles), operationMode) 371 | 372 | handles = [] 373 | if ret == 0: 374 | for i in range(count.value): 375 | handles.append(uiHandles[i]) 376 | #free C buffers 377 | c_ReleaseBuffer(uiHandles) 378 | 379 | return ret, handles 380 | 381 | def simxLoadScene(clientID, scenePathAndName, options, operationMode): 382 | ''' 383 | Please have a look at the function description/documentation in the V-REP user manual 384 | ''' 385 | 386 | return c_LoadScene(clientID, scenePathAndName, options, operationMode) 387 | 388 | def simxStartSimulation(clientID, operationMode): 389 | ''' 390 | Please have a look at the function description/documentation in the V-REP user manual 391 | ''' 392 | 393 | return c_StartSimulation(clientID, operationMode) 394 | 395 | def simxPauseSimulation(clientID, operationMode): 396 | ''' 397 | Please have a look at the function description/documentation in the V-REP user manual 398 | ''' 399 | 400 | return c_PauseSimulation(clientID, operationMode) 401 | 402 | def simxStopSimulation(clientID, operationMode): 403 | ''' 404 | Please have a look at the function description/documentation in the V-REP user manual 405 | ''' 406 | 407 | return c_StopSimulation(clientID, operationMode) 408 | 409 | def simxGetUIHandle(clientID, uiName, operationMode): 410 | ''' 411 | Please have a look at the function description/documentation in the V-REP user manual 412 | ''' 413 | 414 | handle = c_int() 415 | return c_GetUIHandle(clientID, uiName, byref(handle), operationMode), handle.value 416 | 417 | def simxGetUISlider(clientID, uiHandle, uiButtonID, operationMode): 418 | ''' 419 | Please have a look at the function description/documentation in the V-REP user manual 420 | ''' 421 | 422 | position = c_int() 423 | return c_GetUISlider(clientID, uiHandle, uiButtonID, byref(position), operationMode), position.value 424 | 425 | def simxSetUISlider(clientID, uiHandle, uiButtonID, position, operationMode): 426 | ''' 427 | Please have a look at the function description/documentation in the V-REP user manual 428 | ''' 429 | 430 | return c_SetUISlider(clientID, uiHandle, uiButtonID, position, operationMode) 431 | 432 | def simxGetUIEventButton(clientID, uiHandle, operationMode): 433 | ''' 434 | Please have a look at the function description/documentation in the V-REP user manual 435 | ''' 436 | 437 | uiEventButtonID = c_int() 438 | auxValues = (c_int*2)() 439 | ret = c_GetUIEventButton(clientID, uiHandle, byref(uiEventButtonID), auxValues, operationMode) 440 | arr = [] 441 | for i in range(2): 442 | arr.append(auxValues[i]) 443 | return ret, uiEventButtonID.value, arr 444 | 445 | def simxGetUIButtonProperty(clientID, uiHandle, uiButtonID, operationMode): 446 | ''' 447 | Please have a look at the function description/documentation in the V-REP user manual 448 | ''' 449 | 450 | prop = c_int() 451 | return c_GetUIButtonProperty(clientID, uiHandle, uiButtonID, byref(prop), operationMode), prop.value 452 | 453 | def simxSetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode): 454 | ''' 455 | Please have a look at the function description/documentation in the V-REP user manual 456 | ''' 457 | 458 | return c_SetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode) 459 | 460 | def simxAddStatusbarMessage(clientID, message, operationMode): 461 | ''' 462 | Please have a look at the function description/documentation in the V-REP user manual 463 | ''' 464 | 465 | return c_AddStatusbarMessage(clientID, message, operationMode) 466 | 467 | def simxAuxiliaryConsoleOpen(clientID, title, maxLines, mode, position, size, textColor, backgroundColor, operationMode): 468 | ''' 469 | Please have a look at the function description/documentation in the V-REP user manual 470 | ''' 471 | 472 | consoleHandle = c_int() 473 | if position != None: 474 | c_position = (c_int*2)(*position) 475 | else: 476 | c_position = None 477 | if size != None: 478 | c_size = (c_int*2)(*size) 479 | else: 480 | c_size = None 481 | if textColor != None: 482 | c_textColor = (c_float*3)(*textColor) 483 | else: 484 | c_textColor = None 485 | if backgroundColor != None: 486 | c_backgroundColor = (c_float*3)(*backgroundColor) 487 | else: 488 | c_backgroundColor = None 489 | return c_AuxiliaryConsoleOpen(clientID, title, maxLines, mode, c_position, c_size, c_textColor, c_backgroundColor, byref(consoleHandle), operationMode), consoleHandle.value 490 | 491 | def simxAuxiliaryConsoleClose(clientID, consoleHandle, operationMode): 492 | ''' 493 | Please have a look at the function description/documentation in the V-REP user manual 494 | ''' 495 | 496 | return c_AuxiliaryConsoleClose(clientID, consoleHandle, operationMode) 497 | 498 | def simxAuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode): 499 | ''' 500 | Please have a look at the function description/documentation in the V-REP user manual 501 | ''' 502 | 503 | return c_AuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode) 504 | 505 | def simxAuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode): 506 | ''' 507 | Please have a look at the function description/documentation in the V-REP user manual 508 | ''' 509 | 510 | return c_AuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode) 511 | 512 | def simxGetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, operationMode): 513 | ''' 514 | Please have a look at the function description/documentation in the V-REP user manual 515 | ''' 516 | eulerAngles = (c_float*3)() 517 | ret = c_GetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode) 518 | arr = [] 519 | for i in range(3): 520 | arr.append(eulerAngles[i]) 521 | return ret, arr 522 | 523 | def simxGetObjectPosition(clientID, objectHandle, relativeToObjectHandle, operationMode): 524 | ''' 525 | Please have a look at the function description/documentation in the V-REP user manual 526 | ''' 527 | position = (c_float*3)() 528 | ret = c_GetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode) 529 | arr = [] 530 | for i in range(3): 531 | arr.append(position[i]) 532 | return ret, arr 533 | 534 | def simxSetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode): 535 | ''' 536 | Please have a look at the function description/documentation in the V-REP user manual 537 | ''' 538 | 539 | angles = (c_float*3)(*eulerAngles) 540 | return c_SetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, angles, operationMode) 541 | 542 | def simxSetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode): 543 | ''' 544 | Please have a look at the function description/documentation in the V-REP user manual 545 | ''' 546 | 547 | c_position = (c_float*3)(*position) 548 | return c_SetObjectPosition(clientID, objectHandle, relativeToObjectHandle, c_position, operationMode) 549 | 550 | def simxSetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode): 551 | ''' 552 | Please have a look at the function description/documentation in the V-REP user manual 553 | ''' 554 | 555 | return c_SetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode) 556 | 557 | def simxSetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode): 558 | ''' 559 | Please have a look at the function description/documentation in the V-REP user manual 560 | ''' 561 | 562 | return c_SetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode) 563 | 564 | def simxGetLastErrors(clientID, operationMode): 565 | ''' 566 | Please have a look at the function description/documentation in the V-REP user manual 567 | ''' 568 | errors =[] 569 | errorCnt = c_int() 570 | errorStrings = pointer(c_char()) 571 | ret = c_GetLastErrors(clientID, byref(errorCnt), byref(errorStrings), operationMode) 572 | 573 | if ret == 0: 574 | s = 0 575 | for i in range(errorCnt.value): 576 | a = bytearray() 577 | while errorStrings[s] != '\0': 578 | a.append(errorStrings[s]) 579 | s += 1 580 | 581 | s += 1 #skip null 582 | errors.append(str(a)) 583 | 584 | return ret, errors 585 | 586 | def simxGetArrayParameter(clientID, paramIdentifier, operationMode): 587 | ''' 588 | Please have a look at the function description/documentation in the V-REP user manual 589 | ''' 590 | paramValues = (c_float*3)() 591 | ret = c_GetArrayParameter(clientID, paramIdentifier, paramValues, operationMode) 592 | arr = [] 593 | for i in range(3): 594 | arr.append(paramValues[i]) 595 | return ret, arr 596 | 597 | def simxSetArrayParameter(clientID, paramIdentifier, paramValues, operationMode): 598 | ''' 599 | Please have a look at the function description/documentation in the V-REP user manual 600 | ''' 601 | 602 | c_paramValues = (c_float*3)(*paramValues) 603 | return c_SetArrayParameter(clientID, paramIdentifier, c_paramValues, operationMode) 604 | 605 | def simxGetBooleanParameter(clientID, paramIdentifier, operationMode): 606 | ''' 607 | Please have a look at the function description/documentation in the V-REP user manual 608 | ''' 609 | 610 | paramValue = c_ubyte() 611 | return c_GetBooleanParameter(clientID, paramIdentifier, byref(paramValue), operationMode), bool(paramValue.value!=0) 612 | 613 | def simxSetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode): 614 | ''' 615 | Please have a look at the function description/documentation in the V-REP user manual 616 | ''' 617 | 618 | return c_SetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode) 619 | 620 | def simxGetIntegerParameter(clientID, paramIdentifier, operationMode): 621 | ''' 622 | Please have a look at the function description/documentation in the V-REP user manual 623 | ''' 624 | 625 | paramValue = c_int() 626 | return c_GetIntegerParameter(clientID, paramIdentifier, byref(paramValue), operationMode), paramValue.value 627 | 628 | def simxSetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode): 629 | ''' 630 | Please have a look at the function description/documentation in the V-REP user manual 631 | ''' 632 | 633 | return c_SetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode) 634 | 635 | def simxGetFloatingParameter(clientID, paramIdentifier, operationMode): 636 | ''' 637 | Please have a look at the function description/documentation in the V-REP user manual 638 | ''' 639 | 640 | paramValue = c_float() 641 | return c_GetFloatingParameter(clientID, paramIdentifier, byref(paramValue), operationMode), paramValue.value 642 | 643 | def simxSetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode): 644 | ''' 645 | Please have a look at the function description/documentation in the V-REP user manual 646 | ''' 647 | 648 | return c_SetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode) 649 | 650 | def simxGetStringParameter(clientID, paramIdentifier, operationMode): 651 | ''' 652 | Please have a look at the function description/documentation in the V-REP user manual 653 | ''' 654 | paramValue = pointer(c_char()) 655 | ret = c_GetStringParameter(clientID, paramIdentifier, byref(paramValue), operationMode) 656 | 657 | a = bytearray() 658 | if ret == 0: 659 | i = 0 660 | while paramValue[i] != '\0': 661 | a.append(paramValue[i]) 662 | i=i+1 663 | 664 | return ret, str(a) 665 | 666 | def simxGetCollisionHandle(clientID, collisionObjectName, operationMode): 667 | ''' 668 | Please have a look at the function description/documentation in the V-REP user manual 669 | ''' 670 | 671 | handle = c_int() 672 | return c_GetCollisionHandle(clientID, collisionObjectName, byref(handle), operationMode), handle.value 673 | 674 | def simxGetDistanceHandle(clientID, distanceObjectName, operationMode): 675 | ''' 676 | Please have a look at the function description/documentation in the V-REP user manual 677 | ''' 678 | 679 | handle = c_int() 680 | return c_GetDistanceHandle(clientID, distanceObjectName, byref(handle), operationMode), handle.value 681 | 682 | def simxReadCollision(clientID, collisionObjectHandle, operationMode): 683 | ''' 684 | Please have a look at the function description/documentation in the V-REP user manual 685 | ''' 686 | collisionState = c_ubyte() 687 | return c_ReadCollision(clientID, collisionObjectHandle, byref(collisionState), operationMode), bool(collisionState.value!=0) 688 | 689 | def simxReadDistance(clientID, distanceObjectHandle, operationMode): 690 | ''' 691 | Please have a look at the function description/documentation in the V-REP user manual 692 | ''' 693 | 694 | minimumDistance = c_float() 695 | return c_ReadDistance(clientID, distanceObjectHandle, byref(minimumDistance), operationMode), minimumDistance.value 696 | 697 | def simxRemoveObject(clientID, objectHandle, operationMode): 698 | ''' 699 | Please have a look at the function description/documentation in the V-REP user manual 700 | ''' 701 | 702 | return c_RemoveObject(clientID, objectHandle, operationMode) 703 | 704 | def simxRemoveModel(clientID, objectHandle, operationMode): 705 | ''' 706 | Please have a look at the function description/documentation in the V-REP user manual 707 | ''' 708 | 709 | return c_RemoveModel(clientID, objectHandle, operationMode) 710 | 711 | def simxRemoveUI(clientID, uiHandle, operationMode): 712 | ''' 713 | Please have a look at the function description/documentation in the V-REP user manual 714 | ''' 715 | 716 | return c_RemoveUI(clientID, uiHandle, operationMode) 717 | 718 | def simxCloseScene(clientID, operationMode): 719 | ''' 720 | Please have a look at the function description/documentation in the V-REP user manual 721 | ''' 722 | 723 | return c_CloseScene(clientID, operationMode) 724 | 725 | def simxGetObjects(clientID, objectType, operationMode): 726 | ''' 727 | Please have a look at the function description/documentation in the V-REP user manual 728 | ''' 729 | 730 | objectCount = c_int() 731 | objectHandles = pointer(c_int()) 732 | 733 | ret = c_GetObjects(clientID, objectType, byref(objectCount), byref(objectHandles), operationMode) 734 | handles = [] 735 | if ret == 0: 736 | for i in range(objectCount.value): 737 | handles.append(objectHandles[i]) 738 | 739 | return ret, handles 740 | 741 | 742 | def simxDisplayDialog(clientID, titleText, mainText, dialogType, initialText, titleColors, dialogColors, operationMode): 743 | ''' 744 | Please have a look at the function description/documentation in the V-REP user manual 745 | ''' 746 | if titleColors != None: 747 | c_titleColors = (c_float*6)(*titleColors) 748 | else: 749 | c_titleColors = None 750 | if dialogColors != None: 751 | c_dialogColors = (c_float*6)(*dialogColors) 752 | else: 753 | c_dialogColors = None 754 | 755 | c_dialogHandle = c_int() 756 | c_uiHandle = c_int() 757 | return c_DisplayDialog(clientID, titleText, mainText, dialogType, initialText, c_titleColors, c_dialogColors, byref(c_dialogHandle), byref(c_uiHandle), operationMode), c_dialogHandle.value, c_uiHandle.value 758 | 759 | def simxEndDialog(clientID, dialogHandle, operationMode): 760 | ''' 761 | Please have a look at the function description/documentation in the V-REP user manual 762 | ''' 763 | 764 | return c_EndDialog(clientID, dialogHandle, operationMode) 765 | 766 | def simxGetDialogInput(clientID, dialogHandle, operationMode): 767 | ''' 768 | Please have a look at the function description/documentation in the V-REP user manual 769 | ''' 770 | inputText = pointer(c_char()) 771 | ret = c_GetDialogInput(clientID, dialogHandle, byref(inputText), operationMode) 772 | 773 | a = bytearray() 774 | if ret == 0: 775 | i = 0 776 | while inputText[i] != '\0': 777 | a.append(inputText[i]) 778 | i = i+1 779 | 780 | return ret, str(a) 781 | 782 | 783 | def simxGetDialogResult(clientID, dialogHandle, operationMode): 784 | ''' 785 | Please have a look at the function description/documentation in the V-REP user manual 786 | ''' 787 | result = c_int() 788 | return c_GetDialogResult(clientID, dialogHandle, byref(result), operationMode), result.value 789 | 790 | def simxCopyPasteObjects(clientID, objectHandles, operationMode): 791 | ''' 792 | Please have a look at the function description/documentation in the V-REP user manual 793 | ''' 794 | c_objectHandles = (c_int*len(objectHandles))(*objectHandles) 795 | newObjectCount = c_int() 796 | newObjectHandles = pointer(c_int()) 797 | ret = c_CopyPasteObjects(clientID, c_objectHandles, len(objectHandles), byref(newObjectHandles), byref(newObjectCount), operationMode) 798 | 799 | newobj = [] 800 | if ret == 0: 801 | for i in range(newObjectCount.value): 802 | newobj.append(newObjectHandles[i]) 803 | 804 | return ret, newobj 805 | 806 | 807 | def simxGetObjectSelection(clientID, operationMode): 808 | ''' 809 | Please have a look at the function description/documentation in the V-REP user manual 810 | ''' 811 | objectCount = c_int() 812 | objectHandles = pointer(c_int()) 813 | ret = c_GetObjectSelection(clientID, byref(objectHandles), byref(objectCount), operationMode) 814 | 815 | newobj = [] 816 | if ret == 0: 817 | for i in range(objectCount.value): 818 | newobj.append(objectHandles[i]) 819 | 820 | return ret, newobj 821 | 822 | 823 | 824 | def simxSetObjectSelection(clientID, objectHandles, operationMode): 825 | ''' 826 | Please have a look at the function description/documentation in the V-REP user manual 827 | ''' 828 | 829 | c_objectHandles = (c_int*len(objectHandles))(*objectHandles) 830 | return c_SetObjectSelection(clientID, c_objectHandles, len(objectHandles), operationMode) 831 | 832 | def simxClearFloatSignal(clientID, signalName, operationMode): 833 | ''' 834 | Please have a look at the function description/documentation in the V-REP user manual 835 | ''' 836 | 837 | return c_ClearFloatSignal(clientID, signalName, operationMode) 838 | 839 | def simxClearIntegerSignal(clientID, signalName, operationMode): 840 | ''' 841 | Please have a look at the function description/documentation in the V-REP user manual 842 | ''' 843 | 844 | return c_ClearIntegerSignal(clientID, signalName, operationMode) 845 | 846 | def simxClearStringSignal(clientID, signalName, operationMode): 847 | ''' 848 | Please have a look at the function description/documentation in the V-REP user manual 849 | ''' 850 | 851 | return c_ClearStringSignal(clientID, signalName, operationMode) 852 | 853 | def simxGetFloatSignal(clientID, signalName, operationMode): 854 | ''' 855 | Please have a look at the function description/documentation in the V-REP user manual 856 | ''' 857 | 858 | signalValue = c_float() 859 | return c_GetFloatSignal(clientID, signalName, byref(signalValue), operationMode), signalValue.value 860 | 861 | def simxGetIntegerSignal(clientID, signalName, operationMode): 862 | ''' 863 | Please have a look at the function description/documentation in the V-REP user manual 864 | ''' 865 | 866 | signalValue = c_int() 867 | return c_GetIntegerSignal(clientID, signalName, byref(signalValue), operationMode), signalValue.value 868 | 869 | def simxGetStringSignal(clientID, signalName, operationMode): 870 | ''' 871 | Please have a look at the function description/documentation in the V-REP user manual 872 | ''' 873 | 874 | signalLength = c_int(); 875 | signalValue = pointer(c_ubyte()) 876 | ret = c_GetStringSignal(clientID, signalName, byref(signalValue), byref(signalLength), operationMode) 877 | 878 | a = bytearray() 879 | if ret == 0: 880 | for i in range(signalLength.value): 881 | a.append(signalValue[i]) 882 | 883 | return ret, str(a) 884 | 885 | def simxGetAndClearStringSignal(clientID, signalName, operationMode): 886 | ''' 887 | Please have a look at the function description/documentation in the V-REP user manual 888 | ''' 889 | 890 | signalLength = c_int(); 891 | signalValue = pointer(c_ubyte()) 892 | ret = c_GetAndClearStringSignal(clientID, signalName, byref(signalValue), byref(signalLength), operationMode) 893 | 894 | a = bytearray() 895 | if ret == 0: 896 | for i in range(signalLength.value): 897 | a.append(signalValue[i]) 898 | 899 | return ret, str(a) 900 | 901 | def simxReadStringStream(clientID, signalName, operationMode): 902 | ''' 903 | Please have a look at the function description/documentation in the V-REP user manual 904 | ''' 905 | 906 | signalLength = c_int(); 907 | signalValue = pointer(c_ubyte()) 908 | ret = c_ReadStringStream(clientID, signalName, byref(signalValue), byref(signalLength), operationMode) 909 | 910 | a = bytearray() 911 | if ret == 0: 912 | for i in range(signalLength.value): 913 | a.append(signalValue[i]) 914 | 915 | return ret, str(a) 916 | 917 | def simxSetFloatSignal(clientID, signalName, signalValue, operationMode): 918 | ''' 919 | Please have a look at the function description/documentation in the V-REP user manual 920 | ''' 921 | 922 | return c_SetFloatSignal(clientID, signalName, signalValue, operationMode) 923 | 924 | def simxSetIntegerSignal(clientID, signalName, signalValue, operationMode): 925 | ''' 926 | Please have a look at the function description/documentation in the V-REP user manual 927 | ''' 928 | 929 | return c_SetIntegerSignal(clientID, signalName, signalValue, operationMode) 930 | 931 | def simxSetStringSignal(clientID, signalName, signalValue, operationMode): 932 | ''' 933 | Please have a look at the function description/documentation in the V-REP user manual 934 | ''' 935 | 936 | return c_SetStringSignal(clientID, signalName, signalValue, len(signalValue), operationMode) 937 | 938 | def simxAppendStringSignal(clientID, signalName, signalValue, operationMode): 939 | ''' 940 | Please have a look at the function description/documentation in the V-REP user manual 941 | ''' 942 | 943 | return c_AppendStringSignal(clientID, signalName, signalValue, len(signalValue), operationMode) 944 | 945 | def simxWriteStringStream(clientID, signalName, signalValue, operationMode): 946 | ''' 947 | Please have a look at the function description/documentation in the V-REP user manual 948 | ''' 949 | 950 | return c_WriteStringStream(clientID, signalName, signalValue, len(signalValue), operationMode) 951 | 952 | def simxGetObjectFloatParameter(clientID, objectHandle, parameterID, operationMode): 953 | ''' 954 | Please have a look at the function description/documentation in the V-REP user manual 955 | ''' 956 | 957 | parameterValue = c_float() 958 | return c_GetObjectFloatParameter(clientID, objectHandle, parameterID, byref(parameterValue), operationMode), parameterValue.value 959 | 960 | def simxSetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): 961 | ''' 962 | Please have a look at the function description/documentation in the V-REP user manual 963 | ''' 964 | 965 | return c_SetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) 966 | 967 | def simxGetObjectIntParameter(clientID, objectHandle, parameterID, operationMode): 968 | ''' 969 | Please have a look at the function description/documentation in the V-REP user manual 970 | ''' 971 | 972 | parameterValue = c_int() 973 | return c_GetObjectIntParameter(clientID, objectHandle, parameterID, byref(parameterValue), operationMode), parameterValue.value 974 | 975 | def simxSetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): 976 | ''' 977 | Please have a look at the function description/documentation in the V-REP user manual 978 | ''' 979 | 980 | return c_SetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) 981 | 982 | def simxGetModelProperty(clientID, objectHandle, operationMode): 983 | ''' 984 | Please have a look at the function description/documentation in the V-REP user manual 985 | ''' 986 | prop = c_int() 987 | return c_GetModelProperty(clientID, objectHandle, byref(prop), operationMode), prop.value 988 | 989 | def simxSetModelProperty(clientID, objectHandle, prop, operationMode): 990 | ''' 991 | Please have a look at the function description/documentation in the V-REP user manual 992 | ''' 993 | 994 | return c_SetModelProperty(clientID, objectHandle, prop, operationMode) 995 | 996 | def simxStart(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs): 997 | ''' 998 | Please have a look at the function description/documentation in the V-REP user manual 999 | ''' 1000 | 1001 | return c_Start(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs) 1002 | 1003 | def simxFinish(clientID): 1004 | ''' 1005 | Please have a look at the function description/documentation in the V-REP user manual 1006 | ''' 1007 | 1008 | return c_Finish(clientID) 1009 | 1010 | def simxGetPingTime(clientID): 1011 | ''' 1012 | Please have a look at the function description/documentation in the V-REP user manual 1013 | ''' 1014 | pingTime = c_int() 1015 | return c_GetPingTime(clientID, byref(pingTime)), pingTime.value 1016 | 1017 | def simxGetLastCmdTime(clientID): 1018 | ''' 1019 | Please have a look at the function description/documentation in the V-REP user manual 1020 | ''' 1021 | 1022 | return c_GetLastCmdTime(clientID) 1023 | 1024 | def simxSynchronousTrigger(clientID): 1025 | ''' 1026 | Please have a look at the function description/documentation in the V-REP user manual 1027 | ''' 1028 | 1029 | return c_SynchronousTrigger(clientID) 1030 | 1031 | def simxSynchronous(clientID, enable): 1032 | ''' 1033 | Please have a look at the function description/documentation in the V-REP user manual 1034 | ''' 1035 | 1036 | return c_Synchronous(clientID, enable) 1037 | 1038 | def simxPauseCommunication(clientID, enable): 1039 | ''' 1040 | Please have a look at the function description/documentation in the V-REP user manual 1041 | ''' 1042 | 1043 | return c_PauseCommunication(clientID, enable) 1044 | 1045 | def simxGetInMessageInfo(clientID, infoType): 1046 | ''' 1047 | Please have a look at the function description/documentation in the V-REP user manual 1048 | ''' 1049 | info = c_int() 1050 | return c_GetInMessageInfo(clientID, infoType, byref(info)), info.value 1051 | 1052 | def simxGetOutMessageInfo(clientID, infoType): 1053 | ''' 1054 | Please have a look at the function description/documentation in the V-REP user manual 1055 | ''' 1056 | info = c_int() 1057 | return c_GetOutMessageInfo(clientID, infoType, byref(info)), info.value 1058 | 1059 | def simxGetConnectionId(clientID): 1060 | ''' 1061 | Please have a look at the function description/documentation in the V-REP user manual 1062 | ''' 1063 | 1064 | return c_GetConnectionId(clientID) 1065 | 1066 | def simxCreateBuffer(bufferSize): 1067 | ''' 1068 | Please have a look at the function description/documentation in the V-REP user manual 1069 | ''' 1070 | 1071 | return c_CreateBuffer(bufferSize) 1072 | 1073 | def simxReleaseBuffer(buffer): 1074 | ''' 1075 | Please have a look at the function description/documentation in the V-REP user manual 1076 | ''' 1077 | 1078 | return c_ReleaseBuffer(buffer) 1079 | 1080 | def simxTransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode): 1081 | ''' 1082 | Please have a look at the function description/documentation in the V-REP user manual 1083 | ''' 1084 | 1085 | return c_TransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode) 1086 | 1087 | def simxEraseFile(clientID, fileName_serverSide, operationMode): 1088 | ''' 1089 | Please have a look at the function description/documentation in the V-REP user manual 1090 | ''' 1091 | 1092 | return c_EraseFile(clientID, fileName_serverSide, operationMode) 1093 | 1094 | def simxCreateDummy(clientID, size, color, operationMode): 1095 | ''' 1096 | Please have a look at the function description/documentation in the V-REP user manual 1097 | ''' 1098 | 1099 | handle = c_int() 1100 | if color != None: 1101 | c_color = (c_ubyte*12)(*color) 1102 | else: 1103 | c_color = None 1104 | return c_CreateDummy(clientID, size, c_color, byref(handle), operationMode), handle.value 1105 | 1106 | def simxQuery(clientID, signalName, signalValue, retSignalName, timeOutInMs): 1107 | ''' 1108 | Please have a look at the function description/documentation in the V-REP user manual 1109 | ''' 1110 | 1111 | retSignalLength = c_int(); 1112 | retSignalValue = pointer(c_ubyte()) 1113 | 1114 | ret = c_Query(clientID, signalName, signalValue, len(signalValue), retSignalName, byref(retSignalValue), byref(retSignalLength), timeOutInMs) 1115 | 1116 | a = bytearray() 1117 | if ret == 0: 1118 | for i in range(retSignalLength.value): 1119 | a.append(retSignalValue[i]) 1120 | 1121 | return ret, str(a) 1122 | 1123 | def simxGetObjectGroupData(clientID, objectType, dataType, operationMode): 1124 | ''' 1125 | Please have a look at the function description/documentation in the V-REP user manual 1126 | ''' 1127 | 1128 | handles =[] 1129 | intData =[] 1130 | floatData =[] 1131 | stringData =[] 1132 | handlesC = c_int() 1133 | handlesP = pointer(c_int()) 1134 | intDataC = c_int() 1135 | intDataP = pointer(c_int()) 1136 | floatDataC = c_int() 1137 | floatDataP = pointer(c_float()) 1138 | stringDataC = c_int() 1139 | stringDataP = pointer(c_char()) 1140 | ret = c_GetObjectGroupData(clientID, objectType, dataType, byref(handlesC), byref(handlesP), byref(intDataC), byref(intDataP), byref(floatDataC), byref(floatDataP), byref(stringDataC), byref(stringDataP), operationMode) 1141 | 1142 | if ret == 0: 1143 | for i in range(handlesC.value): 1144 | handles.append(handlesP[i]) 1145 | for i in range(intDataC.value): 1146 | intData.append(intDataP[i]) 1147 | for i in range(floatDataC.value): 1148 | floatData.append(floatDataP[i]) 1149 | s = 0 1150 | for i in range(stringDataC.value): 1151 | a = bytearray() 1152 | while stringDataP[s] != '\0': 1153 | a.append(stringDataP[s]) 1154 | s += 1 1155 | s += 1 #skip null 1156 | stringData.append(str(a)) 1157 | 1158 | return ret, handles, intData, floatData, stringData 1159 | 1160 | def simxGetObjectVelocity(clientID, objectHandle, operationMode): 1161 | ''' 1162 | Please have a look at the function description/documentation in the V-REP user manual 1163 | ''' 1164 | linearVel = (c_float*3)() 1165 | angularVel = (c_float*3)() 1166 | ret = c_GetObjectVelocity(clientID, objectHandle, linearVel, angularVel, operationMode) 1167 | arr1 = [] 1168 | for i in range(3): 1169 | arr1.append(linearVel[i]) 1170 | arr2 = [] 1171 | for i in range(3): 1172 | arr2.append(angularVel[i]) 1173 | return ret, arr1, arr2 1174 | 1175 | def simxPackInts(intList): 1176 | ''' 1177 | Please have a look at the function description/documentation in the V-REP user manual 1178 | ''' 1179 | s='' 1180 | for i in range(len(intList)): 1181 | s+=struct.pack('