├── .gitignore ├── CartPole ├── CartPoleEnv.py ├── CartPoleSimModel.py ├── cart_pole.ttt ├── saved_models │ ├── best_model_continuous.zip │ ├── best_model_discrete.zip │ └── tmp │ │ ├── best_model.zip │ │ ├── evaluations.npz │ │ └── monitor.csv └── simple_control.py ├── README.md ├── VREP_RemoteAPIs ├── complexCommandTest.py ├── depth_image_encoding.py ├── pController.py ├── pathPlanningTest.py ├── ply.py ├── readMe.txt ├── remoteApi.dll ├── remoteApi.so ├── sendMovementSequence-mov.py ├── sendMovementSequence-pts.py ├── sendSimultan2MovementSequences-mov.py ├── sim.py ├── simConst.py ├── simpleSynchronousTest.py ├── simpleTest.py ├── synchronousImageTransmission.py └── visualization.py ├── examples ├── demo_cart_pole_learning.py └── gym_cart_pole_a2c.py ├── pic └── cart_pole_demo_1.gif └── utils ├── callbackFunctions.py └── visdom_utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | *__pycache__ 2 | *.pyc 3 | CartPoleRL/saved_models/tmp/* 4 | LIPM/saved_models/tmp/* 5 | -------------------------------------------------------------------------------- /CartPole/CartPoleEnv.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import gym 3 | from gym.utils import seeding 4 | from gym import spaces, logger 5 | import time 6 | 7 | import sys 8 | sys.path.append('../VREP_RemoteAPIs') 9 | import sim as vrep_sim 10 | 11 | from CartPoleSimModel import CartPoleSimModel 12 | 13 | class CartPoleEnv(gym.Env): 14 | """Custom Environment that follows gym interface""" 15 | metadata = {'render.modes': ['human']} 16 | 17 | def __init__(self, action_type='descrete'): 18 | super(CartPoleEnv, self).__init__() 19 | self.action_type = action_type 20 | self.push_force = 0 21 | self.q = [0.0, 0.0] 22 | self.q_last = [0.0, 0.0] 23 | 24 | self.theta_max = 40*np.pi / 360 25 | self.cart_pos_max = 0.8 26 | 27 | high = np.array( 28 | [ 29 | self.cart_pos_max, 30 | self.theta_max, 31 | 1000000.0, 32 | 1000000.0 33 | ], 34 | dtype=np.float32, 35 | ) 36 | 37 | if self.action_type == 'discrete': 38 | self.action_space = spaces.Discrete(3) 39 | elif self.action_type == 'continuous': 40 | self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(1,), dtype=np.float32) 41 | else: 42 | assert 0, "The action type \'" + self.action_type + "\' can not be recognized" 43 | 44 | self.observation_space = spaces.Box(low=-high, high=high, dtype=np.float32) 45 | 46 | self.seed() 47 | self.state = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) 48 | self.counts = 0 49 | self.steps_beyond_done = None 50 | 51 | # Connect to VREP (CoppeliaSim) 52 | vrep_sim.simxFinish(-1) # close all opened connections 53 | while True: 54 | client_ID = vrep_sim.simxStart('127.0.0.1', 19997, True, False, 5000, 5) # Connect to CoppeliaSim 55 | if client_ID > -1: # connected 56 | print('Connect to remote API server.') 57 | break 58 | else: 59 | print('Failed connecting to remote API server! Try it again ...') 60 | 61 | # Open synchronous mode 62 | vrep_sim.simxSynchronous(client_ID, True) 63 | vrep_sim.simxStartSimulation(client_ID, vrep_sim.simx_opmode_oneshot) 64 | vrep_sim.simxSynchronousTrigger(client_ID) 65 | 66 | self.cart_pole_sim_model = CartPoleSimModel() 67 | self.cart_pole_sim_model.initializeSimModel(client_ID) 68 | vrep_sim.simxSynchronousTrigger(client_ID) 69 | 70 | def seed(self, seed=None): 71 | self.np_random, seed = seeding.np_random(seed) 72 | return [seed] 73 | 74 | def step(self, action): 75 | if self.action_type == 'discrete': 76 | assert self.action_space.contains(action), "%r (%s) invalid"%(action, type(action)) 77 | 78 | q = [0.0, 0.0] 79 | q[0] = self.cart_pole_sim_model.getJointPosition('prismatic_joint') 80 | q[1] = self.cart_pole_sim_model.getJointPosition('revolute_joint') 81 | self.q_last = self.q 82 | self.q = q 83 | 84 | if self.action_type == 'discrete': 85 | if action == 0: 86 | self.push_force = 0 87 | elif action == 1: 88 | self.push_force = 1.0 89 | elif action == 2: 90 | self.push_force = -1.0 91 | elif self.action_type == 'continuous': 92 | self.push_force = action*2.0 # The action is in [-1.0, 1.0], therefore the force is in [-2.0, 2.0] 93 | else: 94 | assert 0, "The action type \'" + self.action_type + "\' can not be recognized" 95 | 96 | # set action 97 | self.cart_pole_sim_model.setJointTorque(self.push_force) 98 | 99 | done = (q[0] <= -self.cart_pos_max) or (q[0] >= self.cart_pos_max) or (q[1] < -self.theta_max) or (q[1] > self.theta_max) 100 | done = bool(done) 101 | 102 | if not done: 103 | reward = 1.0 104 | elif self.steps_beyond_done is None: 105 | # Pole just fell! 106 | self.steps_beyond_done = 0 107 | reward = 1.0 108 | else: 109 | if self.steps_beyond_done == 0: 110 | logger.warn( 111 | "You are calling 'step()' even though this " 112 | "environment has already returned done = True. You " 113 | "should always call 'reset()' once you receive 'done = " 114 | "True' -- any further steps are undefined behavior." 115 | ) 116 | self.steps_beyond_done += 1 117 | reward = 0.0 118 | 119 | dt = 0.005 120 | self.v = [(self.q[0] - self.q_last[0])/dt, (self.q[1] - self.q_last[1])/dt] 121 | self.state = (self.q[0], self.q[1], self.v[0], self.v[1]) 122 | self.counts += 1 123 | 124 | vrep_sim.simxSynchronousTrigger(self.cart_pole_sim_model.client_ID) 125 | vrep_sim.simxGetPingTime(self.cart_pole_sim_model.client_ID) 126 | 127 | return np.array(self.state), reward, done, {} 128 | 129 | def reset(self): 130 | # print('Reset the environment after {} counts'.format(self.counts)) 131 | self.counts = 0 132 | self.push_force = 0 133 | self.state = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) 134 | self.steps_beyond_done = None 135 | 136 | vrep_sim.simxStopSimulation(self.cart_pole_sim_model.client_ID, vrep_sim.simx_opmode_blocking) # stop the simulation 137 | vrep_sim.simxGetPingTime(self.cart_pole_sim_model.client_ID) 138 | time.sleep(0.01) # ensure the coppeliasim is stopped 139 | vrep_sim.simxStartSimulation(self.cart_pole_sim_model.client_ID, vrep_sim.simx_opmode_oneshot) 140 | self.cart_pole_sim_model.setJointTorque(0) 141 | vrep_sim.simxSynchronousTrigger(self.cart_pole_sim_model.client_ID) 142 | vrep_sim.simxGetPingTime(self.cart_pole_sim_model.client_ID) 143 | 144 | return np.array(self.state) 145 | 146 | def render(self): 147 | return None 148 | 149 | def close(self): 150 | vrep_sim.simxStopSimulation(self.cart_pole_sim_model.client_ID, vrep_sim.simx_opmode_blocking) # stop the simulation 151 | vrep_sim.simxFinish(-1) # Close the connection 152 | print('Close the environment') 153 | return None 154 | 155 | if __name__ == "__main__": 156 | env = CartPoleEnv() 157 | env.reset() 158 | 159 | for _ in range(500): 160 | action = env.action_space.sample() # random action 161 | env.step(action) 162 | print(env.state) 163 | 164 | env.close() 165 | -------------------------------------------------------------------------------- /CartPole/CartPoleSimModel.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('../VREP_RemoteAPIs') 3 | import sim as vrep_sim 4 | 5 | # CartPole simulation model for VREP 6 | class CartPoleSimModel(): 7 | def __init__(self, name='CartPole'): 8 | """ 9 | :param: name: string 10 | name of objective 11 | """ 12 | super(self.__class__, self).__init__() 13 | self.name = name 14 | self.client_ID = None 15 | 16 | self.prismatic_joint_handle = None 17 | self.revolute_joint_handle = None 18 | 19 | def initializeSimModel(self, client_ID): 20 | try: 21 | print ('Connected to remote API server') 22 | client_ID != -1 23 | except: 24 | print ('Failed connecting to remote API server') 25 | 26 | self.client_ID = client_ID 27 | 28 | return_code, self.prismatic_joint_handle = vrep_sim.simxGetObjectHandle(client_ID, 'prismatic_joint', vrep_sim.simx_opmode_blocking) 29 | if (return_code == vrep_sim.simx_return_ok): 30 | print('get object prismatic joint ok.') 31 | 32 | return_code, self.revolute_joint_handle = vrep_sim.simxGetObjectHandle(client_ID, 'revolute_joint', vrep_sim.simx_opmode_blocking) 33 | if (return_code == vrep_sim.simx_return_ok): 34 | print('get object revolute joint ok.') 35 | 36 | # Get the joint position 37 | return_code, q = vrep_sim.simxGetJointPosition(self.client_ID, self.prismatic_joint_handle, vrep_sim.simx_opmode_streaming) 38 | return_code, q = vrep_sim.simxGetJointPosition(self.client_ID, self.revolute_joint_handle, vrep_sim.simx_opmode_streaming) 39 | 40 | # Set the initialized position for each joint 41 | self.setJointTorque(0) 42 | 43 | def getJointPosition(self, joint_name): 44 | """ 45 | :param: joint_name: string 46 | """ 47 | q = 0 48 | if joint_name == 'prismatic_joint': 49 | return_code, q = vrep_sim.simxGetJointPosition(self.client_ID, self.prismatic_joint_handle, vrep_sim.simx_opmode_buffer) 50 | elif joint_name == 'revolute_joint': 51 | return_code, q = vrep_sim.simxGetJointPosition(self.client_ID, self.revolute_joint_handle, vrep_sim.simx_opmode_buffer) 52 | else: 53 | print('Error: joint name: \' ' + joint_name + '\' can not be recognized.') 54 | 55 | return q 56 | 57 | def setJointTorque(self, torque): 58 | if torque >= 0: 59 | vrep_sim.simxSetJointTargetVelocity(self.client_ID, self.prismatic_joint_handle, 1000, vrep_sim.simx_opmode_oneshot) 60 | else: 61 | vrep_sim.simxSetJointTargetVelocity(self.client_ID, self.prismatic_joint_handle, -1000, vrep_sim.simx_opmode_oneshot) 62 | 63 | vrep_sim.simxSetJointMaxForce(self.client_ID, self.prismatic_joint_handle, abs(torque), vrep_sim.simx_opmode_oneshot) 64 | -------------------------------------------------------------------------------- /CartPole/cart_pole.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/CoppeliaSimRL/8eb98b8a42b515b61eae5ffebcc9f228e2f5fdd1/CartPole/cart_pole.ttt -------------------------------------------------------------------------------- /CartPole/saved_models/best_model_continuous.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/CoppeliaSimRL/8eb98b8a42b515b61eae5ffebcc9f228e2f5fdd1/CartPole/saved_models/best_model_continuous.zip -------------------------------------------------------------------------------- /CartPole/saved_models/best_model_discrete.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/CoppeliaSimRL/8eb98b8a42b515b61eae5ffebcc9f228e2f5fdd1/CartPole/saved_models/best_model_discrete.zip -------------------------------------------------------------------------------- /CartPole/saved_models/tmp/best_model.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/CoppeliaSimRL/8eb98b8a42b515b61eae5ffebcc9f228e2f5fdd1/CartPole/saved_models/tmp/best_model.zip -------------------------------------------------------------------------------- /CartPole/saved_models/tmp/evaluations.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/CoppeliaSimRL/8eb98b8a42b515b61eae5ffebcc9f228e2f5fdd1/CartPole/saved_models/tmp/evaluations.npz -------------------------------------------------------------------------------- /CartPole/saved_models/tmp/monitor.csv: -------------------------------------------------------------------------------- 1 | #{"t_start": 1629062307.8138704, "env_id": null} 2 | r,l,t 3 | 417.0,417,9.062629 4 | 363.0,363,16.94948 5 | 306.0,306,23.69615 6 | 478.0,478,33.939976 7 | 304.0,304,40.702388 8 | 319.0,319,47.606271 9 | 332.0,332,54.795058 10 | 291.0,291,61.143733 11 | 318.0,318,68.025896 12 | 339.0,339,75.374503 13 | -------------------------------------------------------------------------------- /CartPole/simple_control.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('../VREP_RemoteAPIs') 3 | import sim as vrep_sim 4 | from numpy import random 5 | 6 | from CartPoleSimModel import CartPoleSimModel 7 | 8 | 9 | print ('Program started') 10 | 11 | #%% ------------------------------- Connect to VREP (CoppeliaSim) ------------------------------- 12 | vrep_sim.simxFinish(-1) # just in case, close all opened connections 13 | while True: 14 | client_ID = vrep_sim.simxStart('127.0.0.1', 19997, True, False, 5000, 5) # Connect to CoppeliaSim 15 | if client_ID > -1: # connected 16 | print('Connect to remote API server.') 17 | break 18 | else: 19 | print('Failed connecting to remote API server! Try it again ...') 20 | 21 | # Open synchronous mode 22 | vrep_sim.simxSynchronous(client_ID, True) 23 | # Start simulation 24 | vrep_sim.simxStartSimulation(client_ID, vrep_sim.simx_opmode_oneshot) 25 | vrep_sim.simxSynchronousTrigger(client_ID) # trigger one simulation step, takes about 11 ms on Windows 10 26 | 27 | cart_pole_sim_model = CartPoleSimModel('CartPole') 28 | cart_pole_sim_model.initializeSimModel(client_ID) 29 | 30 | q = [0.0, 0.0] 31 | for i in range(1000): 32 | q[0] = cart_pole_sim_model.getJointPosition('prismatic_joint') 33 | q[1] = cart_pole_sim_model.getJointPosition('revolute_joint') 34 | print('q={}'.format(q)) 35 | 36 | action = random.uniform(-1.0, 1.0) 37 | cart_pole_sim_model.setJointTorque(action) 38 | 39 | vrep_sim.simxSynchronousTrigger(client_ID) 40 | _, ping_time = vrep_sim.simxGetPingTime(client_ID) # make sure the last simulation step is finished 41 | 42 | vrep_sim.simxStopSimulation(client_ID, vrep_sim.simx_opmode_blocking) # stop the simulation 43 | vrep_sim.simxFinish(-1) # Close the connection 44 | print('Program terminated') 45 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Reinforcement Learning Workspace 2 | 3 | The basic workspace for reinforcement learning with CoppeliaSim (VREP) simulation environments, including some demonstrated project for beginners. 4 | Some tutorials can be found at: 5 | 6 | - Zhihu: https://zhuanlan.zhihu.com/p/398874515 7 | - WeChat: https://mp.weixin.qq.com/s/id7fw0eGBtLqEqr-zaKKPQ 8 | 9 | All the demos have been tested on Ubuntu 20.04, with Anaconda python environment. 10 | 11 | 12 | --- 13 | ## Environments setup 14 | 15 | You have to install the following softwares and environments for this project, the recommend operating system is Ubuntu. 16 | - CoppeliaSim 4.2 (https://www.coppeliarobotics.com/) 17 | - Python 3.6+ (Anaconda is recommended) 18 | - Gym (https://github.com/openai/gym) 19 | - Stable-baselines3 (https://github.com/DLR-RM/stable-baselines3) 20 | - Pytorch (https://pytorch.org/) 21 | - Visdom (pip install visdom) 22 | 23 | 24 | --- 25 | ## Demo 1: Cart-pole control with the A2C (modified SAC) algorithm 26 | 27 | - Step 1: run CoppeliaSim, import cart_pole.ttt 28 | - Step 2: run visdom in your terminal, open your browser, and visit link: localhost:8097 29 | - Step 3: run the script named 'demo_cart_pole_learning.py' in the sub-path ./examples 30 | 31 | Then we have: 32 | 33 | ![](pic/cart_pole_demo_1.gif) 34 | 35 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/complexCommandTest.py: -------------------------------------------------------------------------------- 1 | # This example illustrates how to execute complex commands from 2 | # a remote API client. You can also use a similar construct for 3 | # commands that are not directly supported by the remote API. 4 | # 5 | # Load the demo scene 'remoteApiCommandServerExample.ttt' in CoppeliaSim, then 6 | # start the simulation and run this program. 7 | # 8 | # IMPORTANT: for each successful call to simxStart, there 9 | # should be a corresponding call to simxFinish at the end! 10 | 11 | try: 12 | import sim 13 | except: 14 | print ('--------------------------------------------------------------') 15 | print ('"sim.py" could not be imported. This means very probably that') 16 | print ('either "sim.py" or the remoteApi library could not be found.') 17 | print ('Make sure both are in the same folder as this file,') 18 | print ('or appropriately adjust the file "sim.py"') 19 | print ('--------------------------------------------------------------') 20 | print ('') 21 | 22 | import sys 23 | import ctypes 24 | print ('Program started') 25 | sim.simxFinish(-1) # just in case, close all opened connections 26 | clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim 27 | if clientID!=-1: 28 | print ('Connected to remote API server') 29 | 30 | # 1. First send a command to display a specific message in a dialog box: 31 | emptyBuff = bytearray() 32 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayText_function',[],[],['Hello world!'],emptyBuff,sim.simx_opmode_blocking) 33 | if res==sim.simx_return_ok: 34 | print ('Return string: ',retStrings[0]) # display the reply from CoppeliaSim (in this case, just a string) 35 | else: 36 | print ('Remote function call failed') 37 | 38 | # 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName': 39 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'createDummy_function',[],[0.1,0.2,0.3],['MyDummyName'],emptyBuff,sim.simx_opmode_blocking) 40 | if res==sim.simx_return_ok: 41 | print ('Dummy handle: ',retInts[0]) # display the reply from CoppeliaSim (in this case, the handle of the created dummy) 42 | else: 43 | print ('Remote function call failed') 44 | 45 | # 3. Now send a code string to execute some random functions: 46 | code="local octreeHandle=simCreateOctree(0.5,0,1)\n" \ 47 | "simInsertVoxelsIntoOctree(octreeHandle,0,{0.1,0.1,0.1},{255,0,255})\n" \ 48 | "return 'done'" 49 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,"remoteApiCommandServer",sim.sim_scripttype_childscript,'executeCode_function',[],[],[code],emptyBuff,sim.simx_opmode_blocking) 50 | if res==sim.simx_return_ok: 51 | print ('Code execution returned: ',retStrings[0]) 52 | else: 53 | print ('Remote function call failed') 54 | 55 | # Now close the connection to CoppeliaSim: 56 | sim.simxFinish(clientID) 57 | else: 58 | print ('Failed connecting to remote API server') 59 | print ('Program ended') 60 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/depth_image_encoding.py: -------------------------------------------------------------------------------- 1 | """Creates an image from a numpy array of floating point depth data. 2 | 3 | For details about the encoding see: 4 | https://sites.google.com/site/brainrobotdata/home/depth-image-encoding 5 | 6 | Examples: 7 | 8 | depth_array is a 2D numpy array of floating point depth data in meters. 9 | 10 | depth_rgb = FloatArrayToRgbImage(depth_array) 11 | depth_rgb is a PIL Image object containing the same data as 24-bit 12 | integers encoded in the RGB bytes. 13 | depth_rgb.save('image_file.png') - to save to a file. 14 | 15 | depth_array2 = ImageToFloatArray(depth_rgb) 16 | depth_array2 is a 2D numpy array containing the same data as 17 | depth_array up to the precision of the RGB image (1/256 mm). 18 | 19 | depth_gray = FloatArrayToGrayImage(depth_array) 20 | depth_gray is a PIL Image object containing the same data rounded to 21 | 8-bit integers. 22 | depth_gray.save('image_file.jpg', quality=95) - to save to a file. 23 | 24 | depth_array3 = ImageToFloatArray(depth_gray) 25 | depth_array3 is a 2D numpy array containing the same data as 26 | depth_array up to the precision of the grayscale image (1 cm). 27 | 28 | The image conversions first scale and round the values and then pack 29 | them into the desired type in a numpy array before converting the 30 | array to a PIL Image object. The Image can be saved in any format. 31 | We are using PNG for RGB and high quality JPEG for grayscale images. 32 | 33 | You can use different numeric types (e.g. np.uint16, np.int32), but 34 | not all combinations of numeric type and image format are supported by 35 | PIL or standard image viewers. 36 | 37 | """ 38 | 39 | import numpy as np 40 | from PIL import Image 41 | from skimage import img_as_ubyte 42 | from skimage.color import grey2rgb 43 | 44 | 45 | def ClipFloatValues(float_array, min_value, max_value): 46 | """Clips values to the range [min_value, max_value]. 47 | 48 | First checks if any values are out of range and prints a message. 49 | Then clips all values to the given range. 50 | 51 | Args: 52 | float_array: 2D array of floating point values to be clipped. 53 | min_value: Minimum value of clip range. 54 | max_value: Maximum value of clip range. 55 | 56 | Returns: 57 | The clipped array. 58 | 59 | """ 60 | if float_array.min() < min_value or float_array.max() > max_value: 61 | print('Image has out-of-range values [%f,%f] not in [%d,%d]', 62 | float_array.min(), float_array.max(), min_value, max_value) 63 | float_array = np.clip(float_array, min_value, max_value) 64 | return float_array 65 | 66 | 67 | DEFAULT_RGB_SCALE_FACTOR = 256000.0 68 | 69 | 70 | def FloatArrayToRgbImage(float_array, 71 | scale_factor=DEFAULT_RGB_SCALE_FACTOR, 72 | drop_blue=False): 73 | """Convert a floating point array of values to an RGB image. 74 | 75 | Convert floating point values to a fixed point representation where 76 | the RGB bytes represent a 24-bit integer. 77 | R is the high order byte. 78 | B is the low order byte. 79 | The precision of the depth image is 1/256 mm. 80 | 81 | Floating point values are scaled so that the integer values cover 82 | the representable range of depths. 83 | 84 | This image representation should only use lossless compression. 85 | 86 | Args: 87 | float_array: Input array of floating point depth values in meters. 88 | scale_factor: Scale value applied to all float values. 89 | drop_blue: Zero out the blue channel to improve compression, results in 1mm 90 | precision depth values. 91 | 92 | Returns: 93 | 24-bit RGB PIL Image object representing depth values. 94 | """ 95 | float_array = np.squeeze(float_array) 96 | # Scale the floating point array. 97 | scaled_array = np.floor(float_array * scale_factor + 0.5) 98 | # Convert the array to integer type and clip to representable range. 99 | min_inttype = 0 100 | max_inttype = 2**24 - 1 101 | scaled_array = ClipFloatValues(scaled_array, min_inttype, max_inttype) 102 | int_array = scaled_array.astype(np.uint32) 103 | # Calculate: 104 | # r = (f / 256) / 256 high byte 105 | # g = (f / 256) % 256 middle byte 106 | # b = f % 256 low byte 107 | rg = np.divide(int_array, 256) 108 | r = np.divide(rg, 256) 109 | g = np.mod(rg, 256) 110 | image_shape = int_array.shape 111 | rgb_array = np.zeros((image_shape[0], image_shape[1], 3), dtype=np.uint8) 112 | rgb_array[..., 0] = r 113 | rgb_array[..., 1] = g 114 | if not drop_blue: 115 | # Calculate the blue channel and add it to the array. 116 | b = np.mod(int_array, 256) 117 | rgb_array[..., 2] = b 118 | image_mode = 'RGB' 119 | image = Image.fromarray(rgb_array, mode=image_mode) 120 | return image 121 | 122 | 123 | DEFAULT_GRAY_SCALE_FACTOR = {np.uint8: 100.0, 124 | np.uint16: 1000.0, 125 | np.int32: DEFAULT_RGB_SCALE_FACTOR} 126 | 127 | 128 | def FloatArrayToGrayImage(float_array, scale_factor=None, image_dtype=np.uint8): 129 | """Convert a floating point array of values to an RGB image. 130 | 131 | Convert floating point values to a fixed point representation with 132 | the given bit depth. 133 | 134 | The precision of the depth image with default scale_factor is: 135 | uint8: 1cm, with a range of [0, 2.55m] 136 | uint16: 1mm, with a range of [0, 65.5m] 137 | int32: 1/256mm, with a range of [0, 8388m] 138 | 139 | Right now, PIL turns uint16 images into a very strange format and 140 | does not decode int32 images properly. Only uint8 works correctly. 141 | 142 | Args: 143 | float_array: Input array of floating point depth values in meters. 144 | scale_factor: Scale value applied to all float values. 145 | image_dtype: Image datatype, which controls the bit depth of the grayscale 146 | image. 147 | 148 | Returns: 149 | Grayscale PIL Image object representing depth values. 150 | 151 | """ 152 | # Ensure that we have a valid numeric type for the image. 153 | if image_dtype == np.uint16: 154 | image_mode = 'I;16' 155 | elif image_dtype == np.int32: 156 | image_mode = 'I' 157 | else: 158 | image_dtype = np.uint8 159 | image_mode = 'L' 160 | if scale_factor is None: 161 | scale_factor = DEFAULT_GRAY_SCALE_FACTOR[image_dtype] 162 | # Scale the floating point array. 163 | scaled_array = np.floor(float_array * scale_factor + 0.5) 164 | # Convert the array to integer type and clip to representable range. 165 | min_dtype = np.iinfo(image_dtype).min 166 | max_dtype = np.iinfo(image_dtype).max 167 | scaled_array = ClipFloatValues(scaled_array, min_dtype, max_dtype) 168 | 169 | image_array = scaled_array.astype(image_dtype) 170 | image = Image.fromarray(image_array, mode=image_mode) 171 | return image 172 | 173 | 174 | def ImageToFloatArray(image, scale_factor=None): 175 | """Recovers the depth values from an image. 176 | 177 | Reverses the depth to image conversion performed by FloatArrayToRgbImage or 178 | FloatArrayToGrayImage. 179 | 180 | The image is treated as an array of fixed point depth values. Each 181 | value is converted to float and scaled by the inverse of the factor 182 | that was used to generate the Image object from depth values. If 183 | scale_factor is specified, it should be the same value that was 184 | specified in the original conversion. 185 | 186 | The result of this function should be equal to the original input 187 | within the precision of the conversion. 188 | 189 | For details see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding. 190 | 191 | Args: 192 | image: Depth image output of FloatArrayTo[Format]Image. 193 | scale_factor: Fixed point scale factor. 194 | 195 | Returns: 196 | A 2D floating point numpy array representing a depth image. 197 | 198 | """ 199 | image_array = np.array(image) 200 | image_dtype = image_array.dtype 201 | image_shape = image_array.shape 202 | 203 | channels = image_shape[2] if len(image_shape) > 2 else 1 204 | assert 2 <= len(image_shape) <= 3 205 | if channels == 3: 206 | # RGB image needs to be converted to 24 bit integer. 207 | float_array = np.sum(image_array * [65536, 256, 1], axis=2) 208 | if scale_factor is None: 209 | scale_factor = DEFAULT_RGB_SCALE_FACTOR 210 | else: 211 | if scale_factor is None: 212 | scale_factor = DEFAULT_GRAY_SCALE_FACTOR[image_dtype.type] 213 | float_array = image_array.astype(np.float32) 214 | scaled_array = float_array / scale_factor 215 | return scaled_array 216 | 217 | 218 | def FloatArrayToRawRGB(im, min_value=0.0, max_value=1.0): 219 | """Convert a grayscale image to rgb, no encoding. 220 | 221 | For proper display try matplotlib's rendering/conversion instead of this version. 222 | Please be aware that this does not incorporate a proper color transform. 223 | http://pillow.readthedocs.io/en/3.4.x/reference/Image.html#PIL.Image.Image.convert 224 | https://en.wikipedia.org/wiki/Rec._601 225 | """ 226 | im = img_as_ubyte(im) 227 | if im.shape[-1] == 1: 228 | im = grey2rgb(im) 229 | return im 230 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/pController.py: -------------------------------------------------------------------------------- 1 | # Make sure to have CoppeliaSim running, with followig scene loaded: 2 | # 3 | # scenes/pControllerViaRemoteApi.ttt 4 | # 5 | # Do not launch simulation, but run this script 6 | # 7 | # The client side (i.e. this script) depends on: 8 | # 9 | # sim.py, simConst.py, and the remote API library available 10 | # in programming/remoteApiBindings/lib/lib 11 | 12 | try: 13 | import sim 14 | except: 15 | print ('--------------------------------------------------------------') 16 | print ('"sim.py" could not be imported. This means very probably that') 17 | print ('either "sim.py" or the remoteApi library could not be found.') 18 | print ('Make sure both are in the same folder as this file,') 19 | print ('or appropriately adjust the file "sim.py"') 20 | print ('--------------------------------------------------------------') 21 | print ('') 22 | 23 | import math 24 | 25 | class Client: 26 | def __enter__(self): 27 | self.intSignalName='legacyRemoteApiStepCounter' 28 | self.stepCounter=0 29 | self.maxForce=100 30 | sim.simxFinish(-1) # just in case, close all opened connections 31 | self.id=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim 32 | return self 33 | 34 | def __exit__(self,*err): 35 | sim.simxFinish(-1) 36 | 37 | with Client() as client: 38 | print("running") 39 | 40 | if client.id!=-1: 41 | print ('Connected to remote API server') 42 | 43 | def stepSimulation(): 44 | currentStep=client.stepCounter 45 | sim.simxSynchronousTrigger(client.id); 46 | while client.stepCounter==currentStep: 47 | retCode,s=sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_buffer) 48 | if retCode==sim.simx_return_ok: 49 | client.stepCounter=s 50 | 51 | def getCurrentJointAngle(): 52 | retCode=-1 53 | jointA=0 54 | while retCode!=sim.simx_return_ok: 55 | retCode,jointA=sim.simxGetJointPosition(client.id,client.jointHandle,sim.simx_opmode_buffer) 56 | return jointA 57 | 58 | def moveToAngle(targetAngle): 59 | jointAngle=getCurrentJointAngle() 60 | while abs(jointAngle-targetAngle)>0.1*math.pi/180: 61 | stepSimulation() 62 | jointAngle=getCurrentJointAngle() 63 | vel=computeTargetVelocity(jointAngle,targetAngle) 64 | sim.simxSetJointTargetVelocity(client.id,client.jointHandle,vel,sim.simx_opmode_oneshot) 65 | sim.simxSetJointMaxForce(client.id,client.jointHandle,client.maxForce,sim.simx_opmode_oneshot) 66 | 67 | def computeTargetVelocity(jointAngle,targetAngle): 68 | dynStepSize=0.005 69 | velUpperLimit=360*math.pi/180 70 | PID_P=0.1 71 | errorValue=targetAngle-jointAngle 72 | sinAngle=math.sin(errorValue) 73 | cosAngle=math.cos(errorValue) 74 | errorValue=math.atan2(sinAngle,cosAngle) 75 | ctrl=errorValue*PID_P 76 | 77 | # Calculate the velocity needed to reach the position in one dynamic time step: 78 | velocity=ctrl/dynStepSize 79 | if (velocity>velUpperLimit): 80 | velocity=velUpperLimit 81 | 82 | if (velocity<-velUpperLimit): 83 | velocity=-velUpperLimit 84 | 85 | return velocity 86 | 87 | # Start streaming client.intSignalName integer signal, that signals when a step is finished: 88 | sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_streaming) 89 | 90 | res,client.jointHandle=sim.simxGetObjectHandle(client.id,'joint',sim.simx_opmode_blocking) 91 | sim.simxSetJointTargetVelocity(client.id,client.jointHandle,360*math.pi/180,sim.simx_opmode_oneshot) 92 | sim.simxGetJointPosition(client.id,client.jointHandle,sim.simx_opmode_streaming) 93 | 94 | # enable the synchronous mode on the client: 95 | sim.simxSynchronous(client.id,True) 96 | 97 | sim.simxStartSimulation(client.id,sim.simx_opmode_oneshot) 98 | 99 | moveToAngle(45*math.pi/180) 100 | moveToAngle(90*math.pi/180) 101 | moveToAngle(-89*math.pi/180) #no -90, to avoid passing below 102 | moveToAngle(0*math.pi/180) 103 | 104 | sim.simxStopSimulation(client.id,sim.simx_opmode_blocking) 105 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/pathPlanningTest.py: -------------------------------------------------------------------------------- 1 | # This example illustrates how to use the path/motion 2 | # planning functionality from a remote API client. 3 | # 4 | # Load the demo scene 'motionPlanningServerDemo.ttt' in CoppeliaSim 5 | # then run this program. 6 | # 7 | # IMPORTANT: for each successful call to simxStart, there 8 | # should be a corresponding call to simxFinish at the end! 9 | 10 | import sim 11 | 12 | print ('Program started') 13 | sim.simxFinish(-1) # just in case, close all opened connections 14 | clientID=sim.simxStart('127.0.0.1',19997,True,True,-500000,5) # Connect to CoppeliaSim, set a very large time-out for blocking commands 15 | if clientID!=-1: 16 | print ('Connected to remote API server') 17 | 18 | emptyBuff = bytearray() 19 | 20 | # Start the simulation: 21 | sim.simxStartSimulation(clientID,sim.simx_opmode_oneshot_wait) 22 | 23 | # Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/coppeliaRobotics/qrelease/release/test.ttm'],emptyBuff,sim.simx_opmode_oneshot_wait) 24 | # robotHandle=retInts[0] 25 | 26 | # Retrieve some handles: 27 | res,robotHandle=sim.simxGetObjectHandle(clientID,'IRB4600#',sim.simx_opmode_oneshot_wait) 28 | res,target1=sim.simxGetObjectHandle(clientID,'testPose1#',sim.simx_opmode_oneshot_wait) 29 | res,target2=sim.simxGetObjectHandle(clientID,'testPose2#',sim.simx_opmode_oneshot_wait) 30 | res,target3=sim.simxGetObjectHandle(clientID,'testPose3#',sim.simx_opmode_oneshot_wait) 31 | res,target4=sim.simxGetObjectHandle(clientID,'testPose4#',sim.simx_opmode_oneshot_wait) 32 | 33 | # Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene 34 | res,retInts,target1Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target1],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 35 | res,retInts,target2Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target2],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 36 | res,retInts,target3Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target3],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 37 | res,retInts,target4Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target4],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 38 | 39 | # Get the robot initial state: 40 | res,retInts,robotInitialState,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 41 | 42 | # Some parameters: 43 | approachVector=[0,0,1] # often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose 44 | maxConfigsForDesiredPose=10 # we will try to find 10 different states corresponding to the goal pose and order them according to distance from initial state 45 | maxTrialsForConfigSearch=300 # a parameter needed for finding appropriate goal states 46 | searchCount=2 # how many times OMPL will run for a given task 47 | minConfigsForPathPlanningPath=400 # interpolation states for the OMPL path 48 | minConfigsForIkPath=100 # interpolation states for the linear approach path 49 | collisionChecking=1 # whether collision checking is on or off 50 | 51 | # Display a message: 52 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several path planning tasks for a given goal pose.&&nSeveral goal states corresponding to the goal pose are tested.&&nFeasability of a linear approach is also tested. Collision detection is on.'],emptyBuff,sim.simx_opmode_oneshot_wait) 53 | 54 | # Do the path planning here (between a start state and a goal pose, including a linear approach phase): 55 | inInts=[robotHandle,collisionChecking,minConfigsForIkPath,minConfigsForPathPlanningPath,maxConfigsForDesiredPose,maxTrialsForConfigSearch,searchCount] 56 | inFloats=robotInitialState+target1Pose+approachVector 57 | res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findPath_goalIsPose',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait) 58 | 59 | if (res==0) and len(path)>0: 60 | # The path could be in 2 parts: a path planning path, and a linear approach path: 61 | part1StateCnt=retInts[0] 62 | part2StateCnt=retInts[1] 63 | path1=path[:part1StateCnt*6] 64 | 65 | # Visualize the first path: 66 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path1,[],emptyBuff,sim.simx_opmode_oneshot_wait) 67 | line1Handle=retInts[0] 68 | 69 | # Make the robot follow the path: 70 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path1,[],emptyBuff,sim.simx_opmode_oneshot_wait) 71 | 72 | # Wait until the end of the movement: 73 | runningPath=True 74 | while runningPath: 75 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 76 | runningPath=retInts[0]==1 77 | 78 | path2=path[part1StateCnt*6:] 79 | 80 | # Visualize the second path (the linear approach): 81 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,0,255,0],path2,[],emptyBuff,sim.simx_opmode_oneshot_wait) 82 | line2Handle=retInts[0] 83 | 84 | # Make the robot follow the path: 85 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path2,[],emptyBuff,sim.simx_opmode_oneshot_wait) 86 | 87 | # Wait until the end of the movement: 88 | runningPath=True 89 | while runningPath: 90 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 91 | runningPath=retInts[0]==1 92 | 93 | # Clear the paths visualizations: 94 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line1Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 95 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line2Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 96 | 97 | # Get the robot current state: 98 | res,retInts,robotCurrentConfig,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 99 | 100 | # Display a message: 101 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several path planning tasks for a given goal state. Collision detection is on.'],emptyBuff,sim.simx_opmode_oneshot_wait) 102 | 103 | # Do the path planning here (between a start state and a goal state): 104 | inInts=[robotHandle,collisionChecking,minConfigsForPathPlanningPath,searchCount] 105 | inFloats=robotCurrentConfig+robotInitialState 106 | res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findPath_goalIsState',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait) 107 | 108 | if (res==0) and len(path)>0: 109 | # Visualize the path: 110 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait) 111 | lineHandle=retInts[0] 112 | 113 | # Make the robot follow the path: 114 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait) 115 | 116 | # Wait until the end of the movement: 117 | runningPath=True 118 | while runningPath: 119 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 120 | runningPath=retInts[0]==1 121 | 122 | # Clear the path visualization: 123 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[lineHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 124 | 125 | # Collision checking off: 126 | collisionChecking=0 127 | 128 | # Display a message: 129 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several linear paths, going through several waypoints. Collision detection is OFF.'],emptyBuff,sim.simx_opmode_oneshot_wait) 130 | 131 | # Find a linear path that runs through several poses: 132 | inInts=[robotHandle,collisionChecking,minConfigsForIkPath] 133 | inFloats=robotInitialState+target2Pose+target1Pose+target3Pose+target4Pose 134 | res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findIkPath',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait) 135 | 136 | if (res==0) and len(path)>0: 137 | # Visualize the path: 138 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,0,255,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait) 139 | line1Handle=retInts[0] 140 | 141 | # Make the robot follow the path: 142 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait) 143 | 144 | # Wait until the end of the movement: 145 | runningPath=True 146 | while runningPath: 147 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 148 | runningPath=retInts[0]==1 149 | 150 | # Clear the path visualization: 151 | res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line1Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait) 152 | 153 | # Stop simulation: 154 | sim.simxStopSimulation(clientID,sim.simx_opmode_oneshot_wait) 155 | 156 | # Now close the connection to CoppeliaSim: 157 | sim.simxFinish(clientID) 158 | else: 159 | print ('Failed connecting to remote API server') 160 | print ('Program ended') 161 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/ply.py: -------------------------------------------------------------------------------- 1 | # MIT LICENSE 2 | # https://github.com/bponsler/kinectToPly/blob/master/kinectToPly.py 3 | import numpy as np 4 | 5 | 6 | class Ply(object): 7 | '''The Ply class provides the ability to write a point cloud represented 8 | by two arrays: an array of points (num points, 3), and an array of colors 9 | (num points, 3) to a PLY file. 10 | ''' 11 | 12 | def __init__(self, points, colors): 13 | ''' 14 | * points -- The matrix of points (num points, 3) 15 | * colors -- The matrix of colors (num points, 3) 16 | ''' 17 | self.__points = points 18 | self.__colors = colors 19 | 20 | def write(self, filename): 21 | '''Write the point cloud data to a PLY file of the given name. 22 | * filename -- The PLY file 23 | ''' 24 | # Write the headers 25 | lines = self.__getLinesForHeader() 26 | 27 | fd = open(filename, "w") 28 | for line in lines: 29 | fd.write("%s\n" % line) 30 | 31 | # Write the points 32 | self.__writePoints(fd, self.__points, self.__colors) 33 | 34 | fd.close() 35 | 36 | def __getLinesForHeader(self): 37 | '''Get the list of lines for the PLY header.''' 38 | lines = [ 39 | "ply", 40 | "format ascii 1.0", 41 | "comment generated by: kinectToPly", 42 | "element vertex %s" % len(self.__points), 43 | "property float x", 44 | "property float y", 45 | "property float z", 46 | "property uchar red", 47 | "property uchar green", 48 | "property uchar blue", 49 | "end_header", 50 | ] 51 | 52 | return lines 53 | 54 | def __writePoints(self, fd, points, colors): 55 | '''Write the point cloud points to a file. 56 | * fd -- The file descriptor 57 | * points -- The matrix of points (num points, 3) 58 | * colors -- The matrix of colors (num points, 3) 59 | ''' 60 | # Stack the two arrays together 61 | stacked = np.column_stack((points, colors)) 62 | 63 | # Write the array to the file 64 | np.savetxt( 65 | fd, 66 | stacked, 67 | delimiter='\n', 68 | fmt="%f %f %f %d %d %d") 69 | 70 | 71 | def write_xyz_rgb_as_ply(point_cloud, rgb_image, path): 72 | """Write a point cloud with associated rgb image to a ply file 73 | 74 | # Arguments 75 | 76 | point_cloud: xyz point cloud in format [height, width, channels] 77 | rgb_image: uint8 image in format [height, width, channels] 78 | path: Where to save the file, ex: '/path/to/folder/file.ply' 79 | """ 80 | xyz = point_cloud.reshape([point_cloud.size/3, 3]) 81 | rgb = np.squeeze(rgb_image).reshape([point_cloud.size/3, 3]) 82 | ply = Ply(xyz, rgb) 83 | ply.write(path) 84 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/readMe.txt: -------------------------------------------------------------------------------- 1 | Make sure you have following files in your directory, in order to run the various examples: 2 | 3 | 1. sim.py 4 | 2. simConst.py 5 | 3. the appropriate remote API library: "remoteApi.dll" (Windows), "remoteApi.dylib" (Mac) or "remoteApi.so" (Linux) 6 | 4. simpleTest.py (or any other example file) -------------------------------------------------------------------------------- /VREP_RemoteAPIs/remoteApi.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/CoppeliaSimRL/8eb98b8a42b515b61eae5ffebcc9f228e2f5fdd1/VREP_RemoteAPIs/remoteApi.dll -------------------------------------------------------------------------------- /VREP_RemoteAPIs/remoteApi.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/CoppeliaSimRL/8eb98b8a42b515b61eae5ffebcc9f228e2f5fdd1/VREP_RemoteAPIs/remoteApi.so -------------------------------------------------------------------------------- /VREP_RemoteAPIs/sendMovementSequence-mov.py: -------------------------------------------------------------------------------- 1 | # Make sure to have CoppeliaSim running, with followig scene loaded: 2 | # 3 | # scenes/movementViaRemoteApi.ttt 4 | # 5 | # Do not launch simulation, then run this script 6 | # 7 | # The client side (i.e. this script) depends on: 8 | # 9 | # sim.py, simConst.py, and the remote API library available 10 | # in programming/remoteApiBindings/lib/lib 11 | # Additionally you will need the python math and msgpack modules 12 | 13 | try: 14 | import sim 15 | except: 16 | print ('--------------------------------------------------------------') 17 | print ('"sim.py" could not be imported. This means very probably that') 18 | print ('either "sim.py" or the remoteApi library could not be found.') 19 | print ('Make sure both are in the same folder as this file,') 20 | print ('or appropriately adjust the file "sim.py"') 21 | print ('--------------------------------------------------------------') 22 | print ('') 23 | 24 | import math 25 | import msgpack 26 | 27 | print ('Program started') 28 | sim.simxFinish(-1) # just in case, close all opened connections 29 | clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim 30 | if clientID!=-1: 31 | print ('Connected to remote API server') 32 | 33 | executedMovId='notReady' 34 | 35 | targetArm='threadedBlueArm' 36 | #targetArm='nonThreadedRedArm' 37 | 38 | stringSignalName=targetArm+'_executedMovId' 39 | 40 | def waitForMovementExecuted(id): 41 | global executedMovId 42 | global stringSignalName 43 | while executedMovId!=id: 44 | retCode,s=sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_buffer) 45 | if retCode==sim.simx_return_ok: 46 | executedMovId=s 47 | 48 | # Start streaming stringSignalName string signal: 49 | sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_streaming) 50 | 51 | # Set-up some movement variables: 52 | mVel=100*math.pi/180 53 | mAccel=150*math.pi/180 54 | maxVel=[mVel,mVel,mVel,mVel,mVel,mVel] 55 | maxAccel=[mAccel,mAccel,mAccel,mAccel,mAccel,mAccel] 56 | targetVel=[0,0,0,0,0,0] 57 | 58 | # Start simulation: 59 | sim.simxStartSimulation(clientID,sim.simx_opmode_blocking) 60 | 61 | # Wait until ready: 62 | waitForMovementExecuted('ready') 63 | 64 | # Send first movement sequence: 65 | targetConfig=[90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180] 66 | movementData={"id":"movSeq1","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} 67 | packedMovementData=msgpack.packb(movementData) 68 | sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) 69 | 70 | # Execute first movement sequence: 71 | sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot) 72 | 73 | # Wait until above movement sequence finished executing: 74 | waitForMovementExecuted('movSeq1') 75 | 76 | # Send second and third movement sequence, where third one should execute immediately after the second one: 77 | targetConfig=[-90*math.pi/180,45*math.pi/180,90*math.pi/180,135*math.pi/180,90*math.pi/180,90*math.pi/180] 78 | targetVel=[-60*math.pi/180,-20*math.pi/180,0,0,0,0] 79 | movementData={"id":"movSeq2","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} 80 | packedMovementData=msgpack.packb(movementData) 81 | sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) 82 | targetConfig=[0,0,0,0,0,0] 83 | targetVel=[0,0,0,0,0,0] 84 | movementData={"id":"movSeq3","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} 85 | packedMovementData=msgpack.packb(movementData) 86 | sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) 87 | 88 | # Execute second and third movement sequence: 89 | sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot) 90 | sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot) 91 | 92 | # Wait until above 2 movement sequences finished executing: 93 | waitForMovementExecuted('movSeq3') 94 | sim.simxStopSimulation(clientID,sim.simx_opmode_blocking) 95 | sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_discontinue) 96 | sim.simxGetPingTime(clientID) 97 | 98 | # Now close the connection to CoppeliaSim: 99 | sim.simxFinish(clientID) 100 | else: 101 | print ('Failed connecting to remote API server') 102 | print ('Program ended') 103 | 104 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/sendMovementSequence-pts.py: -------------------------------------------------------------------------------- 1 | # Make sure to have CoppeliaSim running, with followig scene loaded: 2 | # 3 | # scenes/movementViaRemoteApi.ttt 4 | # 5 | # Do not launch simulation, then run this script 6 | # 7 | # The client side (i.e. this script) depends on: 8 | # 9 | # sim.py, simConst.py, and the remote API library available 10 | # in programming/remoteApiBindings/lib/lib 11 | # Additionally you will need the python math and msgpack modules 12 | 13 | try: 14 | import sim 15 | except: 16 | print ('--------------------------------------------------------------') 17 | print ('"sim.py" could not be imported. This means very probably that') 18 | print ('either "sim.py" or the remoteApi library could not be found.') 19 | print ('Make sure both are in the same folder as this file,') 20 | print ('or appropriately adjust the file "sim.py"') 21 | print ('--------------------------------------------------------------') 22 | print ('') 23 | 24 | import time 25 | import math 26 | import msgpack 27 | 28 | print ('Program started') 29 | sim.simxFinish(-1) # just in case, close all opened connections 30 | clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim 31 | if clientID!=-1: 32 | print ('Connected to remote API server') 33 | 34 | executedMovId='notReady' 35 | 36 | targetArm='threadedBlueArm' 37 | #targetArm='nonThreadedRedArm' 38 | 39 | stringSignalName=targetArm+'_executedMovId' 40 | 41 | def waitForMovementExecuted(id): 42 | global executedMovId 43 | global stringSignalName 44 | while executedMovId!=id: 45 | retCode,s=sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_buffer) 46 | if retCode==sim.simx_return_ok: 47 | executedMovId=s 48 | 49 | # Start streaming stringSignalName string signal: 50 | sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_streaming) 51 | 52 | # Set-up some movement variables: 53 | times=[0.000,0.050,0.100,0.150,0.200,0.250,0.300,0.350,0.400,0.450,0.500,0.550,0.600,0.650,0.700,0.750,0.800,0.850,0.900,0.950,1.000,1.050,1.100,1.150,1.200,1.250,1.300,1.350,1.400,1.450,1.500,1.550,1.600,1.650,1.700,1.750,1.800,1.850] 54 | j1=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.360,0.431,0.506,0.587,0.669,0.753,0.838,0.923,1.008,1.091,1.170,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571] 55 | j2=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.140,0.185,0.237,0.296,0.361,0.431,0.507,0.587,0.670,0.754,0.838,0.924,1.009,1.092,1.171,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571] 56 | j3=[0.000,0.000,-0.002,-0.009,-0.022,-0.042,-0.068,-0.100,-0.139,-0.185,-0.237,-0.296,-0.361,-0.433,-0.511,-0.595,-0.681,-0.767,-0.854,-0.942,-1.027,-1.107,-1.182,-1.249,-1.311,-1.365,-1.414,-1.455,-1.491,-1.519,-1.541,-1.557,-1.566,-1.569,-1.570,-1.571,-1.571,-1.571] 57 | j4=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.107,1.181,1.249,1.310,1.365,1.413,1.455,1.490,1.519,1.541,1.556,1.565,1.569,1.570,1.570,1.570,1.571] 58 | j5=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.028,1.108,1.182,1.250,1.311,1.366,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571] 59 | j6=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.108,1.182,1.249,1.311,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.569,1.570,1.571,1.571,1.571] 60 | 61 | times=[0.000,0.050,0.100,0.150,0.200,0.250,0.300,0.350,0.400,0.450,0.500,0.550,0.600,0.650,0.700,0.750,0.800,0.850,0.900,0.950,1.000,1.050,1.100,1.150,1.200,1.250,1.300,1.350,1.400,1.450,1.500,1.550,1.600,1.650,1.700,1.750,1.800,1.850,1.900,1.950,2.000,2.050,2.100,2.150,2.200,2.250,2.300,2.350,2.400,2.450,2.500,2.550,2.600,2.650,2.700,2.750,2.800,2.850,2.900,2.950,3.000,3.050,3.100,3.150,3.200,3.250,3.300,3.350,3.400,3.450,3.500,3.550,3.600,3.650,3.700,3.750,3.800,3.850,3.900,3.950,4.000,4.050,4.100,4.150,4.200,4.250,4.300,4.350,4.400,4.450,4.500,4.550,4.600,4.650,4.700,4.750,4.800,4.850,4.900,4.950,5.000,5.050,5.100,5.150,5.200,5.250,5.300,5.350,5.400,5.450,5.500,5.550,5.600,5.650,5.700,5.750,5.800,5.850,5.900,5.950,6.000,6.050,6.100,6.150,6.200,6.250,6.300,6.350] 62 | j1=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.360,0.431,0.506,0.587,0.669,0.753,0.838,0.923,1.008,1.091,1.170,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.564,1.556,1.543,1.524,1.498,1.465,1.426,1.380,1.328,1.270,1.205,1.136,1.065,0.994,0.922,0.849,0.777,0.705,0.632,0.560,0.487,0.415,0.342,0.270,0.197,0.125,0.053,-0.020,-0.092,-0.165,-0.237,-0.309,-0.382,-0.454,-0.527,-0.599,-0.671,-0.744,-0.816,-0.888,-0.961,-1.033,-1.106,-1.178,-1.250,-1.323,-1.394,-1.462,-1.523,-1.556,-1.595,-1.632,-1.664,-1.690,-1.709,-1.723,-1.729,-1.730,-1.723,-1.710,-1.691,-1.665,-1.632,-1.593,-1.548,-1.495,-1.437,-1.372,-1.302,-1.226,-1.146,-1.064,-0.980,-0.895,-0.810,-0.724,-0.638,-0.552,-0.469,-0.390,-0.318,-0.254,-0.199,-0.151,-0.110,-0.076,-0.048,-0.027,-0.012,-0.004,-0.001,-0.001,-0.000,-0.000,-0.000] 63 | j2=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.140,0.185,0.237,0.296,0.361,0.431,0.507,0.587,0.670,0.754,0.838,0.924,1.009,1.092,1.171,1.243,1.308,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.564,1.557,1.544,1.529,1.513,1.497,1.481,1.465,1.449,1.432,1.416,1.400,1.384,1.367,1.351,1.335,1.319,1.303,1.286,1.270,1.254,1.238,1.221,1.205,1.189,1.173,1.157,1.140,1.124,1.108,1.092,1.075,1.059,1.043,1.027,1.010,0.994,0.978,0.961,0.945,0.929,0.913,0.896,0.880,0.864,0.848,0.831,0.815,0.799,0.786,0.769,0.749,0.730,0.710,0.689,0.669,0.649,0.629,0.609,0.589,0.569,0.548,0.528,0.508,0.488,0.468,0.448,0.427,0.407,0.387,0.367,0.347,0.327,0.306,0.286,0.266,0.246,0.226,0.206,0.186,0.166,0.146,0.125,0.105,0.084,0.064,0.044,0.025,0.012,0.004,0.001,0.000,0.000,0.000,0.000] 64 | j3=[0.000,0.000,-0.002,-0.009,-0.022,-0.042,-0.068,-0.100,-0.139,-0.185,-0.237,-0.296,-0.361,-0.433,-0.511,-0.595,-0.681,-0.767,-0.854,-0.942,-1.027,-1.107,-1.182,-1.249,-1.311,-1.365,-1.414,-1.455,-1.491,-1.519,-1.541,-1.557,-1.566,-1.564,-1.556,-1.543,-1.524,-1.498,-1.465,-1.426,-1.381,-1.328,-1.270,-1.205,-1.133,-1.055,-0.971,-0.885,-0.798,-0.711,-0.624,-0.537,-0.450,-0.362,-0.275,-0.188,-0.101,-0.013,0.074,0.161,0.249,0.336,0.423,0.510,0.598,0.685,0.772,0.859,0.947,1.032,1.112,1.186,1.253,1.314,1.369,1.416,1.458,1.492,1.521,1.542,1.557,1.566,1.564,1.557,1.544,1.524,1.498,1.466,1.427,1.383,1.338,1.293,1.247,1.201,1.155,1.110,1.064,1.018,0.972,0.926,0.881,0.835,0.789,0.743,0.697,0.652,0.606,0.560,0.514,0.468,0.423,0.377,0.331,0.285,0.239,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000] 65 | j4=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.107,1.181,1.249,1.310,1.365,1.413,1.455,1.490,1.519,1.541,1.556,1.565,1.567,1.574,1.587,1.603,1.619,1.636,1.653,1.670,1.686,1.703,1.720,1.737,1.754,1.771,1.788,1.805,1.822,1.839,1.856,1.873,1.890,1.907,1.923,1.940,1.957,1.974,1.991,2.008,2.025,2.042,2.059,2.076,2.093,2.110,2.127,2.144,2.161,2.178,2.195,2.212,2.229,2.246,2.263,2.280,2.297,2.314,2.331,2.344,2.352,2.350,2.343,2.330,2.310,2.284,2.252,2.212,2.167,2.115,2.056,1.991,1.919,1.841,1.760,1.679,1.597,1.515,1.433,1.350,1.268,1.186,1.104,1.022,0.940,0.858,0.776,0.694,0.612,0.530,0.452,0.379,0.312,0.252,0.198,0.151,0.110,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000] 66 | j5=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.028,1.108,1.182,1.250,1.311,1.366,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.571,1.571,1.568,1.561,1.548,1.529,1.503,1.470,1.431,1.388,1.342,1.297,1.251,1.205,1.159,1.113,1.067,1.021,0.975,0.929,0.883,0.837,0.791,0.745,0.699,0.653,0.607,0.561,0.516,0.470,0.424,0.378,0.332,0.286,0.240,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000] 67 | j6=[0.000,0.000,0.002,0.009,0.022,0.042,0.068,0.100,0.139,0.185,0.237,0.296,0.361,0.433,0.511,0.595,0.681,0.768,0.855,0.942,1.027,1.108,1.182,1.249,1.311,1.365,1.414,1.455,1.491,1.519,1.541,1.557,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.566,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.567,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.568,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.569,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.570,1.571,1.571,1.571,1.569,1.561,1.548,1.529,1.503,1.470,1.431,1.388,1.343,1.297,1.251,1.205,1.159,1.113,1.067,1.021,0.975,0.929,0.883,0.837,0.791,0.745,0.699,0.653,0.607,0.561,0.515,0.470,0.424,0.378,0.332,0.286,0.240,0.194,0.149,0.109,0.076,0.048,0.027,0.012,0.004,0.002,0.001,0.000,0.000,0.000] 68 | 69 | # Start simulation: 70 | sim.simxStartSimulation(clientID,sim.simx_opmode_blocking) 71 | 72 | # Wait until ready: 73 | waitForMovementExecuted('ready') 74 | 75 | # Send the movement sequence: 76 | targetConfig=[90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180] 77 | movementData={"id":"movSeq1","type":"pts","times":times,"j1":j1,"j2":j2,"j3":j3,"j4":j4,"j5":j5,"j6":j6} 78 | packedMovementData=msgpack.packb(movementData) 79 | sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) 80 | 81 | # Execute movement sequence: 82 | sim.simxCallScriptFunction(clientID,targetArm,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot) 83 | 84 | # Wait until above movement sequence finished executing: 85 | waitForMovementExecuted('movSeq1') 86 | 87 | sim.simxStopSimulation(clientID,sim.simx_opmode_blocking) 88 | sim.simxGetIntegerSignal(clientID,integerSignalName,sim.simx_opmode_discontinue) 89 | sim.simxGetPingTime(clientID) 90 | 91 | # Now close the connection to CoppeliaSim: 92 | sim.simxFinish(clientID) 93 | else: 94 | print ('Failed connecting to remote API server') 95 | print ('Program ended') 96 | 97 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/sendSimultan2MovementSequences-mov.py: -------------------------------------------------------------------------------- 1 | # Make sure to have CoppeliaSim running, with followig scene loaded: 2 | # 3 | # scenes/movementViaRemoteApi.ttt 4 | # 5 | # Do not launch simulation, then run this script 6 | # 7 | # The client side (i.e. this script) depends on: 8 | # 9 | # sim.py, simConst.py, and the remote API library available 10 | # in programming/remoteApiBindings/lib/lib 11 | # Additionally you will need the python math and msgpack modules 12 | 13 | try: 14 | import sim 15 | except: 16 | print ('--------------------------------------------------------------') 17 | print ('"sim.py" could not be imported. This means very probably that') 18 | print ('either "sim.py" or the remoteApi library could not be found.') 19 | print ('Make sure both are in the same folder as this file,') 20 | print ('or appropriately adjust the file "sim.py"') 21 | print ('--------------------------------------------------------------') 22 | print ('') 23 | 24 | import math 25 | import msgpack 26 | 27 | class Client: 28 | def __enter__(self): 29 | self.executedMovId1='notReady' 30 | self.executedMovId2='notReady' 31 | sim.simxFinish(-1) # just in case, close all opened connections 32 | self.id=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim 33 | return self 34 | 35 | def __exit__(self,*err): 36 | sim.simxFinish(-1) 37 | print ('Program ended') 38 | 39 | with Client() as client: 40 | print("running") 41 | 42 | if client.id!=-1: 43 | print ('Connected to remote API server') 44 | 45 | targetArm1='threadedBlueArm' 46 | targetArm2='nonThreadedRedArm' 47 | 48 | client.stringSignalName1=targetArm1+'_executedMovId' 49 | client.stringSignalName2=targetArm2+'_executedMovId' 50 | 51 | def waitForMovementExecuted1(id): 52 | while client.executedMovId1!=id: 53 | retCode,s=sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_buffer) 54 | if retCode==sim.simx_return_ok: 55 | client.executedMovId1=s 56 | 57 | def waitForMovementExecuted2(id): 58 | while client.executedMovId2!=id: 59 | retCode,s=sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_buffer) 60 | if retCode==sim.simx_return_ok: 61 | client.executedMovId2=s 62 | 63 | # Start streaming client.stringSignalName1 and client.stringSignalName2 string signals: 64 | sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_streaming) 65 | sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_streaming) 66 | 67 | # Set-up some movement variables: 68 | mVel=100*math.pi/180 69 | mAccel=150*math.pi/180 70 | maxVel=[mVel,mVel,mVel,mVel,mVel,mVel] 71 | maxAccel=[mAccel,mAccel,mAccel,mAccel,mAccel,mAccel] 72 | targetVel=[0,0,0,0,0,0] 73 | 74 | # Start simulation: 75 | sim.simxStartSimulation(client.id,sim.simx_opmode_blocking) 76 | 77 | # Wait until ready: 78 | waitForMovementExecuted1('ready') 79 | waitForMovementExecuted1('ready') 80 | 81 | # Send first movement sequence: 82 | targetConfig=[90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180] 83 | movementData={"id":"movSeq1","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} 84 | packedMovementData=msgpack.packb(movementData) 85 | sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) 86 | sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) 87 | 88 | # Execute first movement sequence: 89 | sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot) 90 | sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq1',sim.simx_opmode_oneshot) 91 | 92 | # Wait until above movement sequence finished executing: 93 | waitForMovementExecuted1('movSeq1') 94 | waitForMovementExecuted1('movSeq1') 95 | 96 | # Send second and third movement sequence, where third one should execute immediately after the second one: 97 | targetConfig=[-90*math.pi/180,45*math.pi/180,90*math.pi/180,135*math.pi/180,90*math.pi/180,90*math.pi/180] 98 | targetVel=[-60*math.pi/180,-20*math.pi/180,0,0,0,0] 99 | movementData={"id":"movSeq2","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} 100 | packedMovementData=msgpack.packb(movementData) 101 | sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) 102 | sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) 103 | targetConfig=[0,0,0,0,0,0] 104 | targetVel=[0,0,0,0,0,0] 105 | movementData={"id":"movSeq3","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} 106 | packedMovementData=msgpack.packb(movementData) 107 | sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) 108 | sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) 109 | 110 | # Execute second and third movement sequence: 111 | sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot) 112 | sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot) 113 | sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot) 114 | sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot) 115 | 116 | # Wait until above 2 movement sequences finished executing: 117 | waitForMovementExecuted1('movSeq3') 118 | waitForMovementExecuted1('movSeq3') 119 | 120 | sim.simxStopSimulation(client.id,sim.simx_opmode_blocking) 121 | sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_discontinue) 122 | sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_discontinue) 123 | sim.simxGetPingTime(client.id) 124 | 125 | # Now close the connection to CoppeliaSim: 126 | sim.simxFinish(client.id) 127 | else: 128 | print ('Failed connecting to remote API server') 129 | 130 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/simConst.py: -------------------------------------------------------------------------------- 1 | #constants 2 | #Scene object types. Values are serialized 3 | sim_object_shape_type =0 4 | sim_object_joint_type =1 5 | sim_object_graph_type =2 6 | sim_object_camera_type =3 7 | sim_object_dummy_type =4 8 | sim_object_proximitysensor_type =5 9 | sim_object_reserved1 =6 10 | sim_object_reserved2 =7 11 | sim_object_path_type =8 12 | sim_object_visionsensor_type =9 13 | sim_object_volume_type =10 14 | sim_object_mill_type =11 15 | sim_object_forcesensor_type =12 16 | sim_object_light_type =13 17 | sim_object_mirror_type =14 18 | 19 | #General object types. Values are serialized 20 | sim_appobj_object_type =109 21 | sim_appobj_collision_type =110 22 | sim_appobj_distance_type =111 23 | sim_appobj_simulation_type =112 24 | sim_appobj_ik_type =113 25 | sim_appobj_constraintsolver_type=114 26 | sim_appobj_collection_type =115 27 | sim_appobj_ui_type =116 28 | sim_appobj_script_type =117 29 | sim_appobj_pathplanning_type =118 30 | sim_appobj_RESERVED_type =119 31 | sim_appobj_texture_type =120 32 | 33 | # Ik calculation methods. Values are serialized 34 | sim_ik_pseudo_inverse_method =0 35 | sim_ik_damped_least_squares_method =1 36 | sim_ik_jacobian_transpose_method =2 37 | 38 | # Ik constraints. Values are serialized 39 | sim_ik_x_constraint =1 40 | sim_ik_y_constraint =2 41 | sim_ik_z_constraint =4 42 | sim_ik_alpha_beta_constraint=8 43 | sim_ik_gamma_constraint =16 44 | sim_ik_avoidance_constraint =64 45 | 46 | # Ik calculation results 47 | sim_ikresult_not_performed =0 48 | sim_ikresult_success =1 49 | sim_ikresult_fail =2 50 | 51 | # Scene object sub-types. Values are serialized 52 | # Light sub-types 53 | sim_light_omnidirectional_subtype =1 54 | sim_light_spot_subtype =2 55 | sim_light_directional_subtype =3 56 | # Joint sub-types 57 | sim_joint_revolute_subtype =10 58 | sim_joint_prismatic_subtype =11 59 | sim_joint_spherical_subtype =12 60 | # Shape sub-types 61 | sim_shape_simpleshape_subtype =20 62 | sim_shape_multishape_subtype =21 63 | # Proximity sensor sub-types 64 | sim_proximitysensor_pyramid_subtype =30 65 | sim_proximitysensor_cylinder_subtype=31 66 | sim_proximitysensor_disc_subtype =32 67 | sim_proximitysensor_cone_subtype =33 68 | sim_proximitysensor_ray_subtype =34 69 | # Mill sub-types 70 | sim_mill_pyramid_subtype =40 71 | sim_mill_cylinder_subtype =41 72 | sim_mill_disc_subtype =42 73 | sim_mill_cone_subtype =42 74 | # No sub-type 75 | sim_object_no_subtype =200 76 | 77 | 78 | #Scene object main properties (serialized) 79 | sim_objectspecialproperty_collidable =0x0001 80 | sim_objectspecialproperty_measurable =0x0002 81 | #reserved =0x0004 82 | #reserved =0x0008 83 | sim_objectspecialproperty_detectable_ultrasonic =0x0010 84 | sim_objectspecialproperty_detectable_infrared =0x0020 85 | sim_objectspecialproperty_detectable_laser =0x0040 86 | sim_objectspecialproperty_detectable_inductive =0x0080 87 | sim_objectspecialproperty_detectable_capacitive =0x0100 88 | sim_objectspecialproperty_renderable =0x0200 89 | sim_objectspecialproperty_detectable_all =sim_objectspecialproperty_detectable_ultrasonic|sim_objectspecialproperty_detectable_infrared|sim_objectspecialproperty_detectable_laser|sim_objectspecialproperty_detectable_inductive|sim_objectspecialproperty_detectable_capacitive 90 | sim_objectspecialproperty_cuttable =0x0400 91 | sim_objectspecialproperty_pathplanning_ignored =0x0800 92 | 93 | # Model properties (serialized) 94 | sim_modelproperty_not_collidable =0x0001 95 | sim_modelproperty_not_measurable =0x0002 96 | sim_modelproperty_not_renderable =0x0004 97 | sim_modelproperty_not_detectable =0x0008 98 | sim_modelproperty_not_cuttable =0x0010 99 | sim_modelproperty_not_dynamic =0x0020 100 | sim_modelproperty_not_respondable =0x0040 # cannot be selected if sim_modelproperty_not_dynamic is not selected 101 | sim_modelproperty_not_reset =0x0080 # Model is not reset at simulation end. This flag is cleared at simulation end 102 | sim_modelproperty_not_visible =0x0100 # Whole model is invisible independent of local visibility settings 103 | sim_modelproperty_not_model =0xf000 # object is not a model 104 | 105 | 106 | # Check the documentation instead of comments below!! 107 | # Following messages are dispatched to the Lua-message container 108 | sim_message_ui_button_state_change =0 # a UI button slider etc. changed (due to a user's action). aux[0]=UI handle aux[1]=button handle aux[2]=button attributes aux[3]=slider position (if slider) 109 | sim_message_reserved9 =1 # Do not use 110 | sim_message_object_selection_changed=2 111 | sim_message_reserved10 =3 # do not use 112 | sim_message_model_loaded =4 113 | sim_message_reserved11 =5 # do not use 114 | sim_message_keypress =6 # a key was pressed while the focus was on a page (aux[0]=key aux[1]=ctrl and shift key state) 115 | sim_message_bannerclicked =7 # a banner was clicked (aux[0]=banner ID) 116 | 117 | 118 | # Following messages are dispatched only to the C-API (not available from Lua) 119 | sim_message_for_c_api_only_start =0x100 # Do not use 120 | sim_message_reserved1 =0x101 # Do not use 121 | sim_message_reserved2 =0x102 # Do not use 122 | sim_message_reserved3 =0x103 # Do not use 123 | sim_message_eventcallback_scenesave =0x104 # about to save a scene 124 | sim_message_eventcallback_modelsave =0x105 # about to save a model (current selection will be saved) 125 | sim_message_eventcallback_moduleopen =0x106 # called when simOpenModule in Lua is called 126 | sim_message_eventcallback_modulehandle =0x107 # called when simHandleModule in Lua is called with argument false 127 | sim_message_eventcallback_moduleclose =0x108 # called when simCloseModule in Lua is called 128 | sim_message_reserved4 =0x109 # Do not use 129 | sim_message_reserved5 =0x10a # Do not use 130 | sim_message_reserved6 =0x10b # Do not use 131 | sim_message_reserved7 =0x10c # Do not use 132 | sim_message_eventcallback_instancepass =0x10d # Called once every main application loop pass. auxiliaryData[0] contains event flags of events that happened since last time 133 | sim_message_eventcallback_broadcast =0x10e 134 | sim_message_eventcallback_imagefilter_enumreset =0x10f 135 | sim_message_eventcallback_imagefilter_enumerate =0x110 136 | sim_message_eventcallback_imagefilter_adjustparams =0x111 137 | sim_message_eventcallback_imagefilter_reserved =0x112 138 | sim_message_eventcallback_imagefilter_process =0x113 139 | sim_message_eventcallback_reserved1 =0x114 # do not use 140 | sim_message_eventcallback_reserved2 =0x115 # do not use 141 | sim_message_eventcallback_reserved3 =0x116 # do not use 142 | sim_message_eventcallback_reserved4 =0x117 # do not use 143 | sim_message_eventcallback_abouttoundo =0x118 # the undo button was hit and a previous state is about to be restored 144 | sim_message_eventcallback_undoperformed =0x119 # the undo button was hit and a previous state restored 145 | sim_message_eventcallback_abouttoredo =0x11a # the redo button was hit and a future state is about to be restored 146 | sim_message_eventcallback_redoperformed =0x11b # the redo button was hit and a future state restored 147 | sim_message_eventcallback_scripticondblclick =0x11c # scipt icon was double clicked. (aux[0]=object handle associated with script set replyData[0] to 1 if script should not be opened) 148 | sim_message_eventcallback_simulationabouttostart =0x11d 149 | sim_message_eventcallback_simulationended =0x11e 150 | sim_message_eventcallback_reserved5 =0x11f # do not use 151 | sim_message_eventcallback_keypress =0x120 # a key was pressed while the focus was on a page (aux[0]=key aux[1]=ctrl and shift key state) 152 | sim_message_eventcallback_modulehandleinsensingpart =0x121 # called when simHandleModule in Lua is called with argument true 153 | sim_message_eventcallback_renderingpass =0x122 # called just before the scene is rendered 154 | sim_message_eventcallback_bannerclicked =0x123 # called when a banner was clicked (aux[0]=banner ID) 155 | sim_message_eventcallback_menuitemselected =0x124 # auxiliaryData[0] indicates the handle of the item auxiliaryData[1] indicates the state of the item 156 | sim_message_eventcallback_refreshdialogs =0x125 # aux[0]=refresh degree (0=light 1=medium 2=full) 157 | sim_message_eventcallback_sceneloaded =0x126 158 | sim_message_eventcallback_modelloaded =0x127 159 | sim_message_eventcallback_instanceswitch =0x128 160 | sim_message_eventcallback_guipass =0x129 161 | sim_message_eventcallback_mainscriptabouttobecalled =0x12a 162 | sim_message_eventcallback_rmlposition =0x12b #the command simRMLPosition was called. The appropriate plugin should handle the call 163 | sim_message_eventcallback_rmlvelocity =0x12c # the command simRMLVelocity was called. The appropriate plugin should handle the call 164 | sim_message_simulation_start_resume_request =0x1000 165 | sim_message_simulation_pause_request =0x1001 166 | sim_message_simulation_stop_request =0x1002 167 | 168 | # Scene object properties. Combine with the | operator 169 | sim_objectproperty_reserved1 =0x0000 170 | sim_objectproperty_reserved2 =0x0001 171 | sim_objectproperty_reserved3 =0x0002 172 | sim_objectproperty_reserved4 =0x0003 173 | sim_objectproperty_reserved5 =0x0004 # formely sim_objectproperty_visible 174 | sim_objectproperty_reserved6 =0x0008 # formely sim_objectproperty_wireframe 175 | sim_objectproperty_collapsed =0x0010 176 | sim_objectproperty_selectable =0x0020 177 | sim_objectproperty_reserved7 =0x0040 178 | sim_objectproperty_selectmodelbaseinstead =0x0080 179 | sim_objectproperty_dontshowasinsidemodel =0x0100 180 | # reserved =0x0200 181 | sim_objectproperty_canupdatedna =0x0400 182 | sim_objectproperty_selectinvisible =0x0800 183 | sim_objectproperty_depthinvisible =0x1000 184 | 185 | 186 | # type of arguments (input and output) for custom lua commands 187 | sim_lua_arg_nil =0 188 | sim_lua_arg_bool =1 189 | sim_lua_arg_int =2 190 | sim_lua_arg_float =3 191 | sim_lua_arg_string =4 192 | sim_lua_arg_invalid =5 193 | sim_lua_arg_table =8 194 | 195 | # custom user interface properties. Values are serialized. 196 | sim_ui_property_visible =0x0001 197 | sim_ui_property_visibleduringsimulationonly =0x0002 198 | sim_ui_property_moveable =0x0004 199 | sim_ui_property_relativetoleftborder =0x0008 200 | sim_ui_property_relativetotopborder =0x0010 201 | sim_ui_property_fixedwidthfont =0x0020 202 | sim_ui_property_systemblock =0x0040 203 | sim_ui_property_settocenter =0x0080 204 | sim_ui_property_rolledup =0x0100 205 | sim_ui_property_selectassociatedobject =0x0200 206 | sim_ui_property_visiblewhenobjectselected =0x0400 207 | 208 | 209 | # button properties. Values are serialized. 210 | sim_buttonproperty_button =0x0000 211 | sim_buttonproperty_label =0x0001 212 | sim_buttonproperty_slider =0x0002 213 | sim_buttonproperty_editbox =0x0003 214 | sim_buttonproperty_staydown =0x0008 215 | sim_buttonproperty_enabled =0x0010 216 | sim_buttonproperty_borderless =0x0020 217 | sim_buttonproperty_horizontallycentered =0x0040 218 | sim_buttonproperty_ignoremouse =0x0080 219 | sim_buttonproperty_isdown =0x0100 220 | sim_buttonproperty_transparent =0x0200 221 | sim_buttonproperty_nobackgroundcolor =0x0400 222 | sim_buttonproperty_rollupaction =0x0800 223 | sim_buttonproperty_closeaction =0x1000 224 | sim_buttonproperty_verticallycentered =0x2000 225 | sim_buttonproperty_downupevent =0x4000 226 | 227 | 228 | # Simulation status 229 | sim_simulation_stopped =0x00 # Simulation is stopped 230 | sim_simulation_paused =0x08 # Simulation is paused 231 | sim_simulation_advancing =0x10 # Simulation is advancing 232 | sim_simulation_advancing_firstafterstop =sim_simulation_advancing|0x00 # First simulation pass (1x) 233 | sim_simulation_advancing_running =sim_simulation_advancing|0x01 # Normal simulation pass (>=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 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/simpleSynchronousTest.py: -------------------------------------------------------------------------------- 1 | # This small example illustrates how to use the remote API 2 | # synchronous mode. The synchronous mode needs to be 3 | # pre-enabled on the server side. You would do this by 4 | # starting the server (e.g. in a child script) with: 5 | # 6 | # simRemoteApi.start(19999,1300,false,true) 7 | # 8 | # But in this example we try to connect on port 9 | # 19997 where there should be a continuous remote API 10 | # server service already running and pre-enabled for 11 | # synchronous mode. 12 | # 13 | # 14 | # IMPORTANT: for each successful call to simxStart, there 15 | # should be a corresponding call to simxFinish at the end! 16 | 17 | try: 18 | import sim 19 | except: 20 | print ('--------------------------------------------------------------') 21 | print ('"sim.py" could not be imported. This means very probably that') 22 | print ('either "sim.py" or the remoteApi library could not be found.') 23 | print ('Make sure both are in the same folder as this file,') 24 | print ('or appropriately adjust the file "sim.py"') 25 | print ('--------------------------------------------------------------') 26 | print ('') 27 | 28 | import time 29 | import sys 30 | 31 | print ('Program started') 32 | sim.simxFinish(-1) # just in case, close all opened connections 33 | clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim 34 | if clientID!=-1: 35 | print ('Connected to remote API server') 36 | 37 | # enable the synchronous mode on the client: 38 | sim.simxSynchronous(clientID,True) 39 | 40 | # start the simulation: 41 | sim.simxStartSimulation(clientID,sim.simx_opmode_blocking) 42 | 43 | # Now step a few times: 44 | for i in range(1,10): 45 | if sys.version_info[0] == 3: 46 | input('Press key to step the simulation!') 47 | else: 48 | raw_input('Press key to step the simulation!') 49 | sim.simxSynchronousTrigger(clientID); 50 | 51 | # stop the simulation: 52 | sim.simxStopSimulation(clientID,sim.simx_opmode_blocking) 53 | 54 | # Now close the connection to CoppeliaSim: 55 | sim.simxFinish(clientID) 56 | else: 57 | print ('Failed connecting to remote API server') 58 | print ('Program ended') 59 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/simpleTest.py: -------------------------------------------------------------------------------- 1 | # Make sure to have the server side running in CoppeliaSim: 2 | # in a child script of a CoppeliaSim scene, add following command 3 | # to be executed just once, at simulation start: 4 | # 5 | # simRemoteApi.start(19999) 6 | # 7 | # then start simulation, and run this program. 8 | # 9 | # IMPORTANT: for each successful call to simxStart, there 10 | # should be a corresponding call to simxFinish at the end! 11 | 12 | try: 13 | import sim 14 | except: 15 | print ('--------------------------------------------------------------') 16 | print ('"sim.py" could not be imported. This means very probably that') 17 | print ('either "sim.py" or the remoteApi library could not be found.') 18 | print ('Make sure both are in the same folder as this file,') 19 | print ('or appropriately adjust the file "sim.py"') 20 | print ('--------------------------------------------------------------') 21 | print ('') 22 | 23 | import time 24 | 25 | print ('Program started') 26 | sim.simxFinish(-1) # just in case, close all opened connections 27 | clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim 28 | if clientID!=-1: 29 | print ('Connected to remote API server') 30 | 31 | # Now try to retrieve data in a blocking fashion (i.e. a service call): 32 | res,objs=sim.simxGetObjects(clientID,sim.sim_handle_all,sim.simx_opmode_blocking) 33 | if res==sim.simx_return_ok: 34 | print ('Number of objects in the scene: ',len(objs)) 35 | else: 36 | print ('Remote API function call returned with error code: ',res) 37 | 38 | time.sleep(2) 39 | 40 | # Now retrieve streaming data (i.e. in a non-blocking fashion): 41 | startTime=time.time() 42 | sim.simxGetIntegerParameter(clientID,sim.sim_intparam_mouse_x,sim.simx_opmode_streaming) # Initialize streaming 43 | while time.time()-startTime < 5: 44 | returnCode,data=sim.simxGetIntegerParameter(clientID,sim.sim_intparam_mouse_x,sim.simx_opmode_buffer) # Try to retrieve the streamed data 45 | if returnCode==sim.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code 46 | print ('Mouse position x: ',data) # Mouse position x is actualized when the cursor is over CoppeliaSim's window 47 | time.sleep(0.005) 48 | 49 | # Now send some data to CoppeliaSim in a non-blocking fashion: 50 | sim.simxAddStatusbarMessage(clientID,'Hello CoppeliaSim!',sim.simx_opmode_oneshot) 51 | 52 | # 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): 53 | sim.simxGetPingTime(clientID) 54 | 55 | # Now close the connection to CoppeliaSim: 56 | sim.simxFinish(clientID) 57 | else: 58 | print ('Failed connecting to remote API server') 59 | print ('Program ended') 60 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/synchronousImageTransmission.py: -------------------------------------------------------------------------------- 1 | # Make sure to have CoppeliaSim running, with followig scene loaded: 2 | # 3 | # scenes/synchronousImageTransmissionViaRemoteApi.ttt 4 | # 5 | # Do not launch simulation, but run this script 6 | # 7 | # The client side (i.e. this script) depends on: 8 | # 9 | # sim.py, simConst.py, and the remote API library available 10 | # in programming/remoteApiBindings/lib/lib 11 | 12 | try: 13 | import sim 14 | except: 15 | print ('--------------------------------------------------------------') 16 | print ('"sim.py" could not be imported. This means very probably that') 17 | print ('either "sim.py" or the remoteApi library could not be found.') 18 | print ('Make sure both are in the same folder as this file,') 19 | print ('or appropriately adjust the file "sim.py"') 20 | print ('--------------------------------------------------------------') 21 | print ('') 22 | 23 | import math 24 | import time 25 | 26 | class Client: 27 | def __enter__(self): 28 | self.intSignalName='legacyRemoteApiStepCounter' 29 | self.stepCounter=0 30 | self.lastImageAcquisitionTime=-1 31 | sim.simxFinish(-1) # just in case, close all opened connections 32 | self.id=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim 33 | return self 34 | 35 | def __exit__(self,*err): 36 | sim.simxFinish(-1) 37 | 38 | with Client() as client: 39 | client.runInSynchronousMode=True 40 | 41 | print("running") 42 | 43 | if client.id!=-1: 44 | print ('Connected to remote API server') 45 | 46 | def stepSimulation(): 47 | if client.runInSynchronousMode: 48 | currentStep=client.stepCounter 49 | sim.simxSynchronousTrigger(client.id); 50 | while client.stepCounter==currentStep: 51 | retCode,s=sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_buffer) 52 | if retCode==sim.simx_return_ok: 53 | client.stepCounter=s 54 | retCode,res,img=sim.simxGetVisionSensorImage(client.id,client.visionSensorHandle,0,sim.simx_opmode_buffer) 55 | client.lastImageAcquisitionTime=sim.simxGetLastCmdTime(client.id) 56 | if retCode==sim.simx_return_ok: 57 | sim.simxSetVisionSensorImage(client.id,client.passiveVisionSensorHandle,img,0,sim.simx_opmode_oneshot) 58 | else: 59 | retCode,res,img=sim.simxGetVisionSensorImage(client.id,client.visionSensorHandle,0,sim.simx_opmode_buffer) 60 | if retCode==sim.simx_return_ok: 61 | imageSimTime=sim.simxGetLastCmdTime(client.id) 62 | if client.lastImageAcquisitionTime!=imageSimTime: 63 | client.lastImageAcquisitionTime=imageSimTime 64 | sim.simxSetVisionSensorImage(client.id,client.passiveVisionSensorHandle,img,0,sim.simx_opmode_oneshot) 65 | 66 | # Start streaming client.intSignalName integer signal, that signals when a step is finished: 67 | sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_streaming) 68 | 69 | res,client.visionSensorHandle=sim.simxGetObjectHandle(client.id,'VisionSensor',sim.simx_opmode_blocking) 70 | res,client.passiveVisionSensorHandle=sim.simxGetObjectHandle(client.id,'PassiveVisionSensor',sim.simx_opmode_blocking) 71 | 72 | # Start streaming the vision sensor image: 73 | sim.simxGetVisionSensorImage(client.id,client.visionSensorHandle,0,sim.simx_opmode_streaming) 74 | 75 | # enable the synchronous mode on the client: 76 | if client.runInSynchronousMode: 77 | sim.simxSynchronous(client.id,True) 78 | 79 | sim.simxStartSimulation(client.id,sim.simx_opmode_oneshot) 80 | 81 | startTime=time.time() 82 | while time.time()-startTime < 5: 83 | stepSimulation() 84 | 85 | # stop data streaming 86 | sim.simxGetIntegerSignal(client.id,client.intSignalName,sim.simx_opmode_discontinue) 87 | sim.simxGetVisionSensorImage(client.id,client.visionSensorHandle,0,sim.simx_opmode_discontinue) 88 | 89 | sim.simxStopSimulation(client.id,sim.simx_opmode_blocking) 90 | -------------------------------------------------------------------------------- /VREP_RemoteAPIs/visualization.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """Code for visualizing data in sim via python. 3 | 4 | Author: Andrew Hundt 5 | 6 | License: Apache v2 https://www.apache.org/licenses/LICENSE-2.0 7 | """ 8 | import os 9 | import errno 10 | import traceback 11 | 12 | import numpy as np 13 | import six # compatibility between python 2 + 3 = six 14 | import matplotlib.pyplot as plt 15 | 16 | try: 17 | import sim as sim 18 | except Exception as e: 19 | print ('--------------------------------------------------------------') 20 | print ('"sim.py" could not be imported. This means very probably that') 21 | print ('either "sim.py" or the remoteApi library could not be found.') 22 | print ('Make sure both are in PYTHONPATH folder relative to this file,') 23 | print ('or appropriately adjust the file "sim.py. Also follow the"') 24 | print ('ReadMe.txt in the sim remote API folder') 25 | print ('--------------------------------------------------------------') 26 | print ('') 27 | raise e 28 | 29 | import tensorflow as tf 30 | 31 | from tensorflow.python.platform import flags 32 | from tensorflow.python.platform import gfile 33 | from tensorflow.python.ops import data_flow_ops 34 | from ply import write_xyz_rgb_as_ply 35 | from PIL import Image 36 | 37 | # progress bars https://github.com/tqdm/tqdm 38 | # import tqdm without enforcing it as a dependency 39 | try: 40 | from tqdm import tqdm 41 | except ImportError: 42 | 43 | def tqdm(*args, **kwargs): 44 | if args: 45 | return args[0] 46 | return kwargs.get('iterable', None) 47 | 48 | from depth_image_encoding import ClipFloatValues 49 | from depth_image_encoding import FloatArrayToRgbImage 50 | from depth_image_encoding import FloatArrayToRawRGB 51 | from skimage.transform import resize 52 | from skimage import img_as_ubyte 53 | from skimage import img_as_uint 54 | from skimage.color import grey2rgb 55 | 56 | try: 57 | import eigen # https://github.com/jrl-umi3218/Eigen3ToPython 58 | import sva # https://github.com/jrl-umi3218/SpaceVecAlg 59 | except ImportError: 60 | print('eigen and sva python modules are not available. To install run the script at:' 61 | 'https://github.com/ahundt/robotics_setup/blob/master/robotics_tasks.sh' 62 | 'or follow the instructions at https://github.com/jrl-umi3218/Eigen3ToPython' 63 | 'and https://github.com/jrl-umi3218/SpaceVecAlg. ' 64 | 'When you build the modules make sure python bindings are enabled.') 65 | 66 | tf.flags.DEFINE_string('csimVisualizeDepthFormat', 'csim_depth_encoded_rgb', 67 | """ Controls how Depth images are displayed. Options are: 68 | None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth). 69 | 'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m 70 | 'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by 71 | the google brain robot data grasp dataset's raw png depth image encoding, 72 | see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding. 73 | 'sim': add a sim prefix to any of the above commands to 74 | rotate image by 180 degrees, flip left over right, then invert the color channels 75 | after the initial conversion step. 76 | This is due to a problem where CoppeliaSim seems to display images differently. 77 | Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb', 78 | see http://forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805. 79 | """) 80 | tf.flags.DEFINE_string('csimVisualizeRGBFormat', 'csim_rgb', 81 | """ Controls how images are displayed. Options are: 82 | None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth). 83 | 'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m 84 | 'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by 85 | the google brain robot data grasp dataset's raw png depth image encoding, 86 | see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding. 87 | 'sim': add a sim prefix to any of the above commands to 88 | rotate image by 180 degrees, flip left over right, then invert the color channels 89 | after the initial conversion step. 90 | This is due to a problem where CoppeliaSim seems to display images differently. 91 | Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb', 92 | see http://forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805. 93 | """) 94 | 95 | # the following line is needed for tf versions before 1.5 96 | # flags.FLAGS._parse_flags() 97 | FLAGS = flags.FLAGS 98 | 99 | 100 | def depth_image_to_point_cloud(depth, intrinsics_matrix, dtype=np.float32, verbose=0): 101 | """Depth images become an XYZ point cloud in the camera frame with shape (depth.shape[0], depth.shape[1], 3). 102 | 103 | Transform a depth image into a point cloud in the camera frame with one point for each 104 | pixel in the image, using the camera transform for a camera 105 | centred at cx, cy with field of view fx, fy. 106 | 107 | Based on: 108 | https://github.com/tensorflow/models/blob/master/research/cognitive_mapping_and_planning/src/depth_utils.py 109 | https://codereview.stackexchange.com/a/84990/10101 110 | 111 | also see grasp_geometry_tf.depth_image_to_point_cloud(). 112 | 113 | # Arguments 114 | 115 | depth: is a 2-D ndarray with shape (rows, cols) containing 116 | 32bit floating point depths in meters. The result is a 3-D array with 117 | shape (rows, cols, 3). Pixels with invalid depth in the input have 118 | NaN or 0 for the z-coordinate in the result. 119 | 120 | intrinsics_matrix: 3x3 matrix for projecting depth values to z values 121 | in the point cloud frame. http://ksimek.github.io/2013/08/13/intrinsic/ 122 | In this case x0, y0 are at index [2, 0] and [2, 1], respectively. 123 | 124 | transform: 4x4 Rt matrix for rotating and translating the point cloud 125 | """ 126 | fy = intrinsics_matrix[1, 1] 127 | fx = intrinsics_matrix[0, 0] 128 | # center of image y coordinate 129 | center_y = intrinsics_matrix[2, 1] 130 | # center of image x coordinate 131 | center_x = intrinsics_matrix[2, 0] 132 | depth = np.squeeze(depth) 133 | y_range, x_range = depth.shape 134 | 135 | y, x = np.meshgrid(np.arange(y_range), 136 | np.arange(x_range), 137 | indexing='ij') 138 | assert y.size == x.size and y.size == depth.size 139 | x = x.flatten() 140 | y = y.flatten() 141 | depth = depth.flatten() 142 | 143 | X = (x - center_x) * depth / fx 144 | Y = (y - center_y) * depth / fy 145 | 146 | assert X.size == Y.size and X.size == depth.size 147 | assert X.shape == Y.shape and X.shape == depth.shape 148 | 149 | if verbose > 0: 150 | print('X np: ', X.shape) 151 | print('Y np: ', Y.shape) 152 | print('depth np: ', depth.shape) 153 | XYZ = np.column_stack([X, Y, depth]) 154 | assert XYZ.shape == (y_range * x_range, 3) 155 | if verbose > 0: 156 | print('XYZ pre reshape np: ', XYZ.shape) 157 | XYZ = XYZ.reshape((y_range, x_range, 3)) 158 | 159 | return XYZ.astype(dtype) 160 | 161 | 162 | def csimPrint(client_id, message): 163 | """Print a message in both the python command line and on the CoppeliaSim Statusbar. 164 | 165 | The Statusbar is the white command line output on the bottom of the CoppeliaSim GUI window. 166 | """ 167 | sim.simxAddStatusbarMessage(client_id, message, sim.simx_opmode_oneshot) 168 | print(message) 169 | 170 | 171 | def create_dummy(client_id, display_name, transform=None, parent_handle=-1, debug=FLAGS.csimDebugMode, operation_mode=sim.simx_opmode_blocking): 172 | """Create a dummy object in the simulation 173 | 174 | # Arguments 175 | 176 | transform_display_name: name string to use for the object in the sim scene 177 | transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim 178 | parent_handle: -1 is the world frame, any other int should be a sim object handle 179 | """ 180 | if transform is None: 181 | transform = np.array([0., 0., 0., 0., 0., 0., 1.]) 182 | # 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName': 183 | empty_buffer = bytearray() 184 | res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( 185 | client_id, 186 | 'remoteApiCommandServer', 187 | sim.sim_scripttype_childscript, 188 | 'createDummy_function', 189 | [parent_handle], 190 | transform, 191 | [display_name], 192 | empty_buffer, 193 | operation_mode) 194 | if res == sim.simx_return_ok: 195 | # display the reply from CoppeliaSim (in this case, the handle of the created dummy) 196 | if debug is not None and 'print_transform' in debug: 197 | print ('Dummy name:', display_name, ' handle: ', ret_ints[0], ' transform: ', transform) 198 | else: 199 | print('create_dummy remote function call failed.') 200 | print(''.join(traceback.format_stack())) 201 | return -1 202 | return ret_ints[0] 203 | 204 | 205 | def setPose(client_id, display_name, transform=None, parent_handle=-1): 206 | """Set the pose of an object in the simulation 207 | 208 | # Arguments 209 | 210 | transform_display_name: name string to use for the object in the sim scene 211 | transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim 212 | parent_handle: -1 is the world frame, any other int should be a sim object handle 213 | """ 214 | if transform is None: 215 | transform = np.array([0., 0., 0., 0., 0., 0., 1.]) 216 | # 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName': 217 | empty_buffer = bytearray() 218 | res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( 219 | client_id, 220 | 'remoteApiCommandServer', 221 | sim.sim_scripttype_childscript, 222 | 'createDummy_function', 223 | [parent_handle], 224 | transform, 225 | [display_name], 226 | empty_buffer, 227 | sim.simx_opmode_blocking) 228 | if res == sim.simx_return_ok: 229 | # display the reply from CoppeliaSim (in this case, the handle of the created dummy) 230 | print ('SetPose object name:', display_name, ' handle: ', ret_ints[0], ' transform: ', transform) 231 | else: 232 | print('setPose remote function call failed.') 233 | print(''.join(traceback.format_stack())) 234 | return -1 235 | return ret_ints[0] 236 | 237 | 238 | def set_vision_sensor_image(client_id, display_name, image, convert=None, scale_factor=256000.0, operation_mode=sim.simx_opmode_oneshot_wait): 239 | """Display vision sensor image data in a CoppeliaSim simulation. 240 | 241 | [CoppeliaSim Vision Sensors](http://www.coppeliarobotics.com/helpFiles/en/visionSensors.htm) 242 | [simSetVisionSensorImage](http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simSetVisionSensorImage) 243 | 244 | # Arguments 245 | 246 | display_name: the string display name of the sensor object in the CoppeliaSim scene 247 | image: an rgb char array containing an image 248 | convert: Controls how images are displayed. Options are: 249 | None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth). 250 | 'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m 251 | 'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by 252 | the google brain robot data grasp dataset's raw png depth image encoding, 253 | see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding. 254 | 'sim': add a sim prefix to any of the above commands to 255 | rotate image by 180 degrees, flip left over right, then invert the color channels 256 | after the initial conversion step. 257 | This is due to a problem where CoppeliaSim seems to display images differently. 258 | Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb', 259 | see http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805. 260 | """ 261 | strings = [display_name] 262 | parent_handle = -1 263 | 264 | # TODO(ahundt) support is_greyscale True again 265 | is_greyscale = 0 266 | csim_conversion = False 267 | if convert is not None: 268 | csim_conversion = 'sim' in convert 269 | 270 | if 'depth_encoded_rgb' in convert: 271 | image = np.array(FloatArrayToRgbImage(image, scale_factor=scale_factor, drop_blue=False), dtype=np.uint8) 272 | elif 'depth_rgb' in convert: 273 | 274 | image = img_as_uint(image) 275 | 276 | elif not csim_conversion: 277 | raise ValueError('set_vision_sensor_image() convert parameter must be one of `depth_encoded_rgb`, `depth_rgb`, or None' 278 | 'with the optional addition of the word `sim` to rotate 180, flip left right, then invert colors.') 279 | 280 | if csim_conversion: 281 | # rotate 180 degrees, flip left over right, then invert the colors 282 | image = np.array(256 - np.fliplr(np.rot90(image, 2)), dtype=np.uint8) 283 | 284 | if np.issubdtype(image.dtype, np.integer): 285 | is_float = 0 286 | floats = [] 287 | color_buffer = bytearray(image.flatten().tobytes()) 288 | color_size = image.size 289 | num_floats = 0 290 | else: 291 | is_float = 1 292 | floats = [image] 293 | color_buffer = bytearray() 294 | num_floats = image.size 295 | color_size = 0 296 | 297 | cloud_handle = -1 298 | res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( 299 | client_id, 300 | 'remoteApiCommandServer', 301 | sim.sim_scripttype_childscript, 302 | 'setVisionSensorImage_function', 303 | [parent_handle, num_floats, is_greyscale, color_size], # int params 304 | np.append(floats, []), # float params 305 | strings, # string params 306 | # byte buffer params 307 | color_buffer, 308 | operation_mode) 309 | if res == sim.simx_return_ok: 310 | print ('point cloud handle: ', ret_ints[0]) # display the reply from CoppeliaSim (in this case, the handle of the created dummy) 311 | # set the transform for the point cloud 312 | return ret_ints[0] 313 | else: 314 | print('insertPointCloud_function remote function call failed.') 315 | print(''.join(traceback.format_stack())) 316 | return res 317 | 318 | 319 | def create_point_cloud(client_id, display_name, transform=None, point_cloud=None, depth_image=None, color_image=None, 320 | camera_intrinsics_matrix=None, parent_handle=-1, clear=True, 321 | max_voxel_size=0.01, max_point_count_per_voxel=10, point_size=10, options=8, 322 | rgb_sensor_display_name=None, depth_sensor_display_name=None, convert_depth=FLAGS.csimVisualizeDepthFormat, 323 | convert_rgb=FLAGS.csimVisualizeRGBFormat, save_ply_path=None, rgb_display_mode='vision_sensor'): 324 | """Create a point cloud object in the simulation, plus optionally render the depth and rgb images. 325 | 326 | # Arguments 327 | 328 | display_name: name string to use for the object in the sim scene 329 | depth_image: A depth image of size [width, height, 3] 330 | transform: [x, y, z, qw, qx, qy, qz] with 3 cartesian (x, y, z) and 4 quaternion (qx, qy, qz, qw) elements, same as sim 331 | This transform is from the parent handle to the point cloud base 332 | parent_handle: -1 is the world frame, any other int should be a sim object handle 333 | clear: clear the point cloud if it already exists with the provided display name 334 | maxVoxelSize: the maximum size of the octree voxels containing points 335 | maxPtCntPerVoxel: the maximum number of points allowed in a same octree voxel 336 | options: bit-coded: 337 | bit0 set (1): points have random colors 338 | bit1 set (2): show octree structure 339 | bit2 set (4): reserved. keep unset 340 | bit3 set (8): do not use an octree structure. When enabled, point cloud operations are limited, and point clouds will not be collidable, measurable or detectable anymore, but adding points will be much faster 341 | bit4 set (16): color is emissive 342 | pointSize: the size of the points, in pixels 343 | reserved: reserved for future extensions. Set to NULL 344 | save_ply_path: save out a ply file with the point cloud data 345 | point_cloud: optional XYZ point cloud of size [width, height, 3], will be generated if not provided. 346 | convert_rgb: Controls how images are displayed. Options are: 347 | None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth). 348 | 'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m 349 | 'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by 350 | the google brain robot data grasp dataset's raw png depth image encoding, 351 | see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding. 352 | 'sim': add a sim prefix to any of the above commands to 353 | rotate image by 180 degrees, flip left over right, then invert the color channels 354 | after the initial conversion step. 355 | This is due to a problem where CoppeliaSim seems to display images differently. 356 | Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb', 357 | see http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805. 358 | rgb_display_mode: Options help with working around quirks in input image data's layout. 359 | 'point_cloud' to display the image when the point cloud is being generated. 360 | 'vision_sensor' to make a separate call go the vison sensor display function. 361 | """ 362 | if transform is None: 363 | transform = np.array([0., 0., 0., 0., 0., 0., 1.]) 364 | 365 | if point_cloud is None: 366 | point_cloud = depth_image_to_point_cloud(depth_image, camera_intrinsics_matrix) 367 | point_cloud = point_cloud.reshape([point_cloud.size/3, 3]) 368 | 369 | # show the depth sensor image 370 | if depth_sensor_display_name is not None and depth_image is not None: 371 | # matplotlib.image.imsave(display_name + depth_sensor_display_name + '_norotfliplr.png', depth_image) 372 | # rotate 180, flip left over right then invert the image colors for display in CoppeliaSim 373 | # depth_image = np.fliplr(np.rot90(depth_image, 2)) 374 | # matplotlib.image.imsave(display_name + depth_sensor_display_name + '_rot90fliplr.png', depth_image) 375 | set_vision_sensor_image(client_id, depth_sensor_display_name, depth_image, convert=convert_depth) 376 | 377 | # show the rgb sensor image, this overwrites the rgb display 378 | # done in insertPointCloud_function, which is buggy 379 | if rgb_sensor_display_name is not None and color_image is not None and rgb_display_mode == 'vision_sensor': 380 | # matplotlib.image.imsave(display_name + rgb_sensor_display_name + '_norotfliplr.png', color_image) 381 | # rotate 180, flip left over right then invert the image colors for display in CoppeliaSim 382 | # matplotlib.image.imsave(display_name + rgb_sensor_display_name + '_rot90fliplr.png', color_image) 383 | set_vision_sensor_image(client_id, rgb_sensor_display_name, color_image, convert=convert_rgb) 384 | 385 | # Save out Point cloud 386 | if save_ply_path is not None: 387 | write_xyz_rgb_as_ply(point_cloud, color_image, save_ply_path) 388 | 389 | # color_buffer is initially empty 390 | color_buffer = bytearray() 391 | strings = [display_name] 392 | if rgb_sensor_display_name is not None and rgb_display_mode == 'point_cloud': 393 | strings = [display_name, rgb_sensor_display_name] 394 | 395 | transform_entries = 7 396 | if clear: 397 | clear = 1 398 | else: 399 | clear = 0 400 | 401 | cloud_handle = -1 402 | # Create the point cloud if it does not exist, or retrieve the handle if it does 403 | res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( 404 | client_id, 405 | 'remoteApiCommandServer', 406 | sim.sim_scripttype_childscript, 407 | 'createPointCloud_function', 408 | # int params 409 | [parent_handle, transform_entries, point_cloud.size, cloud_handle, clear, max_point_count_per_voxel, options, point_size], 410 | # float params 411 | [max_voxel_size], 412 | # string params 413 | strings, 414 | # byte buffer params 415 | color_buffer, 416 | sim.simx_opmode_blocking) 417 | 418 | setPose(client_id, display_name, transform, parent_handle) 419 | 420 | if res == sim.simx_return_ok: 421 | cloud_handle = ret_ints[0] 422 | 423 | # convert the rgb values to a string 424 | color_size = 0 425 | if color_image is not None: 426 | # see simInsertPointsIntoPointCloud() in sim documentation 427 | # 3 indicates the cloud should be in the parent frame, and color is enabled 428 | # bit 2 is 1 so each point is colored 429 | simInsertPointsIntoPointCloudOptions = 3 430 | # color_buffer = bytearray(np.fliplr(np.rot90(color_image, 3)).flatten().tobytes()) 431 | color_buffer = bytearray(color_image.flatten().tobytes()) 432 | color_size = color_image.size 433 | else: 434 | simInsertPointsIntoPointCloudOptions = 1 435 | 436 | # Actually transfer the point cloud 437 | res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( 438 | client_id, 439 | 'remoteApiCommandServer', 440 | sim.sim_scripttype_childscript, 441 | 'insertPointCloud_function', 442 | [parent_handle, transform_entries, point_cloud.size, cloud_handle, color_size, simInsertPointsIntoPointCloudOptions], 443 | np.append(point_cloud, []), 444 | strings, 445 | color_buffer, 446 | sim.simx_opmode_blocking) 447 | 448 | if res == sim.simx_return_ok: 449 | print ('point cloud handle: ', ret_ints[0]) # display the reply from CoppeliaSim (in this case, the handle of the created dummy) 450 | # set the transform for the point cloud 451 | return ret_ints[0] 452 | else: 453 | print('insertPointCloud_function remote function call failed.') 454 | print(''.join(traceback.format_stack())) 455 | return res 456 | 457 | else: 458 | print('createPointCloud_function remote function call failed') 459 | print(''.join(traceback.format_stack())) 460 | return res 461 | 462 | 463 | def drawLines(client_id, display_name, lines, parent_handle=-1, transform=None, debug=FLAGS.csimDebugMode, operation_mode=sim.simx_opmode_blocking): 464 | """Create a line in the simulation. 465 | 466 | Note that there are currently some quirks with this function. Only one line is accepted, 467 | and sometimes CoppeliaSim fails to delete the object correctly and lines will fail to draw. 468 | In that case you need to close and restart CoppeliaSim. 469 | 470 | # Arguments 471 | 472 | transform_display_name: name string to use for the object in the sim scene 473 | transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim 474 | parent_handle: -1 is the world frame, any other int should be a sim object handle 475 | lines: array of line definitions using two endpoints (x0, y0, z0, x1, y1, z1). 476 | Multiple lines can be defined but there should be 6 entries (two points) per line. 477 | """ 478 | # 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName': 479 | empty_buffer = bytearray() 480 | res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction( 481 | client_id, 482 | 'remoteApiCommandServer', 483 | sim.sim_scripttype_childscript, 484 | 'addDrawingObject_function', 485 | [parent_handle, int(lines.size/6)], 486 | # np.append(transform, lines), 487 | lines, 488 | [display_name], 489 | empty_buffer, 490 | operation_mode) 491 | if res == sim.simx_return_ok: 492 | # display the reply from CoppeliaSim (in this case, the handle of the created dummy) 493 | if debug is not None and 'print_drawLines' in debug: 494 | print ('drawLines name:', display_name, ' handle: ', ret_ints[0], ' transform: ', transform) 495 | 496 | if transform is not None: 497 | # set the transform for the point cloud 498 | setPose(client_id, display_name, transform, parent_handle) 499 | else: 500 | print('drawLines remote function call failed.') 501 | print(''.join(traceback.format_stack())) 502 | return -1 503 | return ret_ints[0] 504 | 505 | 506 | def restore_cropped(cropped_image, crop_size, crop_offset, full_size): 507 | """ Restore cropped_image to full size image with zero padding 508 | First scale image back to crop_size, then padding 509 | """ 510 | 511 | cropped_image = np.squeeze(cropped_image) 512 | restored = np.zeros((full_size[0], full_size[1]), dtype=cropped_image.dtype) 513 | scaled_crop = resize(cropped_image, (crop_size[0], crop_size[1])) 514 | restored[crop_offset[0]:crop_offset[0]+crop_size[0], 515 | crop_offset[1]:crop_offset[1]+crop_size[1]] = scaled_crop 516 | 517 | return restored 518 | -------------------------------------------------------------------------------- /examples/demo_cart_pole_learning.py: -------------------------------------------------------------------------------- 1 | from stable_baselines3 import A2C 2 | from stable_baselines3.common.env_checker import check_env 3 | 4 | from stable_baselines3.common.monitor import Monitor 5 | from stable_baselines3.common.callbacks import CallbackList 6 | from stable_baselines3.common.callbacks import EvalCallback 7 | 8 | import sys 9 | sys.path.append("../utils") 10 | from callbackFunctions import VisdomCallback 11 | 12 | sys.path.append("../CartPole") 13 | from CartPoleEnv import CartPoleEnv 14 | 15 | import os 16 | 17 | 18 | # ---------------- Create environment 19 | env = CartPoleEnv(action_type='discrete') # action_type can be set as discrete or continuous 20 | check_env(env) 21 | 22 | # ---------------- Callback functions 23 | log_dir = "../CartPole/saved_models/tmp" 24 | os.makedirs(log_dir, exist_ok=True) 25 | 26 | env = Monitor(env, log_dir) 27 | 28 | callback_visdom = VisdomCallback(name='visdom_cart_pole_rl', check_freq=100, log_dir=log_dir) 29 | callback_save_best_model = EvalCallback(env, best_model_save_path=log_dir, log_path=log_dir, eval_freq=500, deterministic=True, render=False) 30 | callback_list = CallbackList([callback_visdom, callback_save_best_model]) 31 | 32 | 33 | # ---------------- Model 34 | # Option 1: create a new model 35 | # print("create a new model") 36 | # model = A2C(policy='MlpPolicy', env=env, learning_rate=7e-4, verbose=True) 37 | 38 | # Option 2: load the model from files (note that the loaded model can be learned again) 39 | # print("load the model from files") 40 | # model = A2C.load("../CartPole/saved_models/tmp/best_model", env=env) 41 | # model.learning_rate = 1e-4 42 | 43 | # Option 3: load the pre-trained model from files 44 | print("load the pre-trained model from files") 45 | if env.action_type == 'discrete': 46 | model = A2C.load("../CartPole/saved_models/best_model_discrete", env=env) 47 | else: 48 | model = A2C.load("../CartPole/saved_models/best_model_continuous", env=env) 49 | 50 | 51 | # ---------------- Learning 52 | # print('Learning the model') 53 | # model.learn(total_timesteps=400000, callback=callback_list) # 'MlpPolicy' = Actor Critic Policy 54 | # print('Finished') 55 | # del model # delete the model and load the best model to predict 56 | # model = A2C.load("../CartPole/saved_models/tmp/best_model", env=env) 57 | 58 | 59 | # ---------------- Prediction 60 | print('Prediction') 61 | 62 | for _ in range(10): 63 | observation, done = env.reset(), False 64 | episode_reward = 0.0 65 | 66 | while not done: 67 | action, _state = model.predict(observation, deterministic=True) 68 | observation, reward, done, info = env.step(action) 69 | episode_reward += reward 70 | 71 | print([episode_reward, env.counts]) 72 | 73 | env.close() 74 | -------------------------------------------------------------------------------- /examples/gym_cart_pole_a2c.py: -------------------------------------------------------------------------------- 1 | import time 2 | import gym 3 | from stable_baselines3 import A2C 4 | 5 | import os 6 | from stable_baselines3.common.monitor import Monitor 7 | from stable_baselines3.common.callbacks import CallbackList 8 | from stable_baselines3.common.callbacks import EvalCallback 9 | 10 | import sys 11 | sys.path.append("../utils") 12 | from callbackFunctions import VisdomCallback 13 | 14 | # ---------------- create environment 15 | env = gym.make('CartPole-v1') 16 | 17 | # ---------------- callback functions 18 | log_dir = "../CartPole/saved_models/tmp" 19 | os.makedirs(log_dir, exist_ok=True) 20 | env = Monitor(env, log_dir) 21 | 22 | callback_visdom = VisdomCallback(name='visdom_simple_cart_pole_rl', check_freq=100, log_dir=log_dir) 23 | callback_save_best_model = EvalCallback(env, best_model_save_path=log_dir, log_path=log_dir, eval_freq=500, deterministic=True, render=False) 24 | callback_list = CallbackList([callback_visdom, callback_save_best_model]) 25 | 26 | 27 | # ---------------- model learning 28 | print('Learning the model') 29 | model = A2C('MlpPolicy', env, verbose=True) 30 | model.learn(total_timesteps=20000, callback=callback_list) # 'MlpPolicy' = Actor Critic Policy 31 | print('Learning finished') 32 | 33 | del model 34 | 35 | 36 | # ---------------- prediction 37 | print('Prediction') 38 | model = A2C.load("../CartPole/saved_models/tmp/best_model", env=env) 39 | 40 | 41 | observation = env.reset() 42 | for i in range(1000): 43 | action, _state = model.predict(observation, deterministic=True) 44 | observation, reward, done, info = env.step(action) 45 | env.render() 46 | if done: 47 | observation = env.reset() 48 | 49 | time.sleep(0.01) 50 | 51 | env.close() 52 | -------------------------------------------------------------------------------- /pic/cart_pole_demo_1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/CoppeliaSimRL/8eb98b8a42b515b61eae5ffebcc9f228e2f5fdd1/pic/cart_pole_demo_1.gif -------------------------------------------------------------------------------- /utils/callbackFunctions.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from stable_baselines3.common.results_plotter import load_results, ts2xy 3 | from stable_baselines3.common.callbacks import BaseCallback 4 | 5 | from visdom_utils import VisdomLinePlotter 6 | 7 | 8 | class VisdomCallback(BaseCallback): 9 | """ 10 | Callback for visualizing the reward and loss (the check is done every ``check_freq`` steps) 11 | 12 | :param check_freq: (int) 13 | It must contains the file created by the ``Monitor`` wrapper. 14 | :param verbose: (int) 15 | """ 16 | def __init__(self, name:str, check_freq: int, log_dir:str, verbose=1): 17 | super(VisdomCallback, self).__init__(verbose) 18 | self.check_freq = check_freq 19 | self.log_dir = log_dir 20 | self.plotter = VisdomLinePlotter(env_name=name) 21 | 22 | def _on_step(self) -> bool: 23 | if self.n_calls % self.check_freq == 0: 24 | x, y = ts2xy(load_results(self.log_dir), 'timesteps') # Retrieve training reward 25 | 26 | if len(x) > 0: 27 | print("sending values to visdom ...") 28 | mean_reward = np.mean(y[-10:]) # Mean training reward over the last 10 episodes 29 | self.plotter.plot('epochs', 'reward', 'reward', 'reward visual', self.n_calls, mean_reward) 30 | 31 | return True 32 | 33 | -------------------------------------------------------------------------------- /utils/visdom_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from visdom import Visdom 3 | 4 | class VisdomLinePlotter(object): 5 | """Plots to Visdom""" 6 | def __init__(self, env_name='main'): 7 | self.viz = Visdom() 8 | self.env = env_name 9 | self.plots = {} 10 | def plot(self, x_label='x_label', var_name='y_label', split_name='curve_name', title_name='title', x=0, y=0): 11 | if var_name not in self.plots: 12 | self.plots[var_name] = self.viz.line(X=np.array([x,x]), Y=np.array([y,y]), env=self.env, opts=dict( 13 | legend=[split_name], 14 | title=title_name, 15 | xlabel=x_label, 16 | ylabel=var_name 17 | )) 18 | else: 19 | self.viz.line(X=np.array([x]), Y=np.array([y]), env=self.env, win=self.plots[var_name], name=split_name, update = 'append') 20 | 21 | if __name__ == "__main__": 22 | import time 23 | 24 | # run 'visdom' in your terminal to start the visdom server before run this script 25 | 26 | plotter = VisdomLinePlotter(env_name='visdom_test') 27 | 28 | t = np.linspace(0, 10, 500) 29 | x = np.sin(t) 30 | for i in range(500): 31 | plotter.plot('epochs', 'y_label', 'curve_name', 'title', t[i], x[i]) 32 | time.sleep(0.01) 33 | --------------------------------------------------------------------------------