├── .gitignore ├── LICENSE ├── README.md ├── UR5_DH.ttt ├── ik_ur5.py ├── remoteApi.dll ├── sim.py ├── simConst.py ├── ur5_final.png └── ur5_home.png /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Isaac Ayala 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Inverse Kinematics UR5 2 | Author: Isaac Ayala 3 | 4 | ## Description 5 | 6 | A Python implementation of the inverse kinematics of the UR5 is presented. 7 | 8 | `ik_ur5.py` : Main script to execute. Gets the parameters for the UR5 and solves the inverse kinematics of the system for the position written in the code. Connects to CoppeliaSim and sets the new target location. Returns the final position of the robot's end effector. 9 | 10 | `sim.py`, `simConst.py` `remoteApi.dll`, `remoteApi.so`: CoppeliaSim libraries to connect with Python. 11 | 12 | `UR5_DH.ttr` : CoppeliaSim robot scene for the DH parameters. Use this scene for simulations. 13 | 14 | ## Requirements 15 | 16 | `Spyder4` inside conda environment base 17 | 18 | `CoppeliaSim 4.X` 19 | 20 | ## Execution 21 | 22 | 1. Load the `UR5_DH.ttr` scene in CoppeliaSim and run the simulation. 23 | 2. Run the script `ik_ur5.py` in Spyder. Check that the simulation in CoppeliaSim wasn't aborted. If it stopped, just press the "play" button again in CoppeliaSim, it will pick up from where it left off. 24 | 25 | Note: you can run the Python script again without resetting or stopping the simulation in CoppeliaSim (Tested in Windows 10). 26 | 27 | 28 | ## Results 29 | 30 | Robot moves to the desired position/configuration from the home configuration in the DH convention. 31 | 32 | + Original position/configuration 33 | 34 | ![](ur5_home.png) 35 | 36 | + Final position/configuration 37 | ![](ur5_final.png) 38 | 39 | ## References 40 | 41 | + [Denavit-Hartenberg parameters for the UR5](https://www.universal-robots.com/articles/ur-articles/parameters-for-calculations-of-kinematics-and-dynamics/) 42 | + [Modern Robotics](http://hades.mech.northwestern.edu/index.php/Modern_Robotics) 43 | + Notes on the Geometric Jacobian by Alvaro Paz and Gustavo Arechavaleta 44 | -------------------------------------------------------------------------------- /UR5_DH.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/der-coder/hw01_UR5_coppeliaSim/c6d21b6eae57c359af094c084e00d00de4e1ba86/UR5_DH.ttt -------------------------------------------------------------------------------- /ik_ur5.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon May 25 04:30:27 2020 4 | 5 | @author: Isaac Ayala 6 | """ 7 | 8 | # Load coppeliaSim library 9 | try: 10 | import sim 11 | except: 12 | print ('--------------------------------------------------------------') 13 | print ('"sim.py" could not be imported. This means very probably that') 14 | print ('either "sim.py" or the remoteApi library could not be found.') 15 | print ('Make sure both are in the same folder as this file,') 16 | print ('or appropriately adjust the file "sim.py"') 17 | print ('--------------------------------------------------------------') 18 | print ('') 19 | 20 | # Load other libraries for delays and specifying coordinates as multiples of pi 21 | import time 22 | import math as m 23 | import numpy as np 24 | 25 | def ur5_params(): 26 | """ 27 | Returns 28 | ------- 29 | home_position : Home position of the UR5 with DH parameters 30 | screw : Screw matrix for UR5 with DH parameters 31 | """ 32 | # UR5 link parameters (in meters) 33 | l1 = 0.425 34 | l2 = 0.392 35 | h1 = 0.160 36 | h2 = 0.09475 37 | w1 = 0.134 38 | w2 = 0.0815 39 | 40 | home_position = np.array([ 41 | [-1, 0, 0, l1 + l2], 42 | [0, 0, 1, w1 + w2], 43 | [0, 1, 0, h1 - h2], 44 | [0, 0, 0, 1] 45 | ]) 46 | 47 | screw = np.array([ 48 | [0,0,1,0,0,0], 49 | [0,1,0,-h1,0,0], 50 | [0,1,0,-h1, 0,l1], 51 | [0,1,0, -h1, 0, l1+l2], 52 | [0,0,-1, -w1, l1+l2, 0], 53 | [0,1,0, h2-h1, 0, l1+l2] 54 | ]) 55 | return home_position, screw 56 | 57 | def crossProductOperator(vector): 58 | w1,w2,w3 = vector 59 | W = [ 60 | [ 0, -w3, w2], 61 | [ w3, 0, -w1], 62 | [-w2, w1, 0] 63 | ] 64 | Ax = np.asarray(W) 65 | return Ax 66 | 67 | def exponential_map(action_axis, theta): 68 | action_axis = np.asarray(action_axis) 69 | linear = action_axis[3:] 70 | angular = action_axis[:3] 71 | 72 | exp_rot = exponential_form_rotation(angular, theta) 73 | exp_tras = exponential_form_traslation(linear, angular, theta) 74 | 75 | expMap = np.block([[exp_rot, exp_tras], [np.zeros( (1,3) ), 1]]) 76 | return(expMap) 77 | 78 | def exponential_form_rotation(angular, theta): 79 | c = crossProductOperator(angular) @ crossProductOperator(angular) 80 | expRot = np.eye(3) + np.sin(theta) * crossProductOperator(angular) + ( 1-np.cos(theta) ) * c 81 | return expRot 82 | 83 | def exponential_form_traslation(linear, angular, theta): 84 | l1, l2, l3 = linear 85 | lin = np.array([[l1, l2, l3]]) 86 | angular_mat = crossProductOperator(angular) 87 | c = angular_mat @ angular_mat 88 | expTras = (theta * np.eye(3) + ( 1 - np.cos(theta) ) * angular_mat + ( theta - np.sin(theta) ) * c) @ (lin.transpose()) 89 | return expTras 90 | 91 | def Ad(mat4): 92 | mat4 = np.asarray(mat4) 93 | rot = mat4[:3, :3] 94 | tras = mat4[0:3, 3] 95 | Ad = np.block([[rot, crossProductOperator(tras) @ rot],[np.zeros((3,3)), rot]]) 96 | return(Ad) 97 | 98 | def denavit_transformation(theta, index): 99 | """ 100 | Computes the homogeneous transformation according to the Denavit-Hatenberg convention 101 | 102 | Parameters 103 | ---------- 104 | theta : Rotation in z-axis [radians]. 105 | 106 | Internal variables 107 | ------------------ 108 | d : Distance between x-axes in meters. 109 | alpha : Angle between z_1 and and z_0 axes. 110 | r : Distance between z-axes. 111 | 112 | Returns 113 | ------- 114 | G : Homogeneous transformation. 115 | 116 | """ 117 | 118 | d = [0.089159, 0, 0, 0.10915, 0.09465, 0.0523] 119 | alpha = [m.pi/2, 0, 0, m.pi/2, -m.pi/2, 0] 120 | r = [0, -0.425, -0.3922, 0, 0, 0] 121 | 122 | c_theta = m.cos(theta) 123 | s_theta = m.sin(theta) 124 | c_alpha = m.cos(alpha[index]) 125 | s_alpha = m.sin(alpha[index]) 126 | 127 | # print('DH Values: ', c_theta, s_theta, c_alpha, s_alpha) 128 | 129 | R_z = np.array([ 130 | [c_theta, -s_theta, 0, 0], 131 | [s_theta, c_theta, 0, 0], 132 | [0, 0, 1 ,0], 133 | [0, 0, 0, 1] 134 | ]) # DH rotation z-axis 135 | T_z = np.array([ 136 | [1, 0 ,0, 0], 137 | [0, 1, 0, 0], 138 | [0, 0, 1 , d[index]], 139 | [0, 0, 0, 1] 140 | ]) # DH translation z-axis 141 | T_x = np.array([ 142 | [1, 0, 0, r[index]], 143 | [0, 1, 0, 0], 144 | [0, 0, 1, 0], 145 | [0, 0, 0, 1] 146 | ]) # DH translation x-axis 147 | R_x = np.array([ 148 | [1, 0, 0, 0], 149 | [0, c_alpha, -s_alpha, 0], 150 | [0, s_alpha, c_alpha, 0], 151 | [0, 0, 0, 1] 152 | ]) # DH rotation x-axis 153 | 154 | # print(R_z, T_z, T_x, R_x) 155 | 156 | G = R_z @ T_z @ T_x @ R_x 157 | 158 | return G 159 | 160 | def compute_jacobian(theta, screw, dof=6 ): 161 | # Compute DH transformations 162 | # Compute exponential maps too 163 | 164 | G_local = [] 165 | expMap_local = [] 166 | for i in range(dof): 167 | G_local.append(denavit_transformation(theta[i], i)) 168 | expMap_local.append(G_local[i] @ exponential_map(screw[i], theta[i])) 169 | 170 | # Get G for the adjoint operator 171 | G = [] 172 | for i in range(dof): 173 | if i == 0: 174 | g = np.eye(4) 175 | else: 176 | g = g @ G[i-1] 177 | G.append(g @ expMap_local[i]) 178 | 179 | # Get space Jacobian 180 | J_s = [] 181 | for i in range(6): 182 | J_s.append(Ad(G[i]) @ screw[i]) 183 | 184 | # print('Space : \n', J_s) 185 | 186 | # Get location of end effector tip (p_tip) 187 | p_k = np.zeros((3,1)) 188 | # p_k = np.array([[0],[0],[0.5]]) 189 | 190 | p_0_extended = G[5] @ np.block([[p_k],[1]]) 191 | p_0 = p_0_extended[:3] 192 | 193 | p_0_cpo = np.array([[0, -p_0[2], p_0[1]],[p_0[2], 0, -p_0[0]],[-p_0[1], p_0[0], 0]]) 194 | 195 | # Geometric Jacobian 196 | """ 197 | 198 | The geometric Jacobian is obtained from the spatial Jacobian and the vector p_tip 199 | 200 | p_tip : tip point of the end effector 201 | p_0^tip : p measured from the inertial reference frame 202 | p_k^tip : p measured from the frame k, if k is the end effector and the tip is at its origin then p_k = 0 203 | 204 | [p_0^tip; 1] = G_0^k [p_k^tip; 1] 205 | 206 | """ 207 | J_g = np.block([[np.eye(3), -p_0_cpo],[np.zeros((3,3)), np.eye(3)]]) @ J_s 208 | 209 | # print('Geometric : \n', J_g) 210 | 211 | # Get Roll, Pitch, Yaw coordinates 212 | R = G[5][0:3][0:3] 213 | 214 | r_roll = m.atan2(R[2][1],R[2][2]) 215 | r_pitch = m.atan2(-R[2][0],m.sqrt(R[2][2]*R[2][2] + R[2][1]*R[2][1])) 216 | r_yaw = m.atan2(R[1][0],R[0][0]) 217 | 218 | # Build kinematic operator for Roll, Pitch, Yaw configuration 219 | # Taken from Olguin's formulaire book 220 | 221 | B = np.array( 222 | [ 223 | [m.cos(r_pitch) * m.cos(r_yaw), -m.sin(r_yaw), 0], 224 | [m.cos(r_pitch) * m.cos(r_yaw), m.cos(r_yaw), 0], 225 | [- m.sin(r_pitch), 0, 1] 226 | ] 227 | ) 228 | 229 | # print('Kinematic : \n', B) 230 | 231 | # Get Analytic Jacobian 232 | """ 233 | 234 | Obtained from function 235 | 236 | J_a(q) = [[I 0],[0, B(alpha)^-1]] J_g(q) 237 | 238 | B(alpha) = for roll, pitch, yaw 239 | 240 | """ 241 | J_a = np.block( 242 | [ 243 | [np.eye(3), np.zeros((3, 3))], 244 | [np.zeros((3,3)), np.linalg.inv(B)] 245 | ] 246 | ) 247 | 248 | return J_a 249 | 250 | def compute_e(theta_d, theta_0, dof, home_position, screw): 251 | T_0 = compute_T(screw, theta_0, dof, home_position) 252 | T_d = compute_T(screw, theta_d, dof, home_position) 253 | e = T_d - T_0 254 | 255 | # e = theta_d - theta_0 256 | return e 257 | 258 | def root_finding(theta_0, theta_d, tryMax, dof, home_position, screw): 259 | 260 | n_try = 1; # Count number of iterations 261 | tol = 0.0001; # error tolerance 262 | theta = theta_0 263 | e = compute_e(theta_d, theta, dof, home_position, screw) # Gets error from the transformation matrix 264 | 265 | while n_try < tryMax and np.linalg.norm(e) > tol : 266 | ja = compute_jacobian(theta, screw) 267 | j_temp = np.zeros((6,6)) 268 | for i in range(6): 269 | for j in range (6): 270 | j_temp[i][j] = ja[i][j] 271 | 272 | inverse_jacobian = np.linalg.inv(j_temp) 273 | theta = theta + inverse_jacobian @ (theta_d - theta) 274 | e = compute_e(theta_d, theta, dof, home_position, screw) 275 | n_try += 1 276 | return theta, e, n_try 277 | 278 | 279 | def compute_T(screw, theta, dof, M): 280 | """ 281 | 282 | 283 | Parameters 284 | ---------- 285 | screw : screw matrix. 286 | theta : coordinates. 287 | dof : robot degrees of freedom. 288 | M : 4 x 4 matrix describing the position of the end effector at theta_i = 0. 289 | 290 | Returns 291 | ------- 292 | T : New end effector position. 293 | 294 | """ 295 | 296 | expMap_local = [] 297 | T = np.eye(4) 298 | for i in range(dof): 299 | expMap_local.append(exponential_map(screw[i], theta[i])) 300 | T = T @ expMap_local[i] 301 | T = T @ M 302 | return T 303 | 304 | def main(): 305 | np.set_printoptions(precision=3, suppress=True) # Restrict decimals in console output 306 | 307 | dof = 6 308 | home_position, screw = ur5_params() # Get UR5 parameters 309 | 310 | theta_0 = np.array([0, 0, 0, 0, 0, 0]) # Initial position 311 | theta_d = np.array([0,-m.pi/2, 0, 0, m.pi/2, 0]) # Desired position, converted to x_d in the solver 312 | 313 | T_0 = compute_T(screw, theta_0, dof, home_position) 314 | T_d = compute_T(screw, theta_d, dof, home_position) 315 | 316 | print("Home position: \n", T_0, "\n") 317 | print("Desired position: \n", T_d, "\n") 318 | 319 | # Find solution to the system 320 | theta, delta, n = root_finding(theta_0, theta_d, 20, dof, home_position, screw) 321 | T_theta = compute_T(screw, theta, dof, home_position) 322 | 323 | print('Solution : \n', theta, '\n', 'Transformation : \n', T_theta, '\n') 324 | 325 | R = T_theta[0:3][0:3] # Get RPY for the solution 326 | 327 | r_roll = m.atan2(R[2][1],R[2][2]) 328 | r_pitch = m.atan2(-R[2][0],m.sqrt(R[2][2]*R[2][2] + R[2][1]*R[2][1])) 329 | r_yaw = m.atan2(R[1][0],R[0][0]) 330 | 331 | # Begin connection with coppeliaSim. 332 | # Robot simulation must be running in coppeliaSim to connect 333 | sim.simxFinish(-1) # just in case, close all opened connections 334 | clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim 335 | 336 | # clientID stores the ID assingned by coppeliaSim, if the connection failed it will be assigned -1 337 | if clientID!=-1: 338 | print ('Connected to remote API server') 339 | 340 | # Get ID handles for each joint of the robot 341 | res,UR5_joint1 =sim.simxGetObjectHandle( clientID, 'UR5_joint1', sim.simx_opmode_blocking) 342 | res,UR5_joint2 =sim.simxGetObjectHandle( clientID, 'UR5_joint2', sim.simx_opmode_blocking) 343 | res,UR5_joint3 =sim.simxGetObjectHandle( clientID, 'UR5_joint3', sim.simx_opmode_blocking) 344 | res,UR5_joint4 =sim.simxGetObjectHandle( clientID, 'UR5_joint4', sim.simx_opmode_blocking) 345 | res,UR5_joint5 =sim.simxGetObjectHandle( clientID, 'UR5_joint5', sim.simx_opmode_blocking) 346 | res,UR5_joint6 =sim.simxGetObjectHandle( clientID, 'UR5_joint6', sim.simx_opmode_blocking) 347 | 348 | res,UR5_endEffector =sim.simxGetObjectHandle( clientID, 'UR5_connection', sim.simx_opmode_blocking) 349 | 350 | # Store the handles as a list 351 | UR5 = [UR5_joint1, UR5_joint2, UR5_joint3, UR5_joint4, UR5_joint5, UR5_joint6] # Just grab the first 3 for now to test 352 | 353 | # Get current coordinates of each joint 354 | UR5_q = [] 355 | for joint in UR5: 356 | res, value = sim.simxGetJointPosition(clientID, joint, sim.simx_opmode_oneshot) 357 | UR5_q.append(value) # Store current values 358 | 359 | # Set new coordinates for the robot in coppeliaSim 360 | # Add time delays so the animation can be displayed correctly 361 | steps = 100 362 | position_desired = theta 363 | 364 | for t in range(steps): 365 | i = 0 366 | k = 0 367 | for joint in UR5: 368 | sim.simxSetJointTargetPosition(clientID, joint, t*(position_desired[i])/steps, sim.simx_opmode_streaming) 369 | res, value = sim.simxGetJointPosition(clientID, joint, sim.simx_opmode_oneshot) 370 | if t == 0 or t == 99: 371 | k += 1 372 | res, position = sim.simxGetObjectPosition(clientID, UR5_endEffector, UR5_joint1, sim.simx_opmode_oneshot) 373 | res, orientation = sim.simxGetObjectOrientation(clientID, UR5_endEffector, UR5_joint1, sim.simx_opmode_oneshot) 374 | i += 1 375 | time.sleep(2/steps) 376 | 377 | # Convert robot angles to the 4x4 matrix representation for comparison 378 | c1, s1 = np.cos(orientation[0]), np.sin(orientation[0]) 379 | c2, s2 = np.cos(orientation[1]), np.sin(orientation[1]) 380 | c3, s3 = np.cos(orientation[2]), np.sin(orientation[2]) 381 | 382 | R_ur5 = np.block([ 383 | [c2 * c3, -c2 * s3, s2], 384 | [c1 * s3 + c3 * s1 * s2, c1 * c3 - s1 * s2 * s3, -c2 * s1], 385 | [s1 * s3 - c1 * c3 * s2, c3 * s1 + c1 * s2 * s3, c1 * c2] 386 | ]) 387 | 388 | p_ur5 = np.array(position).reshape((3,1)) 389 | 390 | T_ur5 = np.block([ 391 | [R_ur5, p_ur5], 392 | [0, 0, 0, 1] 393 | ]) 394 | 395 | print('\n Robot coordinates: \n ', T_ur5 ) 396 | 397 | # print('\n Absolute Error : \n', abs(T_theta - T_ur5)) 398 | 399 | time.sleep(1) 400 | 401 | # print('\n Returning to home position...') 402 | # for joint in UR5: 403 | # sim.simxSetJointTargetPosition(clientID, joint, 0, sim.simx_opmode_streaming) 404 | 405 | # Now send some data to CoppeliaSim in a non-blocking fashion: 406 | sim.simxAddStatusbarMessage(clientID,'Excuting forward kinematics in Python',sim.simx_opmode_oneshot) 407 | 408 | # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example): 409 | sim.simxGetPingTime(clientID) 410 | 411 | # Now close the connection to CoppeliaSim: 412 | sim.simxFinish(clientID) 413 | 414 | 415 | if __name__ == '__main__': 416 | main() 417 | -------------------------------------------------------------------------------- /remoteApi.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/der-coder/hw01_UR5_coppeliaSim/c6d21b6eae57c359af094c084e00d00de4e1ba86/remoteApi.dll -------------------------------------------------------------------------------- /sim.py: -------------------------------------------------------------------------------- 1 | import platform 2 | import struct 3 | import sys 4 | import os 5 | import ctypes as ct 6 | from simConst import * 7 | 8 | #load library 9 | libsimx = None 10 | try: 11 | file_extension = '.so' 12 | if platform.system() =='cli': 13 | file_extension = '.dll' 14 | elif platform.system() =='Windows': 15 | file_extension = '.dll' 16 | elif platform.system() == 'Darwin': 17 | file_extension = '.dylib' 18 | else: 19 | file_extension = '.so' 20 | libfullpath = os.path.join(os.path.dirname(__file__), 'remoteApi' + file_extension) 21 | libsimx = ct.CDLL(libfullpath) 22 | except: 23 | print ('----------------------------------------------------') 24 | print ('The remoteApi library could not be loaded. Make sure') 25 | print ('it is located in the same folder as "sim.py", or') 26 | print ('appropriately adjust the file "sim.py"') 27 | print ('----------------------------------------------------') 28 | print ('') 29 | 30 | #ctypes wrapper prototypes 31 | c_GetJointPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointPosition", libsimx)) 32 | c_SetJointPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointPosition", libsimx)) 33 | c_GetJointMatrix = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointMatrix", libsimx)) 34 | c_SetSphericalJointMatrix = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetSphericalJointMatrix", libsimx)) 35 | c_SetJointTargetVelocity = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointTargetVelocity", libsimx)) 36 | c_SetJointTargetPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointTargetPosition", libsimx)) 37 | c_GetJointForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointForce", libsimx)) 38 | c_GetJointMaxForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointMaxForce", libsimx)) 39 | c_SetJointForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointMaxForce", libsimx)) 40 | c_SetJointMaxForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointMaxForce", libsimx)) 41 | c_ReadForceSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.c_int32)(("simxReadForceSensor", libsimx)) 42 | c_BreakForceSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxBreakForceSensor", libsimx)) 43 | c_ReadVisionSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.POINTER(ct.c_float)), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxReadVisionSensor", libsimx)) 44 | c_GetObjectHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectHandle", libsimx)) 45 | c_GetVisionSensorImage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_byte)), ct.c_ubyte, ct.c_int32)(("simxGetVisionSensorImage", libsimx)) 46 | c_SetVisionSensorImage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_byte), ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetVisionSensorImage", libsimx)) 47 | c_GetVisionSensorDepthBuffer= ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)), ct.c_int32)(("simxGetVisionSensorDepthBuffer", libsimx)) 48 | c_GetObjectChild = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectChild", libsimx)) 49 | c_GetObjectParent = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectParent", libsimx)) 50 | c_ReadProximitySensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_float), ct.c_int32)(("simxReadProximitySensor", libsimx)) 51 | c_LoadModel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.POINTER(ct.c_int32), ct.c_int32)(("simxLoadModel", libsimx)) 52 | c_LoadUI = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxLoadUI", libsimx)) 53 | c_LoadScene = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.c_int32)(("simxLoadScene", libsimx)) 54 | c_StartSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxStartSimulation", libsimx)) 55 | c_PauseSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxPauseSimulation", libsimx)) 56 | c_StopSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxStopSimulation", libsimx)) 57 | c_GetUIHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIHandle", libsimx)) 58 | c_GetUISlider = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUISlider", libsimx)) 59 | c_SetUISlider = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetUISlider", libsimx)) 60 | c_GetUIEventButton = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIEventButton", libsimx)) 61 | c_GetUIButtonProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIButtonProperty", libsimx)) 62 | c_SetUIButtonProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetUIButtonProperty", libsimx)) 63 | c_AddStatusbarMessage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxAddStatusbarMessage", libsimx)) 64 | c_AuxiliaryConsoleOpen = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.c_int32)(("simxAuxiliaryConsoleOpen", libsimx)) 65 | c_AuxiliaryConsoleClose = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxAuxiliaryConsoleClose", libsimx)) 66 | c_AuxiliaryConsolePrint = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxAuxiliaryConsolePrint", libsimx)) 67 | c_AuxiliaryConsoleShow = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxAuxiliaryConsoleShow", libsimx)) 68 | c_GetObjectOrientation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectOrientation", libsimx)) 69 | c_GetObjectQuaternion = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectQuaternion", libsimx)) 70 | c_GetObjectPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectPosition", libsimx)) 71 | c_SetObjectOrientation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectOrientation", libsimx)) 72 | c_SetObjectQuaternion = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectQuaternion", libsimx)) 73 | c_SetObjectPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectPosition", libsimx)) 74 | c_SetObjectParent = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetObjectParent", libsimx)) 75 | c_SetUIButtonLabel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32)(("simxSetUIButtonLabel", libsimx)) 76 | c_GetLastErrors = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetLastErrors", libsimx)) 77 | c_GetArrayParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetArrayParameter", libsimx)) 78 | c_SetArrayParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetArrayParameter", libsimx)) 79 | c_GetBooleanParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxGetBooleanParameter", libsimx)) 80 | c_SetBooleanParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetBooleanParameter", libsimx)) 81 | c_GetIntegerParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetIntegerParameter", libsimx)) 82 | c_SetIntegerParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetIntegerParameter", libsimx)) 83 | c_GetFloatingParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetFloatingParameter", libsimx)) 84 | c_SetFloatingParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetFloatingParameter", libsimx)) 85 | c_GetStringParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetStringParameter", libsimx)) 86 | c_GetCollisionHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetCollisionHandle", libsimx)) 87 | c_GetDistanceHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetDistanceHandle", libsimx)) 88 | c_GetCollectionHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetCollectionHandle", libsimx)) 89 | c_ReadCollision = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxReadCollision", libsimx)) 90 | c_ReadDistance = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxReadDistance", libsimx)) 91 | c_RemoveObject = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveObject", libsimx)) 92 | c_RemoveModel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveModel", libsimx)) 93 | c_RemoveUI = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveUI", libsimx)) 94 | c_CloseScene = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxCloseScene", libsimx)) 95 | c_GetObjects = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxGetObjects", libsimx)) 96 | c_DisplayDialog = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.c_int32)(("simxDisplayDialog", libsimx)) 97 | c_EndDialog = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxEndDialog", libsimx)) 98 | c_GetDialogInput = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetDialogInput", libsimx)) 99 | c_GetDialogResult = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetDialogResult", libsimx)) 100 | c_CopyPasteObjects = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32, ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxCopyPasteObjects", libsimx)) 101 | c_GetObjectSelection = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectSelection", libsimx)) 102 | c_SetObjectSelection = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32, ct.c_int32)(("simxSetObjectSelection", libsimx)) 103 | c_ClearFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearFloatSignal", libsimx)) 104 | c_ClearIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearIntegerSignal", libsimx)) 105 | c_ClearStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearStringSignal", libsimx)) 106 | c_GetFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_float), ct.c_int32)(("simxGetFloatSignal", libsimx)) 107 | c_GetIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetIntegerSignal", libsimx)) 108 | c_GetStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetStringSignal", libsimx)) 109 | c_SetFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_float, ct.c_int32)(("simxSetFloatSignal", libsimx)) 110 | c_SetIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32)(("simxSetIntegerSignal", libsimx)) 111 | c_SetStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxSetStringSignal", libsimx)) 112 | c_AppendStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxAppendStringSignal", libsimx)) 113 | c_WriteStringStream = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxWriteStringStream", libsimx)) 114 | c_GetObjectFloatParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectFloatParameter", libsimx)) 115 | c_SetObjectFloatParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetObjectFloatParameter", libsimx)) 116 | c_GetObjectIntParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectIntParameter", libsimx)) 117 | c_SetObjectIntParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetObjectIntParameter", libsimx)) 118 | c_GetModelProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetModelProperty", libsimx)) 119 | c_SetModelProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetModelProperty", libsimx)) 120 | c_Start = ct.CFUNCTYPE(ct.c_int32,ct.POINTER(ct.c_char), ct.c_int32, ct.c_ubyte, ct.c_ubyte, ct.c_int32, ct.c_int32)(("simxStart", libsimx)) 121 | c_Finish = ct.CFUNCTYPE(None, ct.c_int32)(("simxFinish", libsimx)) 122 | c_GetPingTime = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetPingTime", libsimx)) 123 | c_GetLastCmdTime = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxGetLastCmdTime", libsimx)) 124 | c_SynchronousTrigger = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxSynchronousTrigger", libsimx)) 125 | c_Synchronous = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_ubyte)(("simxSynchronous", libsimx)) 126 | c_PauseCommunication = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_ubyte)(("simxPauseCommunication", libsimx)) 127 | c_GetInMessageInfo = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetInMessageInfo", libsimx)) 128 | c_GetOutMessageInfo = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetOutMessageInfo", libsimx)) 129 | c_GetConnectionId = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxGetConnectionId", libsimx)) 130 | c_CreateBuffer = ct.CFUNCTYPE(ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxCreateBuffer", libsimx)) 131 | c_ReleaseBuffer = ct.CFUNCTYPE(None, ct.c_void_p)(("simxReleaseBuffer", libsimx)) 132 | c_TransferFile = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32)(("simxTransferFile", libsimx)) 133 | c_EraseFile = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxEraseFile", libsimx)) 134 | c_GetAndClearStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetAndClearStringSignal", libsimx)) 135 | c_ReadStringStream = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxReadStringStream", libsimx)) 136 | c_CreateDummy = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_float, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_int32), ct.c_int32)(("simxCreateDummy", libsimx)) 137 | c_Query = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxQuery", libsimx)) 138 | c_GetObjectGroupData = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetObjectGroupData", libsimx)) 139 | c_GetObjectVelocity = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectVelocity", libsimx)) 140 | c_CallScriptFunction = ct.CFUNCTYPE(ct.c_int32,ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_int32),ct.c_int32,ct.POINTER(ct.c_float),ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_ubyte),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_ubyte)),ct.c_int32)(("simxCallScriptFunction", libsimx)) 141 | 142 | #API functions 143 | def simxGetJointPosition(clientID, jointHandle, operationMode): 144 | ''' 145 | Please have a look at the function description/documentation in the CoppeliaSim user manual 146 | ''' 147 | position = ct.c_float() 148 | return c_GetJointPosition(clientID, jointHandle, ct.byref(position), operationMode), position.value 149 | 150 | def simxSetJointPosition(clientID, jointHandle, position, operationMode): 151 | ''' 152 | Please have a look at the function description/documentation in the CoppeliaSim user manual 153 | ''' 154 | 155 | return c_SetJointPosition(clientID, jointHandle, position, operationMode) 156 | 157 | def simxGetJointMatrix(clientID, jointHandle, operationMode): 158 | ''' 159 | Please have a look at the function description/documentation in the CoppeliaSim user manual 160 | ''' 161 | matrix = (ct.c_float*12)() 162 | ret = c_GetJointMatrix(clientID, jointHandle, matrix, operationMode) 163 | arr = [] 164 | for i in range(12): 165 | arr.append(matrix[i]) 166 | return ret, arr 167 | 168 | def simxSetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode): 169 | ''' 170 | Please have a look at the function description/documentation in the CoppeliaSim user manual 171 | ''' 172 | matrix = (ct.c_float*12)(*matrix) 173 | return c_SetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode) 174 | 175 | def simxSetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode): 176 | ''' 177 | Please have a look at the function description/documentation in the CoppeliaSim user manual 178 | ''' 179 | 180 | return c_SetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode) 181 | 182 | def simxSetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode): 183 | ''' 184 | Please have a look at the function description/documentation in the CoppeliaSim user manual 185 | ''' 186 | 187 | return c_SetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode) 188 | 189 | def simxJointGetForce(clientID, jointHandle, operationMode): 190 | ''' 191 | Please have a look at the function description/documentation in the CoppeliaSim user manual 192 | ''' 193 | force = ct.c_float() 194 | return c_GetJointForce(clientID, jointHandle, ct.byref(force), operationMode), force.value 195 | 196 | def simxGetJointForce(clientID, jointHandle, operationMode): 197 | ''' 198 | Please have a look at the function description/documentation in the CoppeliaSim user manual 199 | ''' 200 | force = ct.c_float() 201 | return c_GetJointForce(clientID, jointHandle, ct.byref(force), operationMode), force.value 202 | 203 | def simxGetJointMaxForce(clientID, jointHandle, operationMode): 204 | ''' 205 | Please have a look at the function description/documentation in the CoppeliaSim user manual 206 | ''' 207 | force = ct.c_float() 208 | return c_GetJointMaxForce(clientID, jointHandle, ct.byref(force), operationMode), force.value 209 | 210 | def simxSetJointForce(clientID, jointHandle, force, operationMode): 211 | ''' 212 | Please have a look at the function description/documentation in the CoppeliaSim user manual 213 | ''' 214 | return c_SetJointMaxForce(clientID, jointHandle, force, operationMode) 215 | 216 | def simxSetJointMaxForce(clientID, jointHandle, force, operationMode): 217 | ''' 218 | Please have a look at the function description/documentation in the CoppeliaSim user manual 219 | ''' 220 | return c_SetJointMaxForce(clientID, jointHandle, force, operationMode) 221 | 222 | def simxReadForceSensor(clientID, forceSensorHandle, operationMode): 223 | ''' 224 | Please have a look at the function description/documentation in the CoppeliaSim user manual 225 | ''' 226 | state = ct.c_ubyte() 227 | forceVector = (ct.c_float*3)() 228 | torqueVector = (ct.c_float*3)() 229 | ret = c_ReadForceSensor(clientID, forceSensorHandle, ct.byref(state), forceVector, torqueVector, operationMode) 230 | arr1 = [] 231 | for i in range(3): 232 | arr1.append(forceVector[i]) 233 | arr2 = [] 234 | for i in range(3): 235 | arr2.append(torqueVector[i]) 236 | #if sys.version_info[0] == 3: 237 | # state=state.value 238 | #else: 239 | # state=ord(state.value) 240 | return ret, state.value, arr1, arr2 241 | 242 | def simxBreakForceSensor(clientID, forceSensorHandle, operationMode): 243 | ''' 244 | Please have a look at the function description/documentation in the CoppeliaSim user manual 245 | ''' 246 | return c_BreakForceSensor(clientID, forceSensorHandle, operationMode) 247 | 248 | def simxReadVisionSensor(clientID, sensorHandle, operationMode): 249 | ''' 250 | Please have a look at the function description/documentation in the CoppeliaSim user manual 251 | ''' 252 | 253 | detectionState = ct.c_ubyte() 254 | auxValues = ct.POINTER(ct.c_float)() 255 | auxValuesCount = ct.POINTER(ct.c_int)() 256 | ret = c_ReadVisionSensor(clientID, sensorHandle, ct.byref(detectionState), ct.byref(auxValues), ct.byref(auxValuesCount), operationMode) 257 | 258 | auxValues2 = [] 259 | if ret == 0: 260 | s = 0 261 | for i in range(auxValuesCount[0]): 262 | auxValues2.append(auxValues[s:s+auxValuesCount[i+1]]) 263 | s += auxValuesCount[i+1] 264 | 265 | #free C buffers 266 | c_ReleaseBuffer(auxValues) 267 | c_ReleaseBuffer(auxValuesCount) 268 | 269 | return ret, bool(detectionState.value!=0), auxValues2 270 | 271 | def simxGetObjectHandle(clientID, objectName, operationMode): 272 | ''' 273 | Please have a look at the function description/documentation in the CoppeliaSim user manual 274 | ''' 275 | handle = ct.c_int() 276 | if (sys.version_info[0] == 3) and (type(objectName) is str): 277 | objectName=objectName.encode('utf-8') 278 | return c_GetObjectHandle(clientID, objectName, ct.byref(handle), operationMode), handle.value 279 | 280 | def simxGetVisionSensorImage(clientID, sensorHandle, options, operationMode): 281 | ''' 282 | Please have a look at the function description/documentation in the CoppeliaSim user manual 283 | ''' 284 | 285 | resolution = (ct.c_int*2)() 286 | c_image = ct.POINTER(ct.c_byte)() 287 | bytesPerPixel = 3 288 | if (options and 1) != 0: 289 | bytesPerPixel = 1 290 | ret = c_GetVisionSensorImage(clientID, sensorHandle, resolution, ct.byref(c_image), options, operationMode) 291 | 292 | reso = [] 293 | image = [] 294 | if (ret == 0): 295 | image = [None]*resolution[0]*resolution[1]*bytesPerPixel 296 | for i in range(resolution[0] * resolution[1] * bytesPerPixel): 297 | image[i] = c_image[i] 298 | for i in range(2): 299 | reso.append(resolution[i]) 300 | return ret, reso, image 301 | 302 | def simxSetVisionSensorImage(clientID, sensorHandle, image, options, operationMode): 303 | ''' 304 | Please have a look at the function description/documentation in the CoppeliaSim user manual 305 | ''' 306 | size = len(image) 307 | image_bytes = (ct.c_byte*size)(*image) 308 | return c_SetVisionSensorImage(clientID, sensorHandle, image_bytes, size, options, operationMode) 309 | 310 | def simxGetVisionSensorDepthBuffer(clientID, sensorHandle, operationMode): 311 | ''' 312 | Please have a look at the function description/documentation in the CoppeliaSim user manual 313 | ''' 314 | c_buffer = ct.POINTER(ct.c_float)() 315 | resolution = (ct.c_int*2)() 316 | ret = c_GetVisionSensorDepthBuffer(clientID, sensorHandle, resolution, ct.byref(c_buffer), operationMode) 317 | reso = [] 318 | buffer = [] 319 | if (ret == 0): 320 | buffer = [None]*resolution[0]*resolution[1] 321 | for i in range(resolution[0] * resolution[1]): 322 | buffer[i] = c_buffer[i] 323 | for i in range(2): 324 | reso.append(resolution[i]) 325 | return ret, reso, buffer 326 | 327 | def simxGetObjectChild(clientID, parentObjectHandle, childIndex, operationMode): 328 | ''' 329 | Please have a look at the function description/documentation in the CoppeliaSim user manual 330 | ''' 331 | childObjectHandle = ct.c_int() 332 | return c_GetObjectChild(clientID, parentObjectHandle, childIndex, ct.byref(childObjectHandle), operationMode), childObjectHandle.value 333 | 334 | def simxGetObjectParent(clientID, childObjectHandle, operationMode): 335 | ''' 336 | Please have a look at the function description/documentation in the CoppeliaSim user manual 337 | ''' 338 | 339 | parentObjectHandle = ct.c_int() 340 | return c_GetObjectParent(clientID, childObjectHandle, ct.byref(parentObjectHandle), operationMode), parentObjectHandle.value 341 | 342 | def simxReadProximitySensor(clientID, sensorHandle, operationMode): 343 | ''' 344 | Please have a look at the function description/documentation in the CoppeliaSim user manual 345 | ''' 346 | 347 | detectionState = ct.c_ubyte() 348 | detectedObjectHandle = ct.c_int() 349 | detectedPoint = (ct.c_float*3)() 350 | detectedSurfaceNormalVector = (ct.c_float*3)() 351 | ret = c_ReadProximitySensor(clientID, sensorHandle, ct.byref(detectionState), detectedPoint, ct.byref(detectedObjectHandle), detectedSurfaceNormalVector, operationMode) 352 | arr1 = [] 353 | for i in range(3): 354 | arr1.append(detectedPoint[i]) 355 | arr2 = [] 356 | for i in range(3): 357 | arr2.append(detectedSurfaceNormalVector[i]) 358 | return ret, bool(detectionState.value!=0), arr1, detectedObjectHandle.value, arr2 359 | 360 | def simxLoadModel(clientID, modelPathAndName, options, operationMode): 361 | ''' 362 | Please have a look at the function description/documentation in the CoppeliaSim user manual 363 | ''' 364 | baseHandle = ct.c_int() 365 | if (sys.version_info[0] == 3) and (type(modelPathAndName) is str): 366 | modelPathAndName=modelPathAndName.encode('utf-8') 367 | return c_LoadModel(clientID, modelPathAndName, options, ct.byref(baseHandle), operationMode), baseHandle.value 368 | 369 | def simxLoadUI(clientID, uiPathAndName, options, operationMode): 370 | ''' 371 | Please have a look at the function description/documentation in the CoppeliaSim user manual 372 | ''' 373 | 374 | count = ct.c_int() 375 | uiHandles = ct.POINTER(ct.c_int)() 376 | if (sys.version_info[0] == 3) and (type(uiPathAndName) is str): 377 | uiPathAndName=uiPathAndName.encode('utf-8') 378 | ret = c_LoadUI(clientID, uiPathAndName, options, ct.byref(count), ct.byref(uiHandles), operationMode) 379 | 380 | handles = [] 381 | if ret == 0: 382 | for i in range(count.value): 383 | handles.append(uiHandles[i]) 384 | #free C buffers 385 | c_ReleaseBuffer(uiHandles) 386 | 387 | return ret, handles 388 | 389 | def simxLoadScene(clientID, scenePathAndName, options, operationMode): 390 | ''' 391 | Please have a look at the function description/documentation in the CoppeliaSim user manual 392 | ''' 393 | 394 | if (sys.version_info[0] == 3) and (type(scenePathAndName) is str): 395 | scenePathAndName=scenePathAndName.encode('utf-8') 396 | return c_LoadScene(clientID, scenePathAndName, options, operationMode) 397 | 398 | def simxStartSimulation(clientID, operationMode): 399 | ''' 400 | Please have a look at the function description/documentation in the CoppeliaSim user manual 401 | ''' 402 | 403 | return c_StartSimulation(clientID, operationMode) 404 | 405 | def simxPauseSimulation(clientID, operationMode): 406 | ''' 407 | Please have a look at the function description/documentation in the CoppeliaSim user manual 408 | ''' 409 | 410 | return c_PauseSimulation(clientID, operationMode) 411 | 412 | def simxStopSimulation(clientID, operationMode): 413 | ''' 414 | Please have a look at the function description/documentation in the CoppeliaSim user manual 415 | ''' 416 | 417 | return c_StopSimulation(clientID, operationMode) 418 | 419 | def simxGetUIHandle(clientID, uiName, operationMode): 420 | ''' 421 | Please have a look at the function description/documentation in the CoppeliaSim user manual 422 | ''' 423 | 424 | handle = ct.c_int() 425 | if (sys.version_info[0] == 3) and (type(uiName) is str): 426 | uiName=uiName.encode('utf-8') 427 | return c_GetUIHandle(clientID, uiName, ct.byref(handle), operationMode), handle.value 428 | 429 | def simxGetUISlider(clientID, uiHandle, uiButtonID, operationMode): 430 | ''' 431 | Please have a look at the function description/documentation in the CoppeliaSim user manual 432 | ''' 433 | 434 | position = ct.c_int() 435 | return c_GetUISlider(clientID, uiHandle, uiButtonID, ct.byref(position), operationMode), position.value 436 | 437 | def simxSetUISlider(clientID, uiHandle, uiButtonID, position, operationMode): 438 | ''' 439 | Please have a look at the function description/documentation in the CoppeliaSim user manual 440 | ''' 441 | 442 | return c_SetUISlider(clientID, uiHandle, uiButtonID, position, operationMode) 443 | 444 | def simxGetUIEventButton(clientID, uiHandle, operationMode): 445 | ''' 446 | Please have a look at the function description/documentation in the CoppeliaSim user manual 447 | ''' 448 | 449 | uiEventButtonID = ct.c_int() 450 | auxValues = (ct.c_int*2)() 451 | ret = c_GetUIEventButton(clientID, uiHandle, ct.byref(uiEventButtonID), auxValues, operationMode) 452 | arr = [] 453 | for i in range(2): 454 | arr.append(auxValues[i]) 455 | return ret, uiEventButtonID.value, arr 456 | 457 | def simxGetUIButtonProperty(clientID, uiHandle, uiButtonID, operationMode): 458 | ''' 459 | Please have a look at the function description/documentation in the CoppeliaSim user manual 460 | ''' 461 | 462 | prop = ct.c_int() 463 | return c_GetUIButtonProperty(clientID, uiHandle, uiButtonID, ct.byref(prop), operationMode), prop.value 464 | 465 | def simxSetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode): 466 | ''' 467 | Please have a look at the function description/documentation in the CoppeliaSim user manual 468 | ''' 469 | 470 | return c_SetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode) 471 | 472 | def simxAddStatusbarMessage(clientID, message, operationMode): 473 | ''' 474 | Please have a look at the function description/documentation in the CoppeliaSim user manual 475 | ''' 476 | 477 | if (sys.version_info[0] == 3) and (type(message) is str): 478 | message=message.encode('utf-8') 479 | return c_AddStatusbarMessage(clientID, message, operationMode) 480 | 481 | def simxAuxiliaryConsoleOpen(clientID, title, maxLines, mode, position, size, textColor, backgroundColor, operationMode): 482 | ''' 483 | Please have a look at the function description/documentation in the CoppeliaSim user manual 484 | ''' 485 | 486 | consoleHandle = ct.c_int() 487 | if (sys.version_info[0] == 3) and (type(title) is str): 488 | title=title.encode('utf-8') 489 | if position != None: 490 | c_position = (ct.c_int*2)(*position) 491 | else: 492 | c_position = None 493 | if size != None: 494 | c_size = (ct.c_int*2)(*size) 495 | else: 496 | c_size = None 497 | if textColor != None: 498 | c_textColor = (ct.c_float*3)(*textColor) 499 | else: 500 | c_textColor = None 501 | if backgroundColor != None: 502 | c_backgroundColor = (ct.c_float*3)(*backgroundColor) 503 | else: 504 | c_backgroundColor = None 505 | return c_AuxiliaryConsoleOpen(clientID, title, maxLines, mode, c_position, c_size, c_textColor, c_backgroundColor, ct.byref(consoleHandle), operationMode), consoleHandle.value 506 | 507 | def simxAuxiliaryConsoleClose(clientID, consoleHandle, operationMode): 508 | ''' 509 | Please have a look at the function description/documentation in the CoppeliaSim user manual 510 | ''' 511 | 512 | return c_AuxiliaryConsoleClose(clientID, consoleHandle, operationMode) 513 | 514 | def simxAuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode): 515 | ''' 516 | Please have a look at the function description/documentation in the CoppeliaSim user manual 517 | ''' 518 | 519 | if (sys.version_info[0] == 3) and (type(txt) is str): 520 | txt=txt.encode('utf-8') 521 | return c_AuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode) 522 | 523 | def simxAuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode): 524 | ''' 525 | Please have a look at the function description/documentation in the CoppeliaSim user manual 526 | ''' 527 | 528 | return c_AuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode) 529 | 530 | def simxGetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, operationMode): 531 | ''' 532 | Please have a look at the function description/documentation in the CoppeliaSim user manual 533 | ''' 534 | eulerAngles = (ct.c_float*3)() 535 | ret = c_GetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode) 536 | arr = [] 537 | for i in range(3): 538 | arr.append(eulerAngles[i]) 539 | return ret, arr 540 | 541 | def simxGetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, operationMode): 542 | ''' 543 | Please have a look at the function description/documentation in the CoppeliaSim user manual 544 | ''' 545 | quaternion = (ct.c_float*4)() 546 | ret = c_GetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, quaternion, operationMode) 547 | arr = [] 548 | for i in range(4): 549 | arr.append(quaternion[i]) 550 | return ret, arr 551 | 552 | def simxGetObjectPosition(clientID, objectHandle, relativeToObjectHandle, operationMode): 553 | ''' 554 | Please have a look at the function description/documentation in the CoppeliaSim user manual 555 | ''' 556 | position = (ct.c_float*3)() 557 | ret = c_GetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode) 558 | arr = [] 559 | for i in range(3): 560 | arr.append(position[i]) 561 | return ret, arr 562 | 563 | def simxSetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode): 564 | ''' 565 | Please have a look at the function description/documentation in the CoppeliaSim user manual 566 | ''' 567 | 568 | angles = (ct.c_float*3)(*eulerAngles) 569 | return c_SetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, angles, operationMode) 570 | 571 | def simxSetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, quaternion, operationMode): 572 | ''' 573 | Please have a look at the function description/documentation in the CoppeliaSim user manual 574 | ''' 575 | 576 | quat = (ct.c_float*4)(*quaternion) 577 | return c_SetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, quat, operationMode) 578 | 579 | def simxSetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode): 580 | ''' 581 | Please have a look at the function description/documentation in the CoppeliaSim user manual 582 | ''' 583 | 584 | c_position = (ct.c_float*3)(*position) 585 | return c_SetObjectPosition(clientID, objectHandle, relativeToObjectHandle, c_position, operationMode) 586 | 587 | def simxSetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode): 588 | ''' 589 | Please have a look at the function description/documentation in the CoppeliaSim user manual 590 | ''' 591 | 592 | return c_SetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode) 593 | 594 | def simxSetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode): 595 | ''' 596 | Please have a look at the function description/documentation in the CoppeliaSim user manual 597 | ''' 598 | 599 | if sys.version_info[0] == 3: 600 | if type(upStateLabel) is str: 601 | upStateLabel=upStateLabel.encode('utf-8') 602 | if type(downStateLabel) is str: 603 | downStateLabel=downStateLabel.encode('utf-8') 604 | return c_SetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode) 605 | 606 | def simxGetLastErrors(clientID, operationMode): 607 | ''' 608 | Please have a look at the function description/documentation in the CoppeliaSim user manual 609 | ''' 610 | errors =[] 611 | errorCnt = ct.c_int() 612 | errorStrings = ct.POINTER(ct.c_char)() 613 | ret = c_GetLastErrors(clientID, ct.byref(errorCnt), ct.byref(errorStrings), operationMode) 614 | if ret == 0: 615 | s = 0 616 | for i in range(errorCnt.value): 617 | a = bytearray() 618 | while errorStrings[s] != b'\0': 619 | if sys.version_info[0] == 3: 620 | a.append(int.from_bytes(errorStrings[s],'big')) 621 | else: 622 | a.append(errorStrings[s]) 623 | s += 1 624 | s += 1 #skip null 625 | if sys.version_info[0] == 3: 626 | errors.append(str(a,'utf-8')) 627 | else: 628 | errors.append(str(a)) 629 | 630 | return ret, errors 631 | 632 | def simxGetArrayParameter(clientID, paramIdentifier, operationMode): 633 | ''' 634 | Please have a look at the function description/documentation in the CoppeliaSim user manual 635 | ''' 636 | paramValues = (ct.c_float*3)() 637 | ret = c_GetArrayParameter(clientID, paramIdentifier, paramValues, operationMode) 638 | arr = [] 639 | for i in range(3): 640 | arr.append(paramValues[i]) 641 | return ret, arr 642 | 643 | def simxSetArrayParameter(clientID, paramIdentifier, paramValues, operationMode): 644 | ''' 645 | Please have a look at the function description/documentation in the CoppeliaSim user manual 646 | ''' 647 | 648 | c_paramValues = (ct.c_float*3)(*paramValues) 649 | return c_SetArrayParameter(clientID, paramIdentifier, c_paramValues, operationMode) 650 | 651 | def simxGetBooleanParameter(clientID, paramIdentifier, operationMode): 652 | ''' 653 | Please have a look at the function description/documentation in the CoppeliaSim user manual 654 | ''' 655 | 656 | paramValue = ct.c_ubyte() 657 | return c_GetBooleanParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), bool(paramValue.value!=0) 658 | 659 | def simxSetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode): 660 | ''' 661 | Please have a look at the function description/documentation in the CoppeliaSim user manual 662 | ''' 663 | 664 | return c_SetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode) 665 | 666 | def simxGetIntegerParameter(clientID, paramIdentifier, operationMode): 667 | ''' 668 | Please have a look at the function description/documentation in the CoppeliaSim user manual 669 | ''' 670 | 671 | paramValue = ct.c_int() 672 | return c_GetIntegerParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), paramValue.value 673 | 674 | def simxSetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode): 675 | ''' 676 | Please have a look at the function description/documentation in the CoppeliaSim user manual 677 | ''' 678 | 679 | return c_SetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode) 680 | 681 | def simxGetFloatingParameter(clientID, paramIdentifier, operationMode): 682 | ''' 683 | Please have a look at the function description/documentation in the CoppeliaSim user manual 684 | ''' 685 | 686 | paramValue = ct.c_float() 687 | return c_GetFloatingParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), paramValue.value 688 | 689 | def simxSetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode): 690 | ''' 691 | Please have a look at the function description/documentation in the CoppeliaSim user manual 692 | ''' 693 | 694 | return c_SetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode) 695 | 696 | def simxGetStringParameter(clientID, paramIdentifier, operationMode): 697 | ''' 698 | Please have a look at the function description/documentation in the CoppeliaSim user manual 699 | ''' 700 | paramValue = ct.POINTER(ct.c_char)() 701 | ret = c_GetStringParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode) 702 | 703 | a = bytearray() 704 | if ret == 0: 705 | i = 0 706 | while paramValue[i] != b'\0': 707 | if sys.version_info[0] == 3: 708 | a.append(int.from_bytes(paramValue[i],'big')) 709 | else: 710 | a.append(paramValue[i]) 711 | i=i+1 712 | if sys.version_info[0] == 3: 713 | a=str(a,'utf-8') 714 | else: 715 | a=str(a) 716 | return ret, a 717 | 718 | def simxGetCollisionHandle(clientID, collisionObjectName, operationMode): 719 | ''' 720 | Please have a look at the function description/documentation in the CoppeliaSim user manual 721 | ''' 722 | 723 | handle = ct.c_int() 724 | if (sys.version_info[0] == 3) and (type(collisionObjectName) is str): 725 | collisionObjectName=collisionObjectName.encode('utf-8') 726 | return c_GetCollisionHandle(clientID, collisionObjectName, ct.byref(handle), operationMode), handle.value 727 | 728 | def simxGetCollectionHandle(clientID, collectionName, operationMode): 729 | ''' 730 | Please have a look at the function description/documentation in the CoppeliaSim user manual 731 | ''' 732 | 733 | handle = ct.c_int() 734 | if (sys.version_info[0] == 3) and (type(collectionName) is str): 735 | collectionName=collectionName.encode('utf-8') 736 | return c_GetCollectionHandle(clientID, collectionName, ct.byref(handle), operationMode), handle.value 737 | 738 | def simxGetDistanceHandle(clientID, distanceObjectName, operationMode): 739 | ''' 740 | Please have a look at the function description/documentation in the CoppeliaSim user manual 741 | ''' 742 | 743 | handle = ct.c_int() 744 | if (sys.version_info[0] == 3) and (type(distanceObjectName) is str): 745 | distanceObjectName=distanceObjectName.encode('utf-8') 746 | return c_GetDistanceHandle(clientID, distanceObjectName, ct.byref(handle), operationMode), handle.value 747 | 748 | def simxReadCollision(clientID, collisionObjectHandle, operationMode): 749 | ''' 750 | Please have a look at the function description/documentation in the CoppeliaSim user manual 751 | ''' 752 | collisionState = ct.c_ubyte() 753 | return c_ReadCollision(clientID, collisionObjectHandle, ct.byref(collisionState), operationMode), bool(collisionState.value!=0) 754 | 755 | def simxReadDistance(clientID, distanceObjectHandle, operationMode): 756 | ''' 757 | Please have a look at the function description/documentation in the CoppeliaSim user manual 758 | ''' 759 | 760 | minimumDistance = ct.c_float() 761 | return c_ReadDistance(clientID, distanceObjectHandle, ct.byref(minimumDistance), operationMode), minimumDistance.value 762 | 763 | def simxRemoveObject(clientID, objectHandle, operationMode): 764 | ''' 765 | Please have a look at the function description/documentation in the CoppeliaSim user manual 766 | ''' 767 | 768 | return c_RemoveObject(clientID, objectHandle, operationMode) 769 | 770 | def simxRemoveModel(clientID, objectHandle, operationMode): 771 | ''' 772 | Please have a look at the function description/documentation in the CoppeliaSim user manual 773 | ''' 774 | 775 | return c_RemoveModel(clientID, objectHandle, operationMode) 776 | 777 | def simxRemoveUI(clientID, uiHandle, operationMode): 778 | ''' 779 | Please have a look at the function description/documentation in the CoppeliaSim user manual 780 | ''' 781 | 782 | return c_RemoveUI(clientID, uiHandle, operationMode) 783 | 784 | def simxCloseScene(clientID, operationMode): 785 | ''' 786 | Please have a look at the function description/documentation in the CoppeliaSim user manual 787 | ''' 788 | 789 | return c_CloseScene(clientID, operationMode) 790 | 791 | def simxGetObjects(clientID, objectType, operationMode): 792 | ''' 793 | Please have a look at the function description/documentation in the CoppeliaSim user manual 794 | ''' 795 | 796 | objectCount = ct.c_int() 797 | objectHandles = ct.POINTER(ct.c_int)() 798 | 799 | ret = c_GetObjects(clientID, objectType, ct.byref(objectCount), ct.byref(objectHandles), operationMode) 800 | handles = [] 801 | if ret == 0: 802 | for i in range(objectCount.value): 803 | handles.append(objectHandles[i]) 804 | 805 | return ret, handles 806 | 807 | 808 | def simxDisplayDialog(clientID, titleText, mainText, dialogType, initialText, titleColors, dialogColors, operationMode): 809 | ''' 810 | Please have a look at the function description/documentation in the CoppeliaSim user manual 811 | ''' 812 | if titleColors != None: 813 | c_titleColors = (ct.c_float*6)(*titleColors) 814 | else: 815 | c_titleColors = None 816 | if dialogColors != None: 817 | c_dialogColors = (ct.c_float*6)(*dialogColors) 818 | else: 819 | c_dialogColors = None 820 | 821 | c_dialogHandle = ct.c_int() 822 | c_uiHandle = ct.c_int() 823 | if sys.version_info[0] == 3: 824 | if type(titleText) is str: 825 | titleText=titleText.encode('utf-8') 826 | if type(mainText) is str: 827 | mainText=mainText.encode('utf-8') 828 | if type(initialText) is str: 829 | initialText=initialText.encode('utf-8') 830 | return c_DisplayDialog(clientID, titleText, mainText, dialogType, initialText, c_titleColors, c_dialogColors, ct.byref(c_dialogHandle), ct.byref(c_uiHandle), operationMode), c_dialogHandle.value, c_uiHandle.value 831 | 832 | def simxEndDialog(clientID, dialogHandle, operationMode): 833 | ''' 834 | Please have a look at the function description/documentation in the CoppeliaSim user manual 835 | ''' 836 | 837 | return c_EndDialog(clientID, dialogHandle, operationMode) 838 | 839 | def simxGetDialogInput(clientID, dialogHandle, operationMode): 840 | ''' 841 | Please have a look at the function description/documentation in the CoppeliaSim user manual 842 | ''' 843 | inputText = ct.POINTER(ct.c_char)() 844 | ret = c_GetDialogInput(clientID, dialogHandle, ct.byref(inputText), operationMode) 845 | 846 | a = bytearray() 847 | if ret == 0: 848 | i = 0 849 | while inputText[i] != b'\0': 850 | if sys.version_info[0] == 3: 851 | a.append(int.from_bytes(inputText[i],'big')) 852 | else: 853 | a.append(inputText[i]) 854 | i = i+1 855 | 856 | if sys.version_info[0] == 3: 857 | a=str(a,'utf-8') 858 | else: 859 | a=str(a) 860 | return ret, a 861 | 862 | 863 | def simxGetDialogResult(clientID, dialogHandle, operationMode): 864 | ''' 865 | Please have a look at the function description/documentation in the CoppeliaSim user manual 866 | ''' 867 | result = ct.c_int() 868 | return c_GetDialogResult(clientID, dialogHandle, ct.byref(result), operationMode), result.value 869 | 870 | def simxCopyPasteObjects(clientID, objectHandles, operationMode): 871 | ''' 872 | Please have a look at the function description/documentation in the CoppeliaSim user manual 873 | ''' 874 | c_objectHandles = (ct.c_int*len(objectHandles))(*objectHandles) 875 | c_objectHandles = ct.cast(c_objectHandles,ct.POINTER(ct.c_int)) # IronPython needs this 876 | newObjectCount = ct.c_int() 877 | newObjectHandles = ct.POINTER(ct.c_int)() 878 | ret = c_CopyPasteObjects(clientID, c_objectHandles, len(objectHandles), ct.byref(newObjectHandles), ct.byref(newObjectCount), operationMode) 879 | 880 | newobj = [] 881 | if ret == 0: 882 | for i in range(newObjectCount.value): 883 | newobj.append(newObjectHandles[i]) 884 | 885 | return ret, newobj 886 | 887 | 888 | def simxGetObjectSelection(clientID, operationMode): 889 | ''' 890 | Please have a look at the function description/documentation in the CoppeliaSim user manual 891 | ''' 892 | objectCount = ct.c_int() 893 | objectHandles = ct.POINTER(ct.c_int)() 894 | ret = c_GetObjectSelection(clientID, ct.byref(objectHandles), ct.byref(objectCount), operationMode) 895 | 896 | newobj = [] 897 | if ret == 0: 898 | for i in range(objectCount.value): 899 | newobj.append(objectHandles[i]) 900 | 901 | return ret, newobj 902 | 903 | 904 | 905 | def simxSetObjectSelection(clientID, objectHandles, operationMode): 906 | ''' 907 | Please have a look at the function description/documentation in the CoppeliaSim user manual 908 | ''' 909 | 910 | c_objectHandles = (ct.c_int*len(objectHandles))(*objectHandles) 911 | return c_SetObjectSelection(clientID, c_objectHandles, len(objectHandles), operationMode) 912 | 913 | def simxClearFloatSignal(clientID, signalName, operationMode): 914 | ''' 915 | Please have a look at the function description/documentation in the CoppeliaSim user manual 916 | ''' 917 | 918 | if (sys.version_info[0] == 3) and (type(signalName) is str): 919 | signalName=signalName.encode('utf-8') 920 | return c_ClearFloatSignal(clientID, signalName, operationMode) 921 | 922 | def simxClearIntegerSignal(clientID, signalName, operationMode): 923 | ''' 924 | Please have a look at the function description/documentation in the CoppeliaSim user manual 925 | ''' 926 | 927 | if (sys.version_info[0] == 3) and (type(signalName) is str): 928 | signalName=signalName.encode('utf-8') 929 | return c_ClearIntegerSignal(clientID, signalName, operationMode) 930 | 931 | def simxClearStringSignal(clientID, signalName, operationMode): 932 | ''' 933 | Please have a look at the function description/documentation in the CoppeliaSim user manual 934 | ''' 935 | 936 | if (sys.version_info[0] == 3) and (type(signalName) is str): 937 | signalName=signalName.encode('utf-8') 938 | return c_ClearStringSignal(clientID, signalName, operationMode) 939 | 940 | def simxGetFloatSignal(clientID, signalName, operationMode): 941 | ''' 942 | Please have a look at the function description/documentation in the CoppeliaSim user manual 943 | ''' 944 | 945 | signalValue = ct.c_float() 946 | if (sys.version_info[0] == 3) and (type(signalName) is str): 947 | signalName=signalName.encode('utf-8') 948 | return c_GetFloatSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value 949 | 950 | def simxGetIntegerSignal(clientID, signalName, operationMode): 951 | ''' 952 | Please have a look at the function description/documentation in the CoppeliaSim user manual 953 | ''' 954 | 955 | signalValue = ct.c_int() 956 | if (sys.version_info[0] == 3) and (type(signalName) is str): 957 | signalName=signalName.encode('utf-8') 958 | return c_GetIntegerSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value 959 | 960 | def simxGetStringSignal(clientID, signalName, operationMode): 961 | ''' 962 | Please have a look at the function description/documentation in the CoppeliaSim user manual 963 | ''' 964 | 965 | signalLength = ct.c_int(); 966 | signalValue = ct.POINTER(ct.c_ubyte)() 967 | if (sys.version_info[0] == 3) and (type(signalName) is str): 968 | signalName=signalName.encode('utf-8') 969 | ret = c_GetStringSignal(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) 970 | 971 | a = bytearray() 972 | if ret == 0: 973 | for i in range(signalLength.value): 974 | a.append(signalValue[i]) 975 | if sys.version_info[0] != 3: 976 | a=str(a) 977 | 978 | return ret, a 979 | 980 | def simxGetAndClearStringSignal(clientID, signalName, operationMode): 981 | ''' 982 | Please have a look at the function description/documentation in the CoppeliaSim user manual 983 | ''' 984 | 985 | signalLength = ct.c_int(); 986 | signalValue = ct.POINTER(ct.c_ubyte)() 987 | if (sys.version_info[0] == 3) and (type(signalName) is str): 988 | signalName=signalName.encode('utf-8') 989 | ret = c_GetAndClearStringSignal(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) 990 | 991 | a = bytearray() 992 | if ret == 0: 993 | for i in range(signalLength.value): 994 | a.append(signalValue[i]) 995 | if sys.version_info[0] != 3: 996 | a=str(a) 997 | 998 | return ret, a 999 | 1000 | def simxReadStringStream(clientID, signalName, operationMode): 1001 | ''' 1002 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1003 | ''' 1004 | 1005 | signalLength = ct.c_int(); 1006 | signalValue = ct.POINTER(ct.c_ubyte)() 1007 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1008 | signalName=signalName.encode('utf-8') 1009 | ret = c_ReadStringStream(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) 1010 | 1011 | a = bytearray() 1012 | if ret == 0: 1013 | for i in range(signalLength.value): 1014 | a.append(signalValue[i]) 1015 | if sys.version_info[0] != 3: 1016 | a=str(a) 1017 | 1018 | return ret, a 1019 | 1020 | def simxSetFloatSignal(clientID, signalName, signalValue, operationMode): 1021 | ''' 1022 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1023 | ''' 1024 | 1025 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1026 | signalName=signalName.encode('utf-8') 1027 | return c_SetFloatSignal(clientID, signalName, signalValue, operationMode) 1028 | 1029 | def simxSetIntegerSignal(clientID, signalName, signalValue, operationMode): 1030 | ''' 1031 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1032 | ''' 1033 | 1034 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1035 | signalName=signalName.encode('utf-8') 1036 | return c_SetIntegerSignal(clientID, signalName, signalValue, operationMode) 1037 | 1038 | def simxSetStringSignal(clientID, signalName, signalValue, operationMode): 1039 | ''' 1040 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1041 | ''' 1042 | 1043 | sigV=signalValue 1044 | if sys.version_info[0] == 3: 1045 | if type(signalName) is str: 1046 | signalName=signalName.encode('utf-8') 1047 | if type(signalValue) is bytearray: 1048 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1049 | if type(signalValue) is str: 1050 | signalValue=signalValue.encode('utf-8') 1051 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1052 | else: 1053 | if type(signalValue) is bytearray: 1054 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1055 | if type(signalValue) is str: 1056 | signalValue=bytearray(signalValue) 1057 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1058 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1059 | return c_SetStringSignal(clientID, signalName, sigV, len(signalValue), operationMode) 1060 | 1061 | def simxAppendStringSignal(clientID, signalName, signalValue, operationMode): 1062 | ''' 1063 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1064 | ''' 1065 | 1066 | sigV=signalValue 1067 | if sys.version_info[0] == 3: 1068 | if type(signalName) is str: 1069 | signalName=signalName.encode('utf-8') 1070 | if type(signalValue) is bytearray: 1071 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1072 | if type(signalValue) is str: 1073 | signalValue=signalValue.encode('utf-8') 1074 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1075 | else: 1076 | if type(signalValue) is bytearray: 1077 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1078 | if type(signalValue) is str: 1079 | signalValue=bytearray(signalValue) 1080 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1081 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1082 | return c_AppendStringSignal(clientID, signalName, sigV, len(signalValue), operationMode) 1083 | 1084 | def simxWriteStringStream(clientID, signalName, signalValue, operationMode): 1085 | ''' 1086 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1087 | ''' 1088 | 1089 | sigV=signalValue 1090 | if sys.version_info[0] == 3: 1091 | if type(signalName) is str: 1092 | signalName=signalName.encode('utf-8') 1093 | if type(signalValue) is bytearray: 1094 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1095 | if type(signalValue) is str: 1096 | signalValue=signalValue.encode('utf-8') 1097 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1098 | else: 1099 | if type(signalValue) is bytearray: 1100 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1101 | if type(signalValue) is str: 1102 | signalValue=bytearray(signalValue) 1103 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1104 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1105 | return c_WriteStringStream(clientID, signalName, sigV, len(signalValue), operationMode) 1106 | 1107 | def simxGetObjectFloatParameter(clientID, objectHandle, parameterID, operationMode): 1108 | ''' 1109 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1110 | ''' 1111 | 1112 | parameterValue = ct.c_float() 1113 | return c_GetObjectFloatParameter(clientID, objectHandle, parameterID, ct.byref(parameterValue), operationMode), parameterValue.value 1114 | 1115 | def simxSetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): 1116 | ''' 1117 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1118 | ''' 1119 | 1120 | return c_SetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) 1121 | 1122 | def simxGetObjectIntParameter(clientID, objectHandle, parameterID, operationMode): 1123 | ''' 1124 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1125 | ''' 1126 | 1127 | parameterValue = ct.c_int() 1128 | return c_GetObjectIntParameter(clientID, objectHandle, parameterID, ct.byref(parameterValue), operationMode), parameterValue.value 1129 | 1130 | def simxSetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): 1131 | ''' 1132 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1133 | ''' 1134 | 1135 | return c_SetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) 1136 | 1137 | def simxGetModelProperty(clientID, objectHandle, operationMode): 1138 | ''' 1139 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1140 | ''' 1141 | prop = ct.c_int() 1142 | return c_GetModelProperty(clientID, objectHandle, ct.byref(prop), operationMode), prop.value 1143 | 1144 | def simxSetModelProperty(clientID, objectHandle, prop, operationMode): 1145 | ''' 1146 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1147 | ''' 1148 | 1149 | return c_SetModelProperty(clientID, objectHandle, prop, operationMode) 1150 | 1151 | def simxStart(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs): 1152 | ''' 1153 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1154 | ''' 1155 | 1156 | if (sys.version_info[0] == 3) and (type(connectionAddress) is str): 1157 | connectionAddress=connectionAddress.encode('utf-8') 1158 | return c_Start(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs) 1159 | 1160 | def simxFinish(clientID): 1161 | ''' 1162 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1163 | ''' 1164 | 1165 | return c_Finish(clientID) 1166 | 1167 | def simxGetPingTime(clientID): 1168 | ''' 1169 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1170 | ''' 1171 | pingTime = ct.c_int() 1172 | return c_GetPingTime(clientID, ct.byref(pingTime)), pingTime.value 1173 | 1174 | def simxGetLastCmdTime(clientID): 1175 | ''' 1176 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1177 | ''' 1178 | 1179 | return c_GetLastCmdTime(clientID) 1180 | 1181 | def simxSynchronousTrigger(clientID): 1182 | ''' 1183 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1184 | ''' 1185 | 1186 | return c_SynchronousTrigger(clientID) 1187 | 1188 | def simxSynchronous(clientID, enable): 1189 | ''' 1190 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1191 | ''' 1192 | 1193 | return c_Synchronous(clientID, enable) 1194 | 1195 | def simxPauseCommunication(clientID, enable): 1196 | ''' 1197 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1198 | ''' 1199 | 1200 | return c_PauseCommunication(clientID, enable) 1201 | 1202 | def simxGetInMessageInfo(clientID, infoType): 1203 | ''' 1204 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1205 | ''' 1206 | info = ct.c_int() 1207 | return c_GetInMessageInfo(clientID, infoType, ct.byref(info)), info.value 1208 | 1209 | def simxGetOutMessageInfo(clientID, infoType): 1210 | ''' 1211 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1212 | ''' 1213 | info = ct.c_int() 1214 | return c_GetOutMessageInfo(clientID, infoType, ct.byref(info)), info.value 1215 | 1216 | def simxGetConnectionId(clientID): 1217 | ''' 1218 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1219 | ''' 1220 | 1221 | return c_GetConnectionId(clientID) 1222 | 1223 | def simxCreateBuffer(bufferSize): 1224 | ''' 1225 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1226 | ''' 1227 | 1228 | return c_CreateBuffer(bufferSize) 1229 | 1230 | def simxReleaseBuffer(buffer): 1231 | ''' 1232 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1233 | ''' 1234 | 1235 | return c_ReleaseBuffer(buffer) 1236 | 1237 | def simxTransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode): 1238 | ''' 1239 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1240 | ''' 1241 | 1242 | if (sys.version_info[0] == 3) and (type(filePathAndName) is str): 1243 | filePathAndName=filePathAndName.encode('utf-8') 1244 | return c_TransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode) 1245 | 1246 | def simxEraseFile(clientID, fileName_serverSide, operationMode): 1247 | ''' 1248 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1249 | ''' 1250 | 1251 | if (sys.version_info[0] == 3) and (type(fileName_serverSide) is str): 1252 | fileName_serverSide=fileName_serverSide.encode('utf-8') 1253 | return c_EraseFile(clientID, fileName_serverSide, operationMode) 1254 | 1255 | def simxCreateDummy(clientID, size, color, operationMode): 1256 | ''' 1257 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1258 | ''' 1259 | 1260 | handle = ct.c_int() 1261 | if color != None: 1262 | c_color = (ct.c_ubyte*12)(*color) 1263 | else: 1264 | c_color = None 1265 | return c_CreateDummy(clientID, size, c_color, ct.byref(handle), operationMode), handle.value 1266 | 1267 | def simxQuery(clientID, signalName, signalValue, retSignalName, timeOutInMs): 1268 | ''' 1269 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1270 | ''' 1271 | 1272 | retSignalLength = ct.c_int(); 1273 | retSignalValue = ct.POINTER(ct.c_ubyte)() 1274 | 1275 | sigV=signalValue 1276 | if sys.version_info[0] == 3: 1277 | if type(signalName) is str: 1278 | signalName=signalName.encode('utf-8') 1279 | if type(retSignalName) is str: 1280 | retSignalName=retSignalName.encode('utf-8') 1281 | if type(signalValue) is bytearray: 1282 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1283 | if type(signalValue) is str: 1284 | signalValue=signalValue.encode('utf-8') 1285 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1286 | else: 1287 | if type(signalValue) is bytearray: 1288 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1289 | if type(signalValue) is str: 1290 | signalValue=bytearray(signalValue) 1291 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1292 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1293 | 1294 | ret = c_Query(clientID, signalName, sigV, len(signalValue), retSignalName, ct.byref(retSignalValue), ct.byref(retSignalLength), timeOutInMs) 1295 | 1296 | a = bytearray() 1297 | if ret == 0: 1298 | for i in range(retSignalLength.value): 1299 | a.append(retSignalValue[i]) 1300 | if sys.version_info[0] != 3: 1301 | a=str(a) 1302 | 1303 | return ret, a 1304 | 1305 | def simxGetObjectGroupData(clientID, objectType, dataType, operationMode): 1306 | ''' 1307 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1308 | ''' 1309 | 1310 | handles =[] 1311 | intData =[] 1312 | floatData =[] 1313 | stringData =[] 1314 | handlesC = ct.c_int() 1315 | handlesP = ct.POINTER(ct.c_int)() 1316 | intDataC = ct.c_int() 1317 | intDataP = ct.POINTER(ct.c_int)() 1318 | floatDataC = ct.c_int() 1319 | floatDataP = ct.POINTER(ct.c_float)() 1320 | stringDataC = ct.c_int() 1321 | stringDataP = ct.POINTER(ct.c_char)() 1322 | ret = c_GetObjectGroupData(clientID, objectType, dataType, ct.byref(handlesC), ct.byref(handlesP), ct.byref(intDataC), ct.byref(intDataP), ct.byref(floatDataC), ct.byref(floatDataP), ct.byref(stringDataC), ct.byref(stringDataP), operationMode) 1323 | 1324 | if ret == 0: 1325 | for i in range(handlesC.value): 1326 | handles.append(handlesP[i]) 1327 | for i in range(intDataC.value): 1328 | intData.append(intDataP[i]) 1329 | for i in range(floatDataC.value): 1330 | floatData.append(floatDataP[i]) 1331 | s = 0 1332 | for i in range(stringDataC.value): 1333 | a = bytearray() 1334 | while stringDataP[s] != b'\0': 1335 | if sys.version_info[0] == 3: 1336 | a.append(int.from_bytes(stringDataP[s],'big')) 1337 | else: 1338 | a.append(stringDataP[s]) 1339 | s += 1 1340 | s += 1 #skip null 1341 | if sys.version_info[0] == 3: 1342 | a=str(a,'utf-8') 1343 | else: 1344 | a=str(a) 1345 | stringData.append(a) 1346 | 1347 | return ret, handles, intData, floatData, stringData 1348 | 1349 | def simxCallScriptFunction(clientID, scriptDescription, options, functionName, inputInts, inputFloats, inputStrings, inputBuffer, operationMode): 1350 | ''' 1351 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1352 | ''' 1353 | 1354 | inputBufferV=inputBuffer 1355 | if sys.version_info[0] == 3: 1356 | if type(scriptDescription) is str: 1357 | scriptDescription=scriptDescription.encode('utf-8') 1358 | if type(functionName) is str: 1359 | functionName=functionName.encode('utf-8') 1360 | if type(inputBuffer) is bytearray: 1361 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 1362 | if type(inputBuffer) is str: 1363 | inputBuffer=inputBuffer.encode('utf-8') 1364 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 1365 | else: 1366 | if type(inputBuffer) is bytearray: 1367 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 1368 | if type(inputBuffer) is str: 1369 | inputBuffer=bytearray(inputBuffer) 1370 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 1371 | inputBufferV=ct.cast(inputBufferV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1372 | 1373 | c_inInts = (ct.c_int*len(inputInts))(*inputInts) 1374 | c_inInts = ct.cast(c_inInts,ct.POINTER(ct.c_int)) # IronPython needs this 1375 | c_inFloats = (ct.c_float*len(inputFloats))(*inputFloats) 1376 | c_inFloats = ct.cast(c_inFloats,ct.POINTER(ct.c_float)) # IronPython needs this 1377 | 1378 | concatStr=''.encode('utf-8') 1379 | for i in range(len(inputStrings)): 1380 | a=inputStrings[i] 1381 | a=a+'\0' 1382 | if type(a) is str: 1383 | a=a.encode('utf-8') 1384 | concatStr=concatStr+a 1385 | c_inStrings = (ct.c_char*len(concatStr))(*concatStr) 1386 | 1387 | intDataOut =[] 1388 | floatDataOut =[] 1389 | stringDataOut =[] 1390 | bufferOut =bytearray() 1391 | 1392 | intDataC = ct.c_int() 1393 | intDataP = ct.POINTER(ct.c_int)() 1394 | floatDataC = ct.c_int() 1395 | floatDataP = ct.POINTER(ct.c_float)() 1396 | stringDataC = ct.c_int() 1397 | stringDataP = ct.POINTER(ct.c_char)() 1398 | bufferS = ct.c_int() 1399 | bufferP = ct.POINTER(ct.c_ubyte)() 1400 | 1401 | ret = c_CallScriptFunction(clientID,scriptDescription,options,functionName,len(inputInts),c_inInts,len(inputFloats),c_inFloats,len(inputStrings),c_inStrings,len(inputBuffer),inputBufferV,ct.byref(intDataC),ct.byref(intDataP),ct.byref(floatDataC),ct.byref(floatDataP),ct.byref(stringDataC),ct.byref(stringDataP),ct.byref(bufferS),ct.byref(bufferP),operationMode) 1402 | 1403 | if ret == 0: 1404 | for i in range(intDataC.value): 1405 | intDataOut.append(intDataP[i]) 1406 | for i in range(floatDataC.value): 1407 | floatDataOut.append(floatDataP[i]) 1408 | s = 0 1409 | for i in range(stringDataC.value): 1410 | a = bytearray() 1411 | while stringDataP[s] != b'\0': 1412 | if sys.version_info[0] == 3: 1413 | a.append(int.from_bytes(stringDataP[s],'big')) 1414 | else: 1415 | a.append(stringDataP[s]) 1416 | s += 1 1417 | s += 1 #skip null 1418 | if sys.version_info[0] == 3: 1419 | a=str(a,'utf-8') 1420 | else: 1421 | a=str(a) 1422 | stringDataOut.append(a) 1423 | for i in range(bufferS.value): 1424 | bufferOut.append(bufferP[i]) 1425 | if sys.version_info[0] != 3: 1426 | bufferOut=str(bufferOut) 1427 | 1428 | return ret, intDataOut, floatDataOut, stringDataOut, bufferOut 1429 | 1430 | def simxGetObjectVelocity(clientID, objectHandle, operationMode): 1431 | ''' 1432 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1433 | ''' 1434 | linearVel = (ct.c_float*3)() 1435 | angularVel = (ct.c_float*3)() 1436 | ret = c_GetObjectVelocity(clientID, objectHandle, linearVel, angularVel, operationMode) 1437 | arr1 = [] 1438 | for i in range(3): 1439 | arr1.append(linearVel[i]) 1440 | arr2 = [] 1441 | for i in range(3): 1442 | arr2.append(angularVel[i]) 1443 | return ret, arr1, arr2 1444 | 1445 | def simxPackInts(intList): 1446 | ''' 1447 | Please have a look at the function description/documentation in the CoppeliaSim user manual 1448 | ''' 1449 | 1450 | if sys.version_info[0] == 3: 1451 | s=bytes() 1452 | for i in range(len(intList)): 1453 | s=s+struct.pack('=1x) 234 | # reserved =sim_simulation_advancing|0x02 235 | sim_simulation_advancing_lastbeforepause =sim_simulation_advancing|0x03 # Last simulation pass before pause (1x) 236 | sim_simulation_advancing_firstafterpause =sim_simulation_advancing|0x04 # First simulation pass after pause (1x) 237 | sim_simulation_advancing_abouttostop =sim_simulation_advancing|0x05 # "Trying to stop" simulation pass (>=1x) 238 | sim_simulation_advancing_lastbeforestop =sim_simulation_advancing|0x06 # Last simulation pass (1x) 239 | 240 | 241 | # Script execution result (first return value) 242 | sim_script_no_error =0 243 | sim_script_main_script_nonexistent =1 244 | sim_script_main_script_not_called =2 245 | sim_script_reentrance_error =4 246 | sim_script_lua_error =8 247 | sim_script_call_error =16 248 | 249 | 250 | # Script types (serialized!) 251 | sim_scripttype_mainscript =0 252 | sim_scripttype_childscript =1 253 | sim_scripttype_jointctrlcallback =4 254 | sim_scripttype_contactcallback =5 255 | sim_scripttype_customizationscript =6 256 | sim_scripttype_generalcallback =7 257 | 258 | # API call error messages 259 | sim_api_errormessage_ignore =0 # does not memorize nor output errors 260 | sim_api_errormessage_report =1 # memorizes errors (default for C-API calls) 261 | sim_api_errormessage_output =2 # memorizes and outputs errors (default for Lua-API calls) 262 | 263 | 264 | # special argument of some functions 265 | sim_handle_all =-2 266 | sim_handle_all_except_explicit =-3 267 | sim_handle_self =-4 268 | sim_handle_main_script =-5 269 | sim_handle_tree =-6 270 | sim_handle_chain =-7 271 | sim_handle_single =-8 272 | sim_handle_default =-9 273 | sim_handle_all_except_self =-10 274 | sim_handle_parent =-11 275 | 276 | 277 | # special handle flags 278 | sim_handleflag_assembly =0x400000 279 | sim_handleflag_model =0x800000 280 | 281 | 282 | # distance calculation methods (serialized) 283 | sim_distcalcmethod_dl =0 284 | sim_distcalcmethod_dac =1 285 | sim_distcalcmethod_max_dl_dac =2 286 | sim_distcalcmethod_dl_and_dac =3 287 | sim_distcalcmethod_sqrt_dl2_and_dac2=4 288 | sim_distcalcmethod_dl_if_nonzero =5 289 | sim_distcalcmethod_dac_if_nonzero =6 290 | 291 | 292 | # Generic dialog styles 293 | sim_dlgstyle_message =0 294 | sim_dlgstyle_input =1 295 | sim_dlgstyle_ok =2 296 | sim_dlgstyle_ok_cancel =3 297 | sim_dlgstyle_yes_no =4 298 | 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 299 | 300 | # Generic dialog return values 301 | sim_dlgret_still_open =0 302 | sim_dlgret_ok =1 303 | sim_dlgret_cancel =2 304 | sim_dlgret_yes =3 305 | sim_dlgret_no =4 306 | 307 | 308 | # Path properties 309 | sim_pathproperty_show_line =0x0001 310 | sim_pathproperty_show_orientation =0x0002 311 | sim_pathproperty_closed_path =0x0004 312 | sim_pathproperty_automatic_orientation =0x0008 313 | sim_pathproperty_invert_velocity =0x0010 314 | sim_pathproperty_infinite_acceleration =0x0020 315 | sim_pathproperty_flat_path =0x0040 316 | sim_pathproperty_show_position =0x0080 317 | sim_pathproperty_auto_velocity_profile_translation =0x0100 318 | sim_pathproperty_auto_velocity_profile_rotation =0x0200 319 | sim_pathproperty_endpoints_at_zero =0x0400 320 | sim_pathproperty_keep_x_up =0x0800 321 | 322 | 323 | # drawing objects 324 | # following are mutually exclusive 325 | sim_drawing_points =0 # 3 values per point (point size in pixels) 326 | sim_drawing_lines =1 # 6 values per line (line size in pixels) 327 | sim_drawing_triangles =2 # 9 values per triangle 328 | sim_drawing_trianglepoints =3 # 6 values per point (3 for triangle position 3 for triangle normal vector) (triangle size in meters) 329 | sim_drawing_quadpoints =4 # 6 values per point (3 for quad position 3 for quad normal vector) (quad size in meters) 330 | sim_drawing_discpoints =5 # 6 values per point (3 for disc position 3 for disc normal vector) (disc size in meters) 331 | sim_drawing_cubepoints =6 # 6 values per point (3 for cube position 3 for cube normal vector) (cube size in meters) 332 | sim_drawing_spherepoints =7 # 3 values per point (sphere size in meters) 333 | 334 | # following can be or-combined 335 | sim_drawing_itemcolors =0x00020 # +3 values per item (each item has its own ambient color (rgb values)). 336 | # Mutually exclusive with sim_drawing_vertexcolors 337 | 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 338 | sim_drawing_itemsizes =0x00080 # +1 value per item (each item has its own size). Not for sim_drawing_triangles 339 | sim_drawing_backfaceculling =0x00100 # back faces are not displayed for all items 340 | sim_drawing_wireframe =0x00200 # all items displayed in wireframe 341 | sim_drawing_painttag =0x00400 # all items are tagged as paint (for additinal processing at a later stage) 342 | sim_drawing_followparentvisibility =0x00800 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible 343 | sim_drawing_cyclic =0x01000 # if the max item count was reached then the first items are overwritten. 344 | sim_drawing_50percenttransparency =0x02000 # the drawing object will be 50% transparent 345 | sim_drawing_25percenttransparency =0x04000 # the drawing object will be 25% transparent 346 | sim_drawing_12percenttransparency =0x08000 # the drawing object will be 12.5% transparent 347 | 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 348 | 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) 349 | sim_drawing_overlay =0x40000 # When specified objects are always drawn on top of "regular objects" 350 | sim_drawing_itemtransparency =0x80000 # +1 value per item (each item has its own transparency value (0-1)). Not compatible with sim_drawing_vertexcolors 351 | 352 | # banner values 353 | # following can be or-combined 354 | sim_banner_left =0x00001 # Banners display on the left of the specified point 355 | sim_banner_right =0x00002 # Banners display on the right of the specified point 356 | sim_banner_nobackground =0x00004 # Banners have no background rectangle 357 | sim_banner_overlay =0x00008 # When specified banners are always drawn on top of "regular objects" 358 | sim_banner_followparentvisibility =0x00010 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible 359 | sim_banner_clickselectsparent =0x00020 # if the object is associated with a scene object then clicking the banner will select the scene object 360 | sim_banner_clicktriggersevent =0x00040 # if the banner is clicked an event is triggered (sim_message_eventcallback_bannerclicked and sim_message_bannerclicked are generated) 361 | sim_banner_facingcamera =0x00080 # If specified the banner will always face the camera by rotating around the banner's vertical axis (y-axis) 362 | 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) 363 | sim_banner_backfaceculling =0x00200 # If specified the banner will only be visible from one side 364 | 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 365 | 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 366 | # to the specified position. Bitmap fonts are not clickable 367 | 368 | 369 | # particle objects following are mutually exclusive 370 | sim_particle_points1 =0 # 6 values per point (pt1 and pt2. Pt1 is start position pt2-pt1 is the initial velocity vector). i 371 | #Point is 1 pixel big. Only appearance is a point internally handled as a perfect sphere 372 | sim_particle_points2 =1 # 6 values per point. Point is 2 pixel big. Only appearance is a point internally handled as a perfect sphere 373 | sim_particle_points4 =2 # 6 values per point. Point is 4 pixel big. Only appearance is a point internally handled as a perfect sphere 374 | sim_particle_roughspheres =3 # 6 values per sphere. Only appearance is rough. Internally a perfect sphere 375 | sim_particle_spheres =4 # 6 values per sphere. Internally a perfect sphere 376 | 377 | 378 | 379 | 380 | # following can be or-combined 381 | 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) 382 | 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) 383 | sim_particle_particlerespondable =0x0080 # the particles are respondable against each other 384 | sim_particle_ignoresgravity =0x0100 # the particles ignore the effect of gravity. Not compatible with sim_particle_water 385 | sim_particle_invisible =0x0200 # the particles are invisible 386 | sim_particle_itemsizes =0x0400 # +1 value per particle (each particle can have a different size) 387 | sim_particle_itemdensities =0x0800 # +1 value per particle (each particle can have a different density) 388 | sim_particle_itemcolors =0x1000 # +3 values per particle (each particle can have a different color) 389 | sim_particle_cyclic =0x2000 # if the max item count was reached then the first items are overwritten. 390 | sim_particle_emissioncolor =0x4000 # When used in combination with sim_particle_itemcolors then the specified colors will be for the emissive component 391 | 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 392 | sim_particle_painttag =0x10000 # The particles can be seen by vision sensors (sim_particle_invisible must not be set) 393 | 394 | 395 | 396 | 397 | # custom user interface menu attributes 398 | sim_ui_menu_title =1 399 | sim_ui_menu_minimize =2 400 | sim_ui_menu_close =4 401 | sim_ui_menu_systemblock =8 402 | 403 | 404 | 405 | # Boolean parameters 406 | sim_boolparam_hierarchy_visible =0 407 | sim_boolparam_console_visible =1 408 | sim_boolparam_collision_handling_enabled =2 409 | sim_boolparam_distance_handling_enabled =3 410 | sim_boolparam_ik_handling_enabled =4 411 | sim_boolparam_gcs_handling_enabled =5 412 | sim_boolparam_dynamics_handling_enabled =6 413 | sim_boolparam_joint_motion_handling_enabled =7 414 | sim_boolparam_path_motion_handling_enabled =8 415 | sim_boolparam_proximity_sensor_handling_enabled =9 416 | sim_boolparam_vision_sensor_handling_enabled =10 417 | sim_boolparam_mill_handling_enabled =11 418 | sim_boolparam_browser_visible =12 419 | sim_boolparam_scene_and_model_load_messages =13 420 | sim_reserved0 =14 421 | sim_boolparam_shape_textures_are_visible =15 422 | sim_boolparam_display_enabled =16 423 | sim_boolparam_infotext_visible =17 424 | sim_boolparam_statustext_open =18 425 | sim_boolparam_fog_enabled =19 426 | sim_boolparam_rml2_available =20 427 | sim_boolparam_rml4_available =21 428 | sim_boolparam_mirrors_enabled =22 429 | sim_boolparam_aux_clip_planes_enabled =23 430 | sim_boolparam_full_model_copy_from_api =24 431 | sim_boolparam_realtime_simulation =25 432 | sim_boolparam_force_show_wireless_emission =27 433 | sim_boolparam_force_show_wireless_reception =28 434 | sim_boolparam_video_recording_triggered =29 435 | sim_boolparam_threaded_rendering_enabled =32 436 | sim_boolparam_fullscreen =33 437 | sim_boolparam_headless =34 438 | sim_boolparam_hierarchy_toolbarbutton_enabled =35 439 | sim_boolparam_browser_toolbarbutton_enabled =36 440 | sim_boolparam_objectshift_toolbarbutton_enabled =37 441 | sim_boolparam_objectrotate_toolbarbutton_enabled=38 442 | sim_boolparam_force_calcstruct_all_visible =39 443 | sim_boolparam_force_calcstruct_all =40 444 | sim_boolparam_exit_request =41 445 | sim_boolparam_play_toolbarbutton_enabled =42 446 | sim_boolparam_pause_toolbarbutton_enabled =43 447 | sim_boolparam_stop_toolbarbutton_enabled =44 448 | sim_boolparam_waiting_for_trigger =45 449 | 450 | 451 | # Integer parameters 452 | sim_intparam_error_report_mode =0 # Check sim_api_errormessage_... constants above for valid values 453 | sim_intparam_program_version =1 # e.g Version 2.1.4 --> 20104. Can only be read 454 | sim_intparam_instance_count =2 # do not use anymore (always returns 1 since CoppeliaSim 2.5.11) 455 | sim_intparam_custom_cmd_start_id =3 # can only be read 456 | sim_intparam_compilation_version =4 # 0=evaluation version 1=full version 2=player version. Can only be read 457 | sim_intparam_current_page =5 458 | sim_intparam_flymode_camera_handle =6 # can only be read 459 | sim_intparam_dynamic_step_divider =7 # can only be read 460 | sim_intparam_dynamic_engine =8 # 0=Bullet 1=ODE. 2=Vortex. 461 | sim_intparam_server_port_start =9 # can only be read 462 | sim_intparam_server_port_range =10 # can only be read 463 | sim_intparam_visible_layers =11 464 | sim_intparam_infotext_style =12 465 | sim_intparam_settings =13 466 | sim_intparam_edit_mode_type =14 # can only be read 467 | sim_intparam_server_port_next =15 # is initialized at sim_intparam_server_port_start 468 | sim_intparam_qt_version =16 # version of the used Qt framework 469 | sim_intparam_event_flags_read =17 # can only be read 470 | sim_intparam_event_flags_read_clear =18 # can only be read 471 | sim_intparam_platform =19 # can only be read 472 | sim_intparam_scene_unique_id =20 # can only be read 473 | sim_intparam_work_thread_count =21 474 | sim_intparam_mouse_x =22 475 | sim_intparam_mouse_y =23 476 | sim_intparam_core_count =24 477 | sim_intparam_work_thread_calc_time_ms =25 478 | sim_intparam_idle_fps =26 479 | sim_intparam_prox_sensor_select_down =27 480 | sim_intparam_prox_sensor_select_up =28 481 | sim_intparam_stop_request_counter =29 482 | sim_intparam_program_revision =30 483 | sim_intparam_mouse_buttons =31 484 | sim_intparam_dynamic_warning_disabled_mask =32 485 | sim_intparam_simulation_warning_disabled_mask =33 486 | sim_intparam_scene_index =34 487 | sim_intparam_motionplanning_seed =35 488 | sim_intparam_speedmodifier =36 489 | 490 | # Float parameters 491 | sim_floatparam_rand=0 # random value (0.0-1.0) 492 | sim_floatparam_simulation_time_step =1 493 | sim_floatparam_stereo_distance =2 494 | 495 | # String parameters 496 | sim_stringparam_application_path=0 # path of CoppeliaSim's executable 497 | sim_stringparam_video_filename=1 498 | sim_stringparam_app_arg1 =2 499 | sim_stringparam_app_arg2 =3 500 | sim_stringparam_app_arg3 =4 501 | sim_stringparam_app_arg4 =5 502 | sim_stringparam_app_arg5 =6 503 | sim_stringparam_app_arg6 =7 504 | sim_stringparam_app_arg7 =8 505 | sim_stringparam_app_arg8 =9 506 | sim_stringparam_app_arg9 =10 507 | sim_stringparam_scene_path_and_name =13 508 | 509 | # Array parameters 510 | sim_arrayparam_gravity =0 511 | sim_arrayparam_fog =1 512 | sim_arrayparam_fog_color =2 513 | sim_arrayparam_background_color1=3 514 | sim_arrayparam_background_color2=4 515 | sim_arrayparam_ambient_light =5 516 | sim_arrayparam_random_euler =6 517 | 518 | sim_objintparam_visibility_layer= 10 519 | sim_objfloatparam_abs_x_velocity= 11 520 | sim_objfloatparam_abs_y_velocity= 12 521 | sim_objfloatparam_abs_z_velocity= 13 522 | sim_objfloatparam_abs_rot_velocity= 14 523 | sim_objfloatparam_objbbox_min_x= 15 524 | sim_objfloatparam_objbbox_min_y= 16 525 | sim_objfloatparam_objbbox_min_z= 17 526 | sim_objfloatparam_objbbox_max_x= 18 527 | sim_objfloatparam_objbbox_max_y= 19 528 | sim_objfloatparam_objbbox_max_z= 20 529 | sim_objfloatparam_modelbbox_min_x= 21 530 | sim_objfloatparam_modelbbox_min_y= 22 531 | sim_objfloatparam_modelbbox_min_z= 23 532 | sim_objfloatparam_modelbbox_max_x= 24 533 | sim_objfloatparam_modelbbox_max_y= 25 534 | sim_objfloatparam_modelbbox_max_z= 26 535 | sim_objintparam_collection_self_collision_indicator= 27 536 | sim_objfloatparam_transparency_offset= 28 537 | sim_objintparam_child_role= 29 538 | sim_objintparam_parent_role= 30 539 | sim_objintparam_manipulation_permissions= 31 540 | sim_objintparam_illumination_handle= 32 541 | 542 | sim_visionfloatparam_near_clipping= 1000 543 | sim_visionfloatparam_far_clipping= 1001 544 | sim_visionintparam_resolution_x= 1002 545 | sim_visionintparam_resolution_y= 1003 546 | sim_visionfloatparam_perspective_angle= 1004 547 | sim_visionfloatparam_ortho_size= 1005 548 | sim_visionintparam_disabled_light_components= 1006 549 | sim_visionintparam_rendering_attributes= 1007 550 | sim_visionintparam_entity_to_render= 1008 551 | sim_visionintparam_windowed_size_x= 1009 552 | sim_visionintparam_windowed_size_y= 1010 553 | sim_visionintparam_windowed_pos_x= 1011 554 | sim_visionintparam_windowed_pos_y= 1012 555 | sim_visionintparam_pov_focal_blur= 1013 556 | sim_visionfloatparam_pov_blur_distance= 1014 557 | sim_visionfloatparam_pov_aperture= 1015 558 | sim_visionintparam_pov_blur_sampled= 1016 559 | sim_visionintparam_render_mode= 1017 560 | 561 | sim_jointintparam_motor_enabled= 2000 562 | sim_jointintparam_ctrl_enabled= 2001 563 | sim_jointfloatparam_pid_p= 2002 564 | sim_jointfloatparam_pid_i= 2003 565 | sim_jointfloatparam_pid_d= 2004 566 | sim_jointfloatparam_intrinsic_x= 2005 567 | sim_jointfloatparam_intrinsic_y= 2006 568 | sim_jointfloatparam_intrinsic_z= 2007 569 | sim_jointfloatparam_intrinsic_qx= 2008 570 | sim_jointfloatparam_intrinsic_qy= 2009 571 | sim_jointfloatparam_intrinsic_qz= 2010 572 | sim_jointfloatparam_intrinsic_qw= 2011 573 | sim_jointfloatparam_velocity= 2012 574 | sim_jointfloatparam_spherical_qx= 2013 575 | sim_jointfloatparam_spherical_qy= 2014 576 | sim_jointfloatparam_spherical_qz= 2015 577 | sim_jointfloatparam_spherical_qw= 2016 578 | sim_jointfloatparam_upper_limit= 2017 579 | sim_jointfloatparam_kc_k= 2018 580 | sim_jointfloatparam_kc_c= 2019 581 | sim_jointfloatparam_ik_weight= 2021 582 | sim_jointfloatparam_error_x= 2022 583 | sim_jointfloatparam_error_y= 2023 584 | sim_jointfloatparam_error_z= 2024 585 | sim_jointfloatparam_error_a= 2025 586 | sim_jointfloatparam_error_b= 2026 587 | sim_jointfloatparam_error_g= 2027 588 | sim_jointfloatparam_error_pos= 2028 589 | sim_jointfloatparam_error_angle= 2029 590 | sim_jointintparam_velocity_lock= 2030 591 | sim_jointintparam_vortex_dep_handle= 2031 592 | sim_jointfloatparam_vortex_dep_multiplication= 2032 593 | sim_jointfloatparam_vortex_dep_offset= 2033 594 | 595 | sim_shapefloatparam_init_velocity_x= 3000 596 | sim_shapefloatparam_init_velocity_y= 3001 597 | sim_shapefloatparam_init_velocity_z= 3002 598 | sim_shapeintparam_static= 3003 599 | sim_shapeintparam_respondable= 3004 600 | sim_shapefloatparam_mass= 3005 601 | sim_shapefloatparam_texture_x= 3006 602 | sim_shapefloatparam_texture_y= 3007 603 | sim_shapefloatparam_texture_z= 3008 604 | sim_shapefloatparam_texture_a= 3009 605 | sim_shapefloatparam_texture_b= 3010 606 | sim_shapefloatparam_texture_g= 3011 607 | sim_shapefloatparam_texture_scaling_x= 3012 608 | sim_shapefloatparam_texture_scaling_y= 3013 609 | sim_shapeintparam_culling= 3014 610 | sim_shapeintparam_wireframe= 3015 611 | sim_shapeintparam_compound= 3016 612 | sim_shapeintparam_convex= 3017 613 | sim_shapeintparam_convex_check= 3018 614 | sim_shapeintparam_respondable_mask= 3019 615 | sim_shapefloatparam_init_velocity_a= 3020 616 | sim_shapefloatparam_init_velocity_b= 3021 617 | sim_shapefloatparam_init_velocity_g= 3022 618 | sim_shapestringparam_color_name= 3023 619 | sim_shapeintparam_edge_visibility= 3024 620 | sim_shapefloatparam_shading_angle= 3025 621 | sim_shapefloatparam_edge_angle= 3026 622 | sim_shapeintparam_edge_borders_hidden= 3027 623 | 624 | sim_proxintparam_ray_invisibility= 4000 625 | 626 | sim_forcefloatparam_error_x= 5000 627 | sim_forcefloatparam_error_y= 5001 628 | sim_forcefloatparam_error_z= 5002 629 | sim_forcefloatparam_error_a= 5003 630 | sim_forcefloatparam_error_b= 5004 631 | sim_forcefloatparam_error_g= 5005 632 | sim_forcefloatparam_error_pos= 5006 633 | sim_forcefloatparam_error_angle= 5007 634 | 635 | sim_lightintparam_pov_casts_shadows= 8000 636 | 637 | sim_cameraintparam_disabled_light_components= 9000 638 | sim_camerafloatparam_perspective_angle= 9001 639 | sim_camerafloatparam_ortho_size= 9002 640 | sim_cameraintparam_rendering_attributes= 9003 641 | sim_cameraintparam_pov_focal_blur= 9004 642 | sim_camerafloatparam_pov_blur_distance= 9005 643 | sim_camerafloatparam_pov_aperture= 9006 644 | sim_cameraintparam_pov_blur_samples= 9007 645 | 646 | sim_dummyintparam_link_type= 10000 647 | 648 | sim_mirrorfloatparam_width= 12000 649 | sim_mirrorfloatparam_height= 12001 650 | sim_mirrorfloatparam_reflectance= 12002 651 | sim_mirrorintparam_enable= 12003 652 | 653 | sim_pplanfloatparam_x_min= 20000 654 | sim_pplanfloatparam_x_range= 20001 655 | sim_pplanfloatparam_y_min= 20002 656 | sim_pplanfloatparam_y_range= 20003 657 | sim_pplanfloatparam_z_min= 20004 658 | sim_pplanfloatparam_z_range= 20005 659 | sim_pplanfloatparam_delta_min= 20006 660 | sim_pplanfloatparam_delta_range= 20007 661 | 662 | sim_mplanintparam_nodes_computed= 25000 663 | sim_mplanintparam_prepare_nodes= 25001 664 | sim_mplanintparam_clear_nodes= 25002 665 | 666 | # User interface elements 667 | sim_gui_menubar =0x0001 668 | sim_gui_popups =0x0002 669 | sim_gui_toolbar1 =0x0004 670 | sim_gui_toolbar2 =0x0008 671 | sim_gui_hierarchy =0x0010 672 | sim_gui_infobar =0x0020 673 | sim_gui_statusbar =0x0040 674 | sim_gui_scripteditor =0x0080 675 | sim_gui_scriptsimulationparameters =0x0100 676 | sim_gui_dialogs =0x0200 677 | sim_gui_browser =0x0400 678 | sim_gui_all =0xffff 679 | 680 | 681 | # Joint modes 682 | sim_jointmode_passive =0 683 | sim_jointmode_motion =1 684 | sim_jointmode_ik =2 685 | sim_jointmode_ikdependent =3 686 | sim_jointmode_dependent =4 687 | sim_jointmode_force =5 688 | 689 | 690 | # Navigation and selection modes with the mouse. Lower byte values are mutually exclusive upper byte bits can be combined 691 | sim_navigation_passive =0x0000 692 | sim_navigation_camerashift =0x0001 693 | sim_navigation_camerarotate =0x0002 694 | sim_navigation_camerazoom =0x0003 695 | sim_navigation_cameratilt =0x0004 696 | sim_navigation_cameraangle =0x0005 697 | sim_navigation_camerafly =0x0006 698 | sim_navigation_objectshift =0x0007 699 | sim_navigation_objectrotate =0x0008 700 | sim_navigation_reserved2 =0x0009 701 | sim_navigation_reserved3 =0x000A 702 | sim_navigation_jointpathtest =0x000B 703 | sim_navigation_ikmanip =0x000C 704 | sim_navigation_objectmultipleselection =0x000D 705 | # Bit-combine following values and add them to one of above's values for a valid navigation mode 706 | sim_navigation_reserved4 =0x0100 707 | sim_navigation_clickselection =0x0200 708 | sim_navigation_ctrlselection =0x0400 709 | sim_navigation_shiftselection =0x0800 710 | sim_navigation_camerazoomwheel =0x1000 711 | sim_navigation_camerarotaterightbutton =0x2000 712 | 713 | 714 | 715 | #Remote API constants 716 | SIMX_VERSION =0 717 | # Remote API message header structure 718 | SIMX_HEADER_SIZE =18 719 | simx_headeroffset_crc =0 # 1 simxUShort. Generated by the client or server. The CRC for the message 720 | simx_headeroffset_version =2 # 1 byte. Generated by the client or server. The version of the remote API software 721 | simx_headeroffset_message_id =3 # 1 simxInt. Generated by the client (and used in a reply by the server) 722 | simx_headeroffset_client_time =7 # 1 simxInt. Client time stamp generated by the client (and sent back by the server) 723 | simx_headeroffset_server_time =11 # 1 simxInt. Generated by the server when a reply is generated. The server timestamp 724 | simx_headeroffset_scene_id =15 # 1 simxUShort. Generated by the server. A unique ID identifying the scene currently displayed 725 | 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) 726 | 727 | # Remote API command header 728 | SIMX_SUBHEADER_SIZE =26 729 | simx_cmdheaderoffset_mem_size =0 # 1 simxInt. Generated by the client or server. The buffer size of the command. 730 | 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). 731 | simx_cmdheaderoffset_pdata_offset0 =8 # 1 simxUShort. Generated by the client or server. The amount of data that is part of the command identification. 732 | 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). 733 | 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. 734 | 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). 735 | 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) 736 | 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 737 | simx_cmdheaderoffset_reserved =25 # 1 byte. Not yet used 738 | 739 | 740 | 741 | 742 | 743 | # Regular operation modes 744 | simx_opmode_oneshot =0x000000 # sends command as one chunk. Reply will also come as one chunk. Doesn't wait for the reply. 745 | simx_opmode_blocking =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). 746 | 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). 747 | simx_opmode_continuous =0x020000 748 | simx_opmode_streaming =0x020000 # sends command as one chunk. Command will be stored on the server and always executed 749 | #(every x ms (as far as possible) where x can be 0-65535. just add x to opmode_continuous). 750 | # A reply will be sent continuously each time as one chunk. Doesn't wait for the reply. 751 | 752 | # Operation modes for heavy data 753 | 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. 754 | simx_opmode_continuous_split =0x040000 755 | 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. 756 | 757 | # Special operation modes 758 | simx_opmode_discontinue =0x050000 # removes and cancels all commands stored on the client or server side (also continuous commands) 759 | 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) 760 | 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) 761 | 762 | 763 | # Command return codes 764 | simx_return_ok =0x000000 765 | simx_return_novalue_flag =0x000001 # input buffer doesn't contain the specified command 766 | simx_return_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode 767 | simx_return_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode 768 | simx_return_remote_error_flag =0x000008 # command caused an error on the server side 769 | simx_return_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) 770 | simx_return_local_error_flag =0x000020 # command caused an error on the client side 771 | simx_return_initialize_error_flag =0x000040 # simxStart was not yet called 772 | 773 | # Following for backward compatibility (same as above) 774 | simx_error_noerror =0x000000 775 | simx_error_novalue_flag =0x000001 # input buffer doesn't contain the specified command 776 | simx_error_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode 777 | simx_error_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode 778 | simx_error_remote_error_flag =0x000008 # command caused an error on the server side 779 | simx_error_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) 780 | simx_error_local_error_flag =0x000020 # command caused an error on the client side 781 | simx_error_initialize_error_flag =0x000040 # simxStart was not yet called 782 | 783 | 784 | -------------------------------------------------------------------------------- /ur5_final.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/der-coder/hw01_UR5_coppeliaSim/c6d21b6eae57c359af094c084e00d00de4e1ba86/ur5_final.png -------------------------------------------------------------------------------- /ur5_home.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/der-coder/hw01_UR5_coppeliaSim/c6d21b6eae57c359af094c084e00d00de4e1ba86/ur5_home.png --------------------------------------------------------------------------------