├── vrep_env ├── __init__.py ├── remoteApi.dll ├── remoteApi.so ├── remoteApi.dylib ├── vrep_env.py └── vrep.py ├── assets ├── vrep-ant.gif ├── screenshot.png ├── hopper-random.gif └── vrep-cartpole.png ├── examples ├── scenes │ ├── hopper.ttt │ └── gym_cartpole.ttt └── envs │ ├── hopper_vrep_env.py │ ├── example_vrep_env.py │ ├── cartpole_vrep_env.py │ └── cartpole_continuous_vrep_env.py ├── .gitignore ├── setup.py ├── LICENSE.md └── README.md /vrep_env/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /assets/vrep-ant.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ycps/vrep-env/HEAD/assets/vrep-ant.gif -------------------------------------------------------------------------------- /assets/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ycps/vrep-env/HEAD/assets/screenshot.png -------------------------------------------------------------------------------- /vrep_env/remoteApi.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ycps/vrep-env/HEAD/vrep_env/remoteApi.dll -------------------------------------------------------------------------------- /vrep_env/remoteApi.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ycps/vrep-env/HEAD/vrep_env/remoteApi.so -------------------------------------------------------------------------------- /assets/hopper-random.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ycps/vrep-env/HEAD/assets/hopper-random.gif -------------------------------------------------------------------------------- /assets/vrep-cartpole.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ycps/vrep-env/HEAD/assets/vrep-cartpole.png -------------------------------------------------------------------------------- /vrep_env/remoteApi.dylib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ycps/vrep-env/HEAD/vrep_env/remoteApi.dylib -------------------------------------------------------------------------------- /examples/scenes/hopper.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ycps/vrep-env/HEAD/examples/scenes/hopper.ttt -------------------------------------------------------------------------------- /examples/scenes/gym_cartpole.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ycps/vrep-env/HEAD/examples/scenes/gym_cartpole.ttt -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ## git ignore file for some python stuff 2 | 3 | # byte-compiled / optimized / dll files 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | 8 | # C extensions 9 | #*.so 10 | 11 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | install_requires=[ 4 | 'gym', 5 | 'numpy' 6 | ] 7 | 8 | setup(name='vrep_env', 9 | version='0.0.2', 10 | description='V-REP integrated with OpenAI Gym', 11 | url='https://github.com/ycps/vrep-env', 12 | packages=[package for package in find_packages() if package.startswith('vrep_env')], 13 | install_requires=install_requires, 14 | package_data={'': ['remoteApi.so']}, 15 | include_package_data=True 16 | ) 17 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | # vrep-env 2 | 3 | The MIT License 4 | 5 | Copyright (c) 2018 Yuri C. P. Soares 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in 15 | all copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 23 | THE SOFTWARE. 24 | 25 | # V-REP remote API 26 | 27 | This repository includes files from the V-REP remote API. 28 | 29 | Please visit [Coppelia Robotics](http://www.coppeliarobotics.com) for more information on those files. 30 | -------------------------------------------------------------------------------- /examples/envs/hopper_vrep_env.py: -------------------------------------------------------------------------------- 1 | 2 | from vrep_env import vrep_env 3 | 4 | import os 5 | vrep_scenes_path = os.environ['VREP_SCENES_PATH'] 6 | 7 | import gym 8 | from gym import spaces 9 | from gym.utils import seeding 10 | import numpy as np 11 | 12 | class HopperVrepEnv(vrep_env.VrepEnv): 13 | metadata = {'render.modes': [],} 14 | def __init__( 15 | self, 16 | server_addr='127.0.0.1', 17 | server_port=-19997, 18 | scene_path=vrep_scenes_path+'/hopper.ttt', 19 | ): 20 | vrep_env.VrepEnv.__init__( 21 | self, 22 | server_addr, 23 | server_port, 24 | scene_path, 25 | ) 26 | 27 | # Settings 28 | self.random_start = False 29 | 30 | # All joints 31 | joint_names = ['thigh_joint','leg_joint','foot_joint'] 32 | # All shapes 33 | shape_names = ['torso','thigh','leg','foot'] 34 | 35 | # Getting object handles 36 | 37 | # Meta 38 | self.camera = self.get_object_handle('camera') 39 | # Actuators 40 | self.oh_joint = list(map(self.get_object_handle, joint_names)) 41 | # Shapes 42 | self.oh_shape = list(map(self.get_object_handle, shape_names)) 43 | 44 | # One action per joint 45 | dim_act = len(self.oh_joint) 46 | # Multiple dimensions per shape 47 | dim_obs = (len(self.oh_shape)*3*2)+1 48 | 49 | high_act = np.ones([dim_act]) 50 | high_obs = np.inf*np.ones([dim_obs]) 51 | 52 | self.action_space = gym.spaces.Box(-high_act, high_act) 53 | self.observation_space = gym.spaces.Box(-high_obs, high_obs) 54 | 55 | # Parameters 56 | self.joints_max_velocity = 8.0 57 | #self.power = 0.75 58 | self.power = 3.75 59 | 60 | self.seed() 61 | 62 | print('HopperVrepEnv: initialized') 63 | 64 | def _make_observation(self): 65 | """Get observation from v-rep and stores in self.observation 66 | """ 67 | lst_o = [] 68 | 69 | # Include z position in observation 70 | torso_pos = self.obj_get_position(self.oh_shape[0]) 71 | lst_o += [torso_pos[2]] 72 | 73 | # Include shapes relative velocities in observation 74 | for i_oh in self.oh_shape: 75 | lin_vel , ang_vel = self.obj_get_velocity(i_oh) 76 | lst_o += ang_vel 77 | lst_o += lin_vel 78 | 79 | self.observation = np.array(lst_o).astype('float32') 80 | 81 | def _make_action(self, a): 82 | """Send action to v-rep 83 | """ 84 | for i_oh, i_a in zip(self.oh_joint, a): 85 | #self.obj_set_velocity(i_oh, i_a) 86 | self.obj_set_velocity(i_oh, self.power*float(np.clip(i_a,-1,+1))) 87 | 88 | def step(self, action): 89 | # Clip xor Assert 90 | #actions = np.clip(actions,-self.joints_max_velocity, self.joints_max_velocity) 91 | assert self.action_space.contains(action), "%r (%s) invalid"%(action, type(action)) 92 | 93 | # Actuate 94 | self._make_action(action) 95 | #self._make_action(action*self.joints_max_velocity) 96 | # Step 97 | self.step_simulation() 98 | # Observe 99 | self._make_observation() 100 | 101 | # Reward 102 | torso_pos_z = self.observation[0] # up/down 103 | torso_lvel_x = self.observation[4] 104 | r_alive = 1.0 105 | 106 | reward = (16.0)*(r_alive) +(8.0)*(torso_lvel_x) 107 | 108 | # Early stop 109 | stand_threshold = 0.10 110 | done = (torso_pos_z < stand_threshold) 111 | #done = False 112 | 113 | return self.observation, reward, done, {} 114 | 115 | def reset(self): 116 | if self.sim_running: 117 | self.stop_simulation() 118 | self.start_simulation() 119 | 120 | # First action is random: emulate random initialization 121 | if self.random_start: 122 | factor = self.np_random.uniform(low=0, high=0.02, size=(1,))[0] 123 | action = self.action_space.sample()*factor 124 | self._make_action(action) 125 | self.step_simulation() 126 | 127 | self._make_observation() 128 | return self.observation 129 | 130 | def render(self, mode='human', close=False): 131 | pass 132 | 133 | def seed(self, seed=None): 134 | self.np_random, seed = seeding.np_random(seed) 135 | return [seed] 136 | 137 | def main(args): 138 | env = HopperVrepEnv() 139 | for i_episode in range(4): 140 | observation = env.reset() 141 | print(observation) 142 | total_reward = 0 143 | for t in range(256): 144 | action = env.action_space.sample() 145 | observation, reward, done, _ = env.step(action) 146 | total_reward += reward 147 | if done: 148 | break 149 | print("Episode finished after {} timesteps.\tTotal reward: {}".format(t+1,total_reward)) 150 | env.close() 151 | return 0 152 | 153 | if __name__ == '__main__': 154 | import sys 155 | sys.exit(main(sys.argv)) 156 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # vrep-env 2 | 3 | V-REP integrated with OpenAI Gym. 4 | This project aims to provide a superclass for V-REP gym environments. 5 | It is analogous to [MuJoCo-env](https://github.com/openai/gym/blob/master/gym/envs/mujoco/mujoco_env.py) for MuJoCo. 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 |
16 | 17 | ## Dependencies 18 | 19 | - [V-REP](http://www.coppeliarobotics.com/downloads.html) 20 | - [OpenAI Gym](https://github.com/openai/gym) 21 | 22 | ## Installation 23 | 24 | In order to smooth the installation process, define the variables `VREP_PATH` and `VREP_SCENES_PATH` that contain your V-REP scenes. 25 | 26 | Example: 27 | ```bash 28 | export VREP_PATH=/example/some/path/to/V-REP_PRO_EDU_V3_4_0_Linux/ 29 | export VREP_SCENES_PATH=/example/again/V-REP_PRO_EDU_V3_4_0_Linux/scenes/ 30 | ``` 31 | These variables will be used as default if the respective argument is not provided. 32 | Next, simply install via pip: 33 | ```bash 34 | pip3 install --upgrade git+https://github.com/ycps/vrep-env.git#egg=vrep_env 35 | ``` 36 | 37 | ## Creating your own V-REP Gym environments 38 | 39 | In order to create your own V-REP Gym environments, simply extend the [`VrepEnv`](vrep_env/vrep_env.py) class and fill in the gaps. 40 | You may use the [`ExampleVrepEnv`](examples/envs/example_vrep_env.py) as a template base or check the fully functional [`HopperVrepEnv`](examples/envs/hopper_vrep_env.py) (similar to the [MuJoCo](https://github.com/openai/gym/blob/master/gym/envs/mujoco/hopper.py) / [Roboschool](https://github.com/openai/roboschool/blob/master/roboschool/gym_mujoco_walkers.py) Hopper) 41 | 42 | ## Usage 43 | 44 | Before starting your environment, an instance of V-REP should already be running. It uses port 19997 by default, but it can be overriden in class initialization. 45 | Check the [`HopperVrepEnv`](examples/envs/hopper_vrep_env.py) for a simple running example. 46 | It can be run as: 47 | ```bash 48 | python3 hopper_vrep_env.py 49 | ``` 50 | If everything was installed correctly, you should see a random agent struggling to hop: 51 | 52 | 53 | 54 | You may have to register the envs as following: 55 | ```python3 56 | register(id='VrepCartPole-v0', entry_point='cartpole_vrep_env:CartPoleVrepEnv', max_episode_steps=200, reward_threshold=195.0) 57 | register(id='VrepCartPoleContinuous-v0', entry_point='cartpole_continuous_vrep_env:CartPoleContinuousVrepEnv', max_episode_steps=200, reward_threshold=195.0) 58 | register(id='VrepHopper-v0', entry_point='hopper_vrep_env:HopperVrepEnv', max_episode_steps=1000) 59 | ``` 60 | 61 | ## Example Environments 62 | 63 | | Environment Id | Observation Space | Action Space | tStepL | BasedOn | 64 | |---|---|---|---|---| 65 | |VrepCartPole-v0|Box(4)|Discrete(2)|200|[CartPole-v1](https://gym.openai.com/envs/CartPole-v1)| 66 | |VrepCartPoleContinuous-v0|Box(4)|Box(1)|200|[CartPole-v1](https://gym.openai.com/envs/CartPole-v1)| 67 | |VrepHopper-v0|Box(25)|Box(3)|1000|[Hopper-v*](https://github.com/openai/gym/blob/master/gym/envs/mujoco/hopper.py)| 68 | |VrepAnt-v0|Box(28)|Box(8)|1000|[Ant-v*](https://github.com/openai/gym/blob/master/gym/envs/mujoco/ant.py)| 69 | 70 | ### VrepCartPole-v0 71 | Based on Gym CartPole-v1 (cart-pole problem described by Barto, Sutton, and Anderson). 72 | An agent trained in CartPole-v1 may be able to succeed in VrepCartPole-v0 without additional training. 73 | ### VrepCartPoleContinuous-v0 74 | Similar to VrepCartPole-v0, but with continuous actions values. 75 | ### VrepHopper-v0 76 | Loosely based on [MuJoCo](https://github.com/openai/gym/tree/master/gym/envs/mujoco)/[Roboschool](https://github.com/openai/roboschool)/[PyBullet](https://github.com/benelot/pybullet-gym) Hopper, but the dynamics act numerically different. 77 | (Warning: it is not known if this env is learnable nor if the model is capable of hopping.) 78 | ### VrepAnt-v0 (WIP) 79 | Based on [MuJoCo](https://github.com/openai/gym/tree/master/gym/envs/mujoco)/[Roboschool](https://github.com/openai/roboschool)/[PyBullet](https://github.com/benelot/pybullet-gym) Ant, the dynamics act numerically similar (but not identical). 80 | An agent trained in the original Ant envs may be able to succeed in VrepAnt-v0 with little or no additional training. 81 | 82 | ## Similar projects 83 | 84 | There are other similar projects that attempt to wrap V-Rep and create a gym interface. 85 | Some of these projects also contain interesting scenes for learning different tasks. 86 | 87 | - https://github.com/ctmakro/vrepper 88 | - https://github.com/fgolemo/vrepper 89 | - https://github.com/bhyang/gym-vrep 90 | - https://github.com/kbys-t/gym_vrep 91 | - https://github.com/sayantanauddy/acrobotVREP 92 | - https://github.com/Souphis/gym-vrep 93 | -------------------------------------------------------------------------------- /examples/envs/example_vrep_env.py: -------------------------------------------------------------------------------- 1 | 2 | # This file is a template for V-rep environments 3 | # all names in this file are just examples 4 | # Search for '#modify' and replace accordingly 5 | 6 | from vrep_env import vrep_env 7 | from vrep_env import vrep # vrep.sim_handle_parent 8 | 9 | import os 10 | vrep_scenes_path = os.environ['VREP_SCENES_PATH'] 11 | 12 | import gym 13 | from gym import spaces 14 | 15 | import numpy as np 16 | 17 | # #modify: the env class name 18 | class ExampleVrepEnv(vrep_env.VrepEnv): 19 | metadata = { 20 | 'render.modes': [], 21 | } 22 | def __init__( 23 | self, 24 | server_addr='127.0.0.1', 25 | server_port=19997, 26 | # #modify: the filename of your v-rep scene 27 | scene_path=vrep_scenes_path+'/example.ttt' 28 | ): 29 | 30 | vrep_env.VrepEnv.__init__(self,server_addr,server_port,scene_path) 31 | # #modify: the name of the joints to be used in action space 32 | joint_names = [ 33 | 'example_joint_0', 34 | 'example_left_joint_0','example_right_joint_0', 35 | 'example_joint_1', 36 | 'example_left_joint_1','example_right_joint_1', 37 | ] 38 | # #modify: the name of the shapes to be used in observation space 39 | shape_names = [ 40 | 'example_head', 41 | 'example_left_arm','example_right_arm', 42 | 'example_torso', 43 | 'example_left_leg','example_right_leg', 44 | ] 45 | 46 | # Getting object handles 47 | 48 | # we will store V-rep object handles (oh = object handle) 49 | 50 | # Meta 51 | # #modify: if you want additional object handles 52 | self.camera = self.get_object_handle('camera') 53 | 54 | # Actuators 55 | self.oh_joint = list(map(self.get_object_handle, joint_names)) 56 | # Shapes 57 | self.oh_shape = list(map(self.get_object_handle, shape_names)) 58 | 59 | 60 | # #modify: if size of action space is different than number of joints 61 | # Example: One action per joint 62 | num_act = len(self.oh_joint) 63 | 64 | # #modify: if size of observation space is different than number of joints 65 | # Example: 3 dimensions of linear and angular (2) velocities + 6 additional dimension 66 | num_obs = (len(self.oh_shape)*3*2) + 3*2 67 | 68 | # #modify: action_space and observation_space to suit your needs 69 | self.joints_max_velocity = 3.0 70 | act = np.array( [self.joints_max_velocity] * num_act ) 71 | obs = np.array( [np.inf] * num_obs ) 72 | 73 | self.action_space = spaces.Box(-act,act) 74 | self.observation_space = spaces.Box(-obs,obs) 75 | 76 | # #modify: optional message 77 | print('ExampleVrepEnv: initialized') 78 | 79 | def _make_observation(self): 80 | """Query V-rep to make observation. 81 | The observation is stored in self.observation 82 | """ 83 | # start with empty list 84 | lst_o = []; 85 | 86 | # #modify: optionally include positions or velocities 87 | pos = self.obj_get_position(self.oh_shape[0]) 88 | lin_vel , ang_vel = self.obj_get_velocity(self.oh_shape[0]) 89 | lst_o += pos 90 | lst_o += lin_vel 91 | 92 | # #modify 93 | # example: include position, linear and angular velocities of all shapes 94 | for i_oh in self.oh_shape: 95 | rel_pos = self.obj_get_position(i_oh, relative_to=vrep.sim_handle_parent) 96 | lst_o += rel_pos ; 97 | lin_vel , ang_vel = self.obj_get_velocity(i_oh) 98 | lst_o += ang_vel ; 99 | lst_o += lin_vel ; 100 | 101 | self.observation = np.array(lst_o).astype('float32'); 102 | 103 | def _make_action(self, a): 104 | """Query V-rep to make action. 105 | no return value 106 | """ 107 | # #modify 108 | # example: set a velocity for each joint 109 | for i_oh, i_a in zip(self.oh_joint, a): 110 | self.obj_set_velocity(i_oh, i_a) 111 | 112 | def step(self, action): 113 | """Gym environment 'step' 114 | """ 115 | # #modify Either clip the actions outside the space or assert the space contains them 116 | # actions = np.clip(actions,-self.joints_max_velocity, self.joints_max_velocity) 117 | assert self.action_space.contains(action), "Action {} ({}) is invalid".format(action, type(action)) 118 | 119 | # Actuate 120 | self._make_action(action) 121 | # Step 122 | self.step_simulation() 123 | # Observe 124 | self._make_observation() 125 | 126 | # Reward 127 | # #modify the reward computation 128 | # example: possible variables used in reward 129 | head_pos_x = self.observation[0] # front/back 130 | head_pos_y = self.observation[1] # left/right 131 | head_pos_z = self.observation[2] # up/down 132 | nrm_action = np.linalg.norm(actions) 133 | r_regul = -(nrm_action**2) 134 | r_alive = 1.0 135 | # example: different weights in reward 136 | reward = (8.0)*(r_alive) +(4.0)*(head_pos_x) +(1.0)*(head_pos_z) 137 | 138 | # Early stop 139 | # #modify if the episode should end earlier 140 | tolerable_threshold = 0.20 141 | done = (head_pos_z < tolerable_threshold) 142 | #done = False 143 | 144 | return self.observation, reward, done, {} 145 | 146 | def reset(self): 147 | """Gym environment 'reset' 148 | """ 149 | if self.sim_running: 150 | self.stop_simulation() 151 | self.start_simulation() 152 | self._make_observation() 153 | return self.observation 154 | 155 | def render(self, mode='human', close=False): 156 | """Gym environment 'render' 157 | """ 158 | pass 159 | 160 | def seed(self, seed=None): 161 | """Gym environment 'seed' 162 | """ 163 | return [] 164 | 165 | def main(args): 166 | """main function used as test and example. 167 | Agent does random actions with 'action_space.sample()' 168 | """ 169 | # #modify: the env class name 170 | env = ExampleVrepEnv() 171 | for i_episode in range(8): 172 | observation = env.reset() 173 | total_reward = 0 174 | for t in range(256): 175 | action = env.action_space.sample() 176 | observation, reward, done, _ = env.step(action) 177 | total_reward += reward 178 | if done: 179 | break 180 | print("Episode finished after {} timesteps.\tTotal reward: {}".format(t+1,total_reward)) 181 | env.close() 182 | return 0 183 | 184 | if __name__ == '__main__': 185 | import sys 186 | sys.exit(main(sys.argv)) 187 | -------------------------------------------------------------------------------- /examples/envs/cartpole_vrep_env.py: -------------------------------------------------------------------------------- 1 | """ 2 | Adaptation from: 3 | https://github.com/openai/gym/blob/master/gym/envs/classic_control/cartpole.py 4 | Which is based in: 5 | Classic cart-pole system implemented by Rich Sutton et al. 6 | Copied from http://incompleteideas.net/sutton/book/code/pole.c 7 | permalink: https://perma.cc/C9ZM-652R 8 | """ 9 | 10 | from vrep_env import vrep_env 11 | from vrep_env import vrep 12 | 13 | import os 14 | vrep_scenes_path = os.environ['VREP_SCENES_PATH'] 15 | 16 | import math 17 | import gym 18 | from gym import spaces 19 | from gym.utils import seeding 20 | import numpy as np 21 | 22 | class CartPoleVrepEnv(vrep_env.VrepEnv): 23 | metadata = { 24 | 'render.modes': ['human', 'rgb_array'], 25 | 'video.frames_per_second' : 50 26 | } 27 | def __init__( 28 | self, 29 | server_addr='127.0.0.1', 30 | server_port=19997, 31 | scene_path=vrep_scenes_path+'/gym_cartpole.ttt', 32 | ): 33 | vrep_env.VrepEnv.__init__( 34 | self, 35 | server_addr, 36 | server_port, 37 | scene_path, 38 | ) 39 | 40 | # getting object handles 41 | self.action = self.get_object_handle('action') 42 | self.cart = self.get_object_handle('cart') 43 | self.pole = self.get_object_handle('pole') 44 | self.viewer = self.get_object_handle('viewer') 45 | 46 | # adjusting parameters 47 | self.tau = 0.02 # seconds between state updates 48 | self.gravity = 9.8 49 | self.force_mag = 10.0 50 | 51 | self.set_float_parameter(vrep.sim_floatparam_simulation_time_step, self.tau) 52 | self.set_array_parameter(vrep.sim_arrayparam_gravity,[0,0,-self.gravity]) 53 | self.obj_set_force(self.action,self.force_mag) 54 | 55 | # Angle at which to fail the episode 56 | self.theta_threshold_radians = 12 * 2 * math.pi / 360 57 | self.x_threshold = 2.4 58 | # Angle limit set to 2 * theta_threshold_radians so failing observation is still within bounds 59 | high = np.array([ 60 | self.x_threshold * 2, np.finfo(np.float32).max, 61 | self.theta_threshold_radians * 2, np.finfo(np.float32).max]) 62 | 63 | self.action_space = spaces.Discrete(2) 64 | self.observation_space = spaces.Box(-high, high) 65 | 66 | self.seed() 67 | self.viewer = None 68 | self.state = None 69 | self.steps_beyond_done = None 70 | 71 | def seed(self, seed=None): 72 | self.np_random, seed = seeding.np_random(seed) 73 | return [seed] 74 | 75 | def _make_observation(self): 76 | # discard y and z values 77 | [ x ,_,_ ] = self.obj_get_position(self.cart) 78 | [x_dot ,_,_ ] , _ = self.obj_get_velocity(self.cart) 79 | 80 | [_, theta ,_] = self.obj_get_orientation(self.pole) 81 | _ , [_, theta_dot ,_] = self.obj_get_velocity(self.pole) 82 | 83 | self.state = (x,x_dot,theta,theta_dot) 84 | 85 | def _make_action(self, a): 86 | v = 1 if a==1 else -1 87 | # 2000 is an arbitrary high value 88 | # more info at: forum.coppeliarobotics.com/viewtopic.php?t=497 89 | self.obj_set_velocity(self.action,v*2000.0) 90 | 91 | def step(self, action): 92 | assert self.action_space.contains(action), "%r (%s) invalid"%(action, type(action)) 93 | 94 | # Actuate 95 | self._make_action(action) 96 | # Step 97 | self.step_simulation() 98 | # Observe 99 | self._make_observation() 100 | 101 | (x,x_dot,theta,theta_dot) = self.state 102 | 103 | done = x < -self.x_threshold or theta < -self.theta_threshold_radians \ 104 | or x > self.x_threshold or theta > self.theta_threshold_radians 105 | done = bool(done) 106 | 107 | if not done: 108 | reward = 1.0 109 | elif self.steps_beyond_done is None: 110 | self.steps_beyond_done = 0 111 | reward = 1.0 112 | else: 113 | if self.steps_beyond_done == 0: 114 | logger.warning("You are calling 'step()' even though this environment has already returned done = True. You should always call 'reset()' once you receive 'done = True' -- any further steps are undefined behavior.") 115 | self.steps_beyond_done += 1 116 | reward = 0.0 117 | 118 | return np.array(self.state), reward, done, {} 119 | 120 | def reset(self): 121 | if self.sim_running: 122 | self.stop_simulation() 123 | self.start_simulation() 124 | self.steps_beyond_done = None 125 | 126 | v = self.np_random.uniform(low=-0.04, high=0.04, size=(1,)) 127 | self.obj_set_velocity(self.action,v) 128 | self.step_simulation() 129 | 130 | self._make_observation() 131 | return np.array(self.state) 132 | 133 | def render(self, mode='human'): 134 | screen_width = 600 135 | screen_height = 400 136 | 137 | world_width = self.x_threshold*2 138 | scale = screen_width/world_width 139 | carty = 100 # TOP OF CART 140 | polewidth = 10.0 141 | polelen = scale * 1.0 142 | cartwidth = 50.0 143 | cartheight = 30.0 144 | 145 | if self.viewer is None: 146 | from gym.envs.classic_control import rendering 147 | self.viewer = rendering.Viewer(screen_width, screen_height) 148 | l,r,t,b = -cartwidth/2, cartwidth/2, cartheight/2, -cartheight/2 149 | axleoffset =cartheight/4.0 150 | cart = rendering.FilledPolygon([(l,b), (l,t), (r,t), (r,b)]) 151 | self.carttrans = rendering.Transform() 152 | cart.add_attr(self.carttrans) 153 | self.viewer.add_geom(cart) 154 | l,r,t,b = -polewidth/2,polewidth/2,polelen-polewidth/2,-polewidth/2 155 | pole = rendering.FilledPolygon([(l,b), (l,t), (r,t), (r,b)]) 156 | pole.set_color(.8,.6,.4) 157 | self.poletrans = rendering.Transform(translation=(0, axleoffset)) 158 | pole.add_attr(self.poletrans) 159 | pole.add_attr(self.carttrans) 160 | self.viewer.add_geom(pole) 161 | self.axle = rendering.make_circle(polewidth/2) 162 | self.axle.add_attr(self.poletrans) 163 | self.axle.add_attr(self.carttrans) 164 | self.axle.set_color(.5,.5,.8) 165 | self.viewer.add_geom(self.axle) 166 | self.track = rendering.Line((0,carty), (screen_width,carty)) 167 | self.track.set_color(0,0,0) 168 | self.viewer.add_geom(self.track) 169 | 170 | if self.state is None: return None 171 | 172 | x = self.state 173 | cartx = x[0]*scale+screen_width/2.0 # MIDDLE OF CART 174 | self.carttrans.set_translation(cartx, carty) 175 | self.poletrans.set_rotation(-x[2]) 176 | 177 | return self.viewer.render(return_rgb_array = mode=='rgb_array') 178 | 179 | def close(self): 180 | if self.viewer: self.viewer.close() 181 | vrep_env.VrepEnv.close(self) 182 | 183 | def main(args): 184 | env = CartPoleVrepEnv() 185 | for i_episode in range(8): 186 | observation = env.reset() 187 | total_reward = 0 188 | for t in range(200): 189 | action = env.action_space.sample() 190 | observation, reward, done, _ = env.step(action) 191 | total_reward += reward 192 | if done: 193 | break 194 | print("Episode finished after {} timesteps.\tTotal reward: {}".format(t+1,total_reward)) 195 | env.close() 196 | return 0 197 | 198 | if __name__ == '__main__': 199 | import sys 200 | sys.exit(main(sys.argv)) 201 | -------------------------------------------------------------------------------- /examples/envs/cartpole_continuous_vrep_env.py: -------------------------------------------------------------------------------- 1 | """ 2 | Adaptation from: 3 | https://github.com/openai/gym/blob/master/gym/envs/classic_control/cartpole.py 4 | Which is based in: 5 | Classic cart-pole system implemented by Rich Sutton et al. 6 | Copied from http://incompleteideas.net/sutton/book/code/pole.c 7 | permalink: https://perma.cc/C9ZM-652R 8 | """ 9 | 10 | from vrep_env import vrep_env 11 | from vrep_env import vrep 12 | 13 | import os 14 | vrep_scenes_path = os.environ['VREP_SCENES_PATH'] 15 | 16 | import math 17 | import gym 18 | from gym import spaces 19 | from gym.utils import seeding 20 | import numpy as np 21 | 22 | class CartPoleContinuousVrepEnv(vrep_env.VrepEnv): 23 | metadata = { 24 | 'render.modes': ['human', 'rgb_array'], 25 | 'video.frames_per_second' : 50 26 | } 27 | def __init__( 28 | self, 29 | server_addr='127.0.0.1', 30 | server_port=19997, 31 | scene_path=vrep_scenes_path+'/gym_cartpole.ttt', 32 | ): 33 | vrep_env.VrepEnv.__init__( 34 | self, 35 | server_addr, 36 | server_port, 37 | scene_path, 38 | ) 39 | 40 | # getting object handles 41 | self.action = self.get_object_handle('action') 42 | self.cart = self.get_object_handle('cart') 43 | self.pole = self.get_object_handle('pole') 44 | self.viewer = self.get_object_handle('viewer') 45 | 46 | # adjusting parameters 47 | self.tau = 0.02 # seconds between state updates 48 | self.gravity = 9.8 49 | #self.force_mag = 10.0 50 | self.force_mag = 100.0 51 | 52 | self.set_float_parameter(vrep.sim_floatparam_simulation_time_step, self.tau) 53 | self.set_array_parameter(vrep.sim_arrayparam_gravity,[0,0,-self.gravity]) 54 | self.obj_set_force(self.action,self.force_mag) 55 | 56 | # Angle at which to fail the episode 57 | self.theta_threshold_radians = 12 * 2 * math.pi / 360 58 | self.x_threshold = 2.4 59 | # Angle limit set to 2 * theta_threshold_radians so failing observation is still within bounds 60 | high = np.array([ 61 | self.x_threshold * 2, np.finfo(np.float32).max, 62 | self.theta_threshold_radians * 2, np.finfo(np.float32).max]) 63 | 64 | self.min_action = -1.0 65 | self.max_action = 1.0 66 | 67 | self.action_space = spaces.Box(low=self.min_action, high=self.max_action, shape=(1,)) 68 | self.observation_space = spaces.Box(-high, high) 69 | 70 | self.seed() 71 | self.viewer = None 72 | self.state = None 73 | self.steps_beyond_done = None 74 | 75 | def seed(self, seed=None): 76 | self.np_random, seed = seeding.np_random(seed) 77 | return [seed] 78 | 79 | def _make_observation(self): 80 | # discard y and z values 81 | [ x ,_,_ ] = self.obj_get_position(self.cart) 82 | [x_dot ,_,_ ] , _ = self.obj_get_velocity(self.cart) 83 | 84 | [_, theta ,_] = self.obj_get_orientation(self.pole) 85 | _ , [_, theta_dot ,_] = self.obj_get_velocity(self.pole) 86 | 87 | self.state = (x,x_dot,theta,theta_dot) 88 | 89 | def _make_action(self, a): 90 | self.obj_set_velocity(self.action,a*2.0) 91 | 92 | def step(self, action): 93 | assert self.action_space.contains(action), "%r (%s) invalid"%(action, type(action)) 94 | 95 | # Actuate 96 | self._make_action(action) 97 | # Step 98 | self.step_simulation() 99 | # Observe 100 | self._make_observation() 101 | 102 | (x,x_dot,theta,theta_dot) = self.state 103 | 104 | done = x < -self.x_threshold or theta < -self.theta_threshold_radians \ 105 | or x > self.x_threshold or theta > self.theta_threshold_radians 106 | done = bool(done) 107 | 108 | if not done: 109 | reward = 1.0 110 | elif self.steps_beyond_done is None: 111 | self.steps_beyond_done = 0 112 | reward = 1.0 113 | else: 114 | if self.steps_beyond_done == 0: 115 | logger.warning("You are calling 'step()' even though this environment has already returned done = True. You should always call 'reset()' once you receive 'done = True' -- any further steps are undefined behavior.") 116 | self.steps_beyond_done += 1 117 | reward = 0.0 118 | 119 | return np.array(self.state), reward, done, {} 120 | 121 | def reset(self): 122 | if self.sim_running: 123 | self.stop_simulation() 124 | self.start_simulation() 125 | self.steps_beyond_done = None 126 | 127 | v = self.np_random.uniform(low=-0.04, high=0.04, size=(1,)) 128 | self.obj_set_velocity(self.action,v) 129 | self.step_simulation() 130 | 131 | self._make_observation() 132 | return np.array(self.state) 133 | 134 | def render(self, mode='human'): 135 | screen_width = 600 136 | screen_height = 400 137 | 138 | world_width = self.x_threshold*2 139 | scale = screen_width/world_width 140 | carty = 100 # TOP OF CART 141 | polewidth = 10.0 142 | polelen = scale * 1.0 143 | cartwidth = 50.0 144 | cartheight = 30.0 145 | 146 | if self.viewer is None: 147 | from gym.envs.classic_control import rendering 148 | self.viewer = rendering.Viewer(screen_width, screen_height) 149 | l,r,t,b = -cartwidth/2, cartwidth/2, cartheight/2, -cartheight/2 150 | axleoffset =cartheight/4.0 151 | cart = rendering.FilledPolygon([(l,b), (l,t), (r,t), (r,b)]) 152 | self.carttrans = rendering.Transform() 153 | cart.add_attr(self.carttrans) 154 | self.viewer.add_geom(cart) 155 | l,r,t,b = -polewidth/2,polewidth/2,polelen-polewidth/2,-polewidth/2 156 | pole = rendering.FilledPolygon([(l,b), (l,t), (r,t), (r,b)]) 157 | pole.set_color(.8,.6,.4) 158 | self.poletrans = rendering.Transform(translation=(0, axleoffset)) 159 | pole.add_attr(self.poletrans) 160 | pole.add_attr(self.carttrans) 161 | self.viewer.add_geom(pole) 162 | self.axle = rendering.make_circle(polewidth/2) 163 | self.axle.add_attr(self.poletrans) 164 | self.axle.add_attr(self.carttrans) 165 | self.axle.set_color(.5,.5,.8) 166 | self.viewer.add_geom(self.axle) 167 | self.track = rendering.Line((0,carty), (screen_width,carty)) 168 | self.track.set_color(0,0,0) 169 | self.viewer.add_geom(self.track) 170 | 171 | if self.state is None: return None 172 | 173 | x = self.state 174 | cartx = x[0]*scale+screen_width/2.0 # MIDDLE OF CART 175 | self.carttrans.set_translation(cartx, carty) 176 | self.poletrans.set_rotation(-x[2]) 177 | 178 | return self.viewer.render(return_rgb_array = mode=='rgb_array') 179 | 180 | def close(self): 181 | if self.viewer: self.viewer.close() 182 | vrep_env.VrepEnv.close(self) 183 | 184 | def main(args): 185 | env = CartPoleContinuousVrepEnv() 186 | for i_episode in range(8): 187 | observation = env.reset() 188 | total_reward = 0 189 | for t in range(200): 190 | action = env.action_space.sample() 191 | observation, reward, done, _ = env.step(action) 192 | total_reward += reward 193 | if done: 194 | break 195 | print("Episode finished after {} timesteps.\tTotal reward: {}".format(t+1,total_reward)) 196 | env.close() 197 | return 0 198 | 199 | if __name__ == '__main__': 200 | import sys 201 | sys.exit(main(sys.argv)) 202 | -------------------------------------------------------------------------------- /vrep_env/vrep_env.py: -------------------------------------------------------------------------------- 1 | 2 | from vrep_env import vrep 3 | 4 | import gym 5 | import time 6 | import numpy as np 7 | 8 | class VrepEnv(gym.Env): 9 | """Superclass for V-REP environments. 10 | """ 11 | def __init__(self,server_addr,server_port,scene_path=None): 12 | # Parameters 13 | self.server_addr = server_addr 14 | self.server_port = server_port 15 | self.scene_path = scene_path 16 | 17 | self.opM_get = vrep.simx_opmode_blocking 18 | #self.opM_get = vrep.simx_opmode_oneshot 19 | self.opM_set = vrep.simx_opmode_oneshot 20 | 21 | # Status 22 | self.cID = -1 23 | self.connected = False 24 | self.scene_loaded = (scene_path == None) 25 | self.sim_running = False 26 | 27 | # Remote API function meaningful return codes 28 | self.str_simx_return = [ 29 | 'simx_return_ok', 30 | 'simx_return_novalue_flag', 31 | 'simx_return_timeout_flag', 32 | 'simx_return_illegal_opmode_flag', 33 | 'simx_return_remote_error_flag', 34 | 'simx_return_split_progress_flag', 35 | 'simx_return_local_error_flag', 36 | 'simx_return_initialize_error_flag'] 37 | 38 | self.connect(server_addr,server_port) 39 | if not self.scene_loaded: 40 | self.load_scene(scene_path) 41 | 42 | # internal methods 43 | 44 | # Remote API call wrapper 45 | #def RAPI_rc(self, ret_tuple, tolerance=vrep.simx_return_ok): 46 | def RAPI_rc(self, ret_tuple, tolerance=vrep.simx_return_novalue_flag): 47 | istuple = isinstance(ret_tuple, tuple) 48 | ret = ret_tuple[0] if istuple else ret_tuple 49 | if (ret != vrep.simx_return_ok) and (ret != tolerance): 50 | raise RuntimeError('Remote API return code: ('+str(ret)+': '+self.str_simx_return[ret.bit_length()]+')') 51 | 52 | return ret_tuple[1:] if istuple else None 53 | 54 | def connect(self, server_addr, server_port): 55 | if self.connected: 56 | raise RuntimeError('Client is already connected.') 57 | attempts = 0 58 | max_attempts = 64 59 | while True: 60 | self.cID = vrep.simxStart( 61 | connectionAddress = server_addr, 62 | connectionPort = server_port, 63 | waitUntilConnected = True, 64 | doNotReconnectOnceDisconnected = True, 65 | timeOutInMs = 1000, 66 | commThreadCycleInMs = 0) 67 | attempts += 1 68 | if self.cID != -1: 69 | self.connected = True 70 | break 71 | elif attempts < max_attempts: 72 | print('Unable to connect to V-REP at ',server_addr,':',server_port,'. Retrying...') 73 | time.sleep(4) 74 | else: 75 | raise RuntimeError('Unable to connect to V-REP.') 76 | 77 | # Setting up debug signal 78 | self.set_integer_signal('sig_debug',1337) 79 | 80 | # Getting useful parameter values 81 | self.is_headless = self.get_boolean_parameter(vrep.sim_boolparam_headless) 82 | 83 | # If not headless, remove GUI clutter 84 | if not self.is_headless: 85 | self.set_boolean_parameter(vrep.sim_boolparam_browser_visible ,False) 86 | self.set_boolean_parameter(vrep.sim_boolparam_hierarchy_visible,False) 87 | #self.set_boolean_parameter(vrep.sim_boolparam_display_enabled ,False) 88 | # Remove GUI controls 89 | #self.set_boolean_parameter(vrep.sim_boolparam_play_toolbarbutton_enabled ,False) 90 | #self.set_boolean_parameter(vrep.sim_boolparam_pause_toolbarbutton_enabled ,False) 91 | #self.set_boolean_parameter(vrep.sim_boolparam_stop_toolbarbutton_enabled ,False) 92 | self.set_boolean_parameter(vrep.sim_boolparam_console_visible ,False) 93 | 94 | # Optionally override real-time mode 95 | self.set_boolean_parameter(vrep.sim_boolparam_realtime_simulation, False) 96 | 97 | def disconnect(self): 98 | if not self.connected: 99 | raise RuntimeError('Client is not even connected.') 100 | # Clearing debug signal 101 | vrep.simxClearIntegerSignal(self.cID,'sig_debug', vrep.simx_opmode_blocking) 102 | vrep.simxFinish(self.cID) 103 | self.connected = False 104 | 105 | def load_scene(self, scene_path): 106 | if self.scene_loaded: 107 | raise RuntimeError('Scene is already loaded.') 108 | self.RAPI_rc(vrep.simxLoadScene(self.cID,scene_path,0, vrep.simx_opmode_blocking)) 109 | self.scene_loaded = True 110 | 111 | def close_scene(self): 112 | if not self.scene_loaded: 113 | raise RuntimeError('Scene is not loaded.') 114 | self.RAPI_rc(vrep.simxCloseScene(self.cID, vrep.simx_opmode_blocking)) 115 | self.scene_loaded = False 116 | 117 | def start_simulation(self): 118 | if self.sim_running: 119 | raise RuntimeError('Simulation is already running.') 120 | 121 | # Optionally override physics engine ( 0=Bullet, 1=ODE, 2=Vortex, 3=Newton ) 122 | #self.set_integer_parameter(vrep.sim_intparam_dynamic_engine, 0) # 0=Bullet 123 | 124 | # Optionally override delta time 125 | #self.set_float_parameter(vrep.sim_floatparam_simulation_time_step, 25) 126 | 127 | self.RAPI_rc(vrep.simxSynchronous(self.cID,True)) 128 | self.RAPI_rc(vrep.simxStartSimulation(self.cID, vrep.simx_opmode_blocking)) 129 | 130 | # Enable Threaded Rendering for faster simulation 131 | if not self.is_headless: 132 | self.set_boolean_parameter(vrep.sim_boolparam_threaded_rendering_enabled,True) 133 | 134 | self.sim_running = True 135 | 136 | def stop_simulation(self): 137 | if not self.sim_running: 138 | raise RuntimeError('Simulation is not running.') 139 | 140 | self.RAPI_rc(vrep.simxStopSimulation(self.cID, vrep.simx_opmode_blocking)) 141 | 142 | # Checking if the server really stopped 143 | try: 144 | while True: 145 | self.RAPI_rc(vrep.simxGetIntegerSignal(self.cID,'sig_debug',vrep.simx_opmode_blocking)) 146 | e = vrep.simxGetInMessageInfo(self.cID,vrep.simx_headeroffset_server_state) 147 | still_running = e[1] & 1 148 | if not still_running: 149 | break 150 | except: pass 151 | self.sim_running = False 152 | 153 | def step_simulation(self): 154 | self.RAPI_rc(vrep.simxSynchronousTrigger(self.cID)) 155 | 156 | # Below are all wrapped methods unrelated to connection/scene 157 | 158 | # misc methods 159 | 160 | def add_statusbar_message(self, message): 161 | self.RAPI_rc(vrep.simxAddStatusbarMessage(self.cID, message, vrep.simx_opmode_blocking)) 162 | 163 | # object methods 164 | 165 | def get_object_handle(self, name): 166 | handle, = self.RAPI_rc(vrep.simxGetObjectHandle(self.cID, name, vrep.simx_opmode_blocking)) 167 | return handle 168 | 169 | # "getters" 170 | 171 | def obj_get_position(self, handle, relative_to=None): 172 | position, = self.RAPI_rc(vrep.simxGetObjectPosition( self.cID,handle, 173 | -1 if relative_to is None else relative_to, 174 | self.opM_get)) 175 | return position 176 | def obj_get_orientation(self, handle, relative_to=None): 177 | eulerAngles, = self.RAPI_rc(vrep.simxGetObjectOrientation( self.cID,handle, 178 | -1 if relative_to is None else relative_to, 179 | self.opM_get)) 180 | return eulerAngles 181 | def obj_get_orientation_continuous(self, handle, relative_to=None): 182 | ea = self.obj_get_orientation(handle,relative_to) 183 | return [ 184 | np.sin(ea[0]),np.cos(ea[0]), 185 | np.sin(ea[1]),np.cos(ea[1]), 186 | np.sin(ea[2]),np.cos(ea[2])] 187 | 188 | # (linearVel, angularVel) 189 | def obj_get_velocity(self, handle): 190 | return self.RAPI_rc(vrep.simxGetObjectVelocity( self.cID,handle, 191 | self.opM_get)) 192 | def obj_get_joint_angle(self, handle): 193 | angle, = self.RAPI_rc(vrep.simxGetJointPosition( self.cID,handle, 194 | self.opM_get)) 195 | #return -np.rad2deg(angle[0]) 196 | return angle 197 | def obj_get_joint_angle_continuous(self, handle): 198 | rad = self.obj_get_joint_angle(handle) 199 | return [np.sin(rad),np.cos(rad)] 200 | def obj_get_joint_force(self, handle): 201 | force = self.RAPI_rc(vrep.simxGetJointForce( self.cID,handle, 202 | self.opM_get)) 203 | return force 204 | def obj_read_force_sensor(self, handle): 205 | state, forceVector, torqueVector = self.RAPI_rc(vrep.simxReadForceSensor( self.cID,handle, 206 | self.opM_get)) 207 | if state & 1 != 1: # bit 0 not set 208 | return None # sensor data not (yet) available 209 | elif state & 2 == 1: # bit 1 set 210 | return 0 # force sensor is broken 211 | else: 212 | return forceVector, torqueVector 213 | def obj_get_vision_image(self, handle): 214 | resolution, image = self.RAPI_rc(vrep.simxGetVisionSensorImage( self.cID,handle, 215 | 0, # assume RGB 216 | self.opM_get,)) 217 | dim, im = resolution, image 218 | nim = np.array(im, dtype='uint8') 219 | nim = np.reshape(nim, (dim[1], dim[0], 3)) 220 | nim = np.flip(nim, 0) # horizontal flip 221 | #nim = np.flip(nim, 2) # RGB -> BGR 222 | return nim 223 | 224 | # "setters" 225 | 226 | def obj_set_position_target(self, handle, angle): 227 | return self.RAPI_rc(vrep.simxSetJointTargetPosition( self.cID,handle, 228 | -np.deg2rad(angle), 229 | self.opM_set)) 230 | def obj_set_velocity(self, handle, v): 231 | return self.RAPI_rc(vrep.simxSetJointTargetVelocity( self.cID,handle, 232 | v, 233 | self.opM_set)) 234 | def obj_set_force(self, handle, f): 235 | return self.RAPI_rc(vrep.simxSetJointForce( self.cID,handle, 236 | f, 237 | self.opM_set)) 238 | def obj_set_position(self, handle, pos, relative_to=None): 239 | return self.RAPI_rc(vrep.simxSetObjectPosition( self.cID,handle, 240 | -1 if relative_to is None else relative_to, 241 | pos, 242 | self.opM_set)) 243 | def obj_set_orientation(self, handle, eulerAngles, relative_to=None): 244 | return self.RAPI_rc(vrep.simxSetObjectOrientation( self.cID,handle, 245 | -1 if relative_to is None else relative_to, 246 | eulerAngles, 247 | self.opM_set)) 248 | # collisions 249 | 250 | def get_collision_handle(self, name): 251 | handle, = self.RAPI_rc(vrep.simxGetCollisionHandle(self.cID, name, vrep.simx_opmode_blocking)) 252 | return handle 253 | def read_collision(self, handle): 254 | collisionState, = self.RAPI_rc(vrep.simxReadCollision( self.cID,handle, 255 | self.opM_get)) 256 | return collisionState 257 | 258 | # signals 259 | 260 | def set_integer_signal(self, sig_name, sig_val): 261 | return self.RAPI_rc(vrep.simxSetIntegerSignal( self.cID, 262 | sig_name, sig_val, 263 | self.opM_set)) 264 | def set_float_signal(self, sig_name, sig_val): 265 | return self.RAPI_rc(vrep.SetFloatSignal( self.cID, 266 | sig_name, sig_val, 267 | self.opM_set)) 268 | def set_string_signal(self, sig_name, sig_val): 269 | return self.RAPI_rc(vrep.SetStringSignal( self.cID, 270 | sig_name, sig_val, 271 | self.opM_set)) 272 | 273 | def get_integer_signal(self, sig_name): 274 | return self.RAPI_rc(vrep.simxGetIntegerSignal( self.cID, 275 | sig_name, 276 | self.opM_get)) 277 | def get_float_signal(self, sig_name): 278 | return self.RAPI_rc(vrep.simxGetFloatSignal( self.cID, 279 | sig_name, 280 | self.opM_get)) 281 | def get_string_signal(self, sig_name): 282 | return self.RAPI_rc(vrep.simxGetStringSignal( self.cID, 283 | sig_name, 284 | self.opM_get)) 285 | 286 | # parameters 287 | 288 | def set_boolean_parameter(self, param_id, param_val): 289 | return self.RAPI_rc(vrep.simxSetBooleanParameter( self.cID, 290 | param_id, param_val, 291 | vrep.simx_opmode_blocking)) 292 | def set_integer_parameter(self, param_id, param_val): 293 | return self.RAPI_rc(vrep.simxSetIntegerParameter( self.cID, 294 | param_id, param_val, 295 | vrep.simx_opmode_blocking)) 296 | def set_float_parameter(self, param_id, param_val): 297 | return self.RAPI_rc(vrep.simxSetFloatingParameter( self.cID, 298 | param_id, param_val, 299 | vrep.simx_opmode_blocking)) 300 | def set_array_parameter(self, param_id, param_val): 301 | return self.RAPI_rc(vrep.simxSetArrayParameter( self.cID, 302 | param_id, param_val, 303 | vrep.simx_opmode_blocking)) 304 | 305 | def get_boolean_parameter(self, param_id): 306 | return self.RAPI_rc(vrep.simxGetBooleanParameter( self.cID, 307 | param_id, 308 | vrep.simx_opmode_blocking))[0] 309 | def get_integer_parameter(self, param_id): 310 | return self.RAPI_rc(vrep.simxGetIntegerParameter( self.cID, 311 | param_id, 312 | vrep.simx_opmode_blocking))[0] 313 | def get_float_parameter(self, param_id): 314 | return self.RAPI_rc(vrep.simxGetFloatingParameter( self.cID, 315 | param_id, 316 | vrep.simx_opmode_blocking))[0] 317 | def get_array_parameter(self, param_id): 318 | return self.RAPI_rc(vrep.simxGetArrayParameter( self.cID, 319 | param_id, 320 | vrep.simx_opmode_blocking))[0] 321 | 322 | # scripts 323 | # child scripts 324 | def call_childscript_function(self,obj_name,func_name,in_tuple): 325 | return self.RAPI_rc(vrep.simxCallScriptFunction(self.cID, 326 | obj_name,vrep.sim_scripttype_childscript,func_name, 327 | in_tuple[0],in_tuple[1],in_tuple[2],in_tuple[3], 328 | vrep.simx_opmode_blocking)) 329 | 330 | # openai/gym 331 | 332 | # Set this in SOME subclasses 333 | #metadata = {'render.modes': []} 334 | #reward_range = (-np.inf, np.inf) 335 | 336 | # Override in SOME subclasses 337 | #def _close(self): pass 338 | 339 | # Set these in ALL subclasses 340 | #action_space = None 341 | #observation_space = None 342 | 343 | # Override in ALL subclasses 344 | #def _step(self, action): raise NotImplementedError 345 | #def _reset(self): raise NotImplementedError 346 | #def _render(self, mode='human', close=False): return 347 | #def _seed(self, seed=None): return [] 348 | 349 | #def _close(self): 350 | def close(self): 351 | if self.sim_running: 352 | self.stop_simulation() 353 | # Closing the scene is unnecessary 354 | #if self.scene_loaded: 355 | # self.close_scene() 356 | if self.connected: 357 | self.disconnect() 358 | 359 | -------------------------------------------------------------------------------- /vrep_env/vrep.py: -------------------------------------------------------------------------------- 1 | # This file is part of the REMOTE API 2 | # 3 | # Copyright 2006-2017 Coppelia Robotics GmbH. All rights reserved. 4 | # marc@coppeliarobotics.com 5 | # www.coppeliarobotics.com 6 | # 7 | # The REMOTE API is licensed under the terms of GNU GPL: 8 | # 9 | # ------------------------------------------------------------------- 10 | # The REMOTE API is free software: you can redistribute it and/or modify 11 | # it under the terms of the GNU General Public License as published by 12 | # the Free Software Foundation, either version 3 of the License, or 13 | # (at your option) any later version. 14 | # 15 | # THE REMOTE API IS DISTRIBUTED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED 16 | # WARRANTY. THE USER WILL USE IT AT HIS/HER OWN RISK. THE ORIGINAL 17 | # AUTHORS AND COPPELIA ROBOTICS GMBH WILL NOT BE LIABLE FOR DATA LOSS, 18 | # DAMAGES, LOSS OF PROFITS OR ANY OTHER KIND OF LOSS WHILE USING OR 19 | # MISUSING THIS SOFTWARE. 20 | # 21 | # See the GNU General Public License for more details. 22 | # 23 | # You should have received a copy of the GNU General Public License 24 | # along with the REMOTE API. If not, see . 25 | # ------------------------------------------------------------------- 26 | # 27 | # This file was automatically created for V-REP release V3.4.0 rev. 1 on April 5th 2017 28 | 29 | import platform 30 | import struct 31 | import sys 32 | import os 33 | import ctypes as ct 34 | 35 | # from vrepConst import * 36 | 37 | # vrepConst.py 38 | #Scene object types. Values are serialized 39 | sim_object_shape_type =0 40 | sim_object_joint_type =1 41 | sim_object_graph_type =2 42 | sim_object_camera_type =3 43 | sim_object_dummy_type =4 44 | sim_object_proximitysensor_type =5 45 | sim_object_reserved1 =6 46 | sim_object_reserved2 =7 47 | sim_object_path_type =8 48 | sim_object_visionsensor_type =9 49 | sim_object_volume_type =10 50 | sim_object_mill_type =11 51 | sim_object_forcesensor_type =12 52 | sim_object_light_type =13 53 | sim_object_mirror_type =14 54 | 55 | #General object types. Values are serialized 56 | sim_appobj_object_type =109 57 | sim_appobj_collision_type =110 58 | sim_appobj_distance_type =111 59 | sim_appobj_simulation_type =112 60 | sim_appobj_ik_type =113 61 | sim_appobj_constraintsolver_type=114 62 | sim_appobj_collection_type =115 63 | sim_appobj_ui_type =116 64 | sim_appobj_script_type =117 65 | sim_appobj_pathplanning_type =118 66 | sim_appobj_RESERVED_type =119 67 | sim_appobj_texture_type =120 68 | 69 | # Ik calculation methods. Values are serialized 70 | sim_ik_pseudo_inverse_method =0 71 | sim_ik_damped_least_squares_method =1 72 | sim_ik_jacobian_transpose_method =2 73 | 74 | # Ik constraints. Values are serialized 75 | sim_ik_x_constraint =1 76 | sim_ik_y_constraint =2 77 | sim_ik_z_constraint =4 78 | sim_ik_alpha_beta_constraint=8 79 | sim_ik_gamma_constraint =16 80 | sim_ik_avoidance_constraint =64 81 | 82 | # Ik calculation results 83 | sim_ikresult_not_performed =0 84 | sim_ikresult_success =1 85 | sim_ikresult_fail =2 86 | 87 | # Scene object sub-types. Values are serialized 88 | # Light sub-types 89 | sim_light_omnidirectional_subtype =1 90 | sim_light_spot_subtype =2 91 | sim_light_directional_subtype =3 92 | # Joint sub-types 93 | sim_joint_revolute_subtype =10 94 | sim_joint_prismatic_subtype =11 95 | sim_joint_spherical_subtype =12 96 | # Shape sub-types 97 | sim_shape_simpleshape_subtype =20 98 | sim_shape_multishape_subtype =21 99 | # Proximity sensor sub-types 100 | sim_proximitysensor_pyramid_subtype =30 101 | sim_proximitysensor_cylinder_subtype=31 102 | sim_proximitysensor_disc_subtype =32 103 | sim_proximitysensor_cone_subtype =33 104 | sim_proximitysensor_ray_subtype =34 105 | # Mill sub-types 106 | sim_mill_pyramid_subtype =40 107 | sim_mill_cylinder_subtype =41 108 | sim_mill_disc_subtype =42 109 | sim_mill_cone_subtype =42 110 | # No sub-type 111 | sim_object_no_subtype =200 112 | 113 | 114 | #Scene object main properties (serialized) 115 | sim_objectspecialproperty_collidable =0x0001 116 | sim_objectspecialproperty_measurable =0x0002 117 | #reserved =0x0004 118 | #reserved =0x0008 119 | sim_objectspecialproperty_detectable_ultrasonic =0x0010 120 | sim_objectspecialproperty_detectable_infrared =0x0020 121 | sim_objectspecialproperty_detectable_laser =0x0040 122 | sim_objectspecialproperty_detectable_inductive =0x0080 123 | sim_objectspecialproperty_detectable_capacitive =0x0100 124 | sim_objectspecialproperty_renderable =0x0200 125 | sim_objectspecialproperty_detectable_all =sim_objectspecialproperty_detectable_ultrasonic|sim_objectspecialproperty_detectable_infrared|sim_objectspecialproperty_detectable_laser|sim_objectspecialproperty_detectable_inductive|sim_objectspecialproperty_detectable_capacitive 126 | sim_objectspecialproperty_cuttable =0x0400 127 | sim_objectspecialproperty_pathplanning_ignored =0x0800 128 | 129 | # Model properties (serialized) 130 | sim_modelproperty_not_collidable =0x0001 131 | sim_modelproperty_not_measurable =0x0002 132 | sim_modelproperty_not_renderable =0x0004 133 | sim_modelproperty_not_detectable =0x0008 134 | sim_modelproperty_not_cuttable =0x0010 135 | sim_modelproperty_not_dynamic =0x0020 136 | sim_modelproperty_not_respondable =0x0040 # cannot be selected if sim_modelproperty_not_dynamic is not selected 137 | sim_modelproperty_not_reset =0x0080 # Model is not reset at simulation end. This flag is cleared at simulation end 138 | sim_modelproperty_not_visible =0x0100 # Whole model is invisible independent of local visibility settings 139 | sim_modelproperty_not_model =0xf000 # object is not a model 140 | 141 | 142 | # Check the documentation instead of comments below!! 143 | # Following messages are dispatched to the Lua-message container 144 | 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) 145 | sim_message_reserved9 =1 # Do not use 146 | sim_message_object_selection_changed=2 147 | sim_message_reserved10 =3 # do not use 148 | sim_message_model_loaded =4 149 | sim_message_reserved11 =5 # do not use 150 | 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) 151 | sim_message_bannerclicked =7 # a banner was clicked (aux[0]=banner ID) 152 | 153 | 154 | # Following messages are dispatched only to the C-API (not available from Lua) 155 | sim_message_for_c_api_only_start =0x100 # Do not use 156 | sim_message_reserved1 =0x101 # Do not use 157 | sim_message_reserved2 =0x102 # Do not use 158 | sim_message_reserved3 =0x103 # Do not use 159 | sim_message_eventcallback_scenesave =0x104 # about to save a scene 160 | sim_message_eventcallback_modelsave =0x105 # about to save a model (current selection will be saved) 161 | sim_message_eventcallback_moduleopen =0x106 # called when simOpenModule in Lua is called 162 | sim_message_eventcallback_modulehandle =0x107 # called when simHandleModule in Lua is called with argument false 163 | sim_message_eventcallback_moduleclose =0x108 # called when simCloseModule in Lua is called 164 | sim_message_reserved4 =0x109 # Do not use 165 | sim_message_reserved5 =0x10a # Do not use 166 | sim_message_reserved6 =0x10b # Do not use 167 | sim_message_reserved7 =0x10c # Do not use 168 | sim_message_eventcallback_instancepass =0x10d # Called once every main application loop pass. auxiliaryData[0] contains event flags of events that happened since last time 169 | sim_message_eventcallback_broadcast =0x10e 170 | sim_message_eventcallback_imagefilter_enumreset =0x10f 171 | sim_message_eventcallback_imagefilter_enumerate =0x110 172 | sim_message_eventcallback_imagefilter_adjustparams =0x111 173 | sim_message_eventcallback_imagefilter_reserved =0x112 174 | sim_message_eventcallback_imagefilter_process =0x113 175 | sim_message_eventcallback_reserved1 =0x114 # do not use 176 | sim_message_eventcallback_reserved2 =0x115 # do not use 177 | sim_message_eventcallback_reserved3 =0x116 # do not use 178 | sim_message_eventcallback_reserved4 =0x117 # do not use 179 | sim_message_eventcallback_abouttoundo =0x118 # the undo button was hit and a previous state is about to be restored 180 | sim_message_eventcallback_undoperformed =0x119 # the undo button was hit and a previous state restored 181 | sim_message_eventcallback_abouttoredo =0x11a # the redo button was hit and a future state is about to be restored 182 | sim_message_eventcallback_redoperformed =0x11b # the redo button was hit and a future state restored 183 | 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) 184 | sim_message_eventcallback_simulationabouttostart =0x11d 185 | sim_message_eventcallback_simulationended =0x11e 186 | sim_message_eventcallback_reserved5 =0x11f # do not use 187 | 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) 188 | sim_message_eventcallback_modulehandleinsensingpart =0x121 # called when simHandleModule in Lua is called with argument true 189 | sim_message_eventcallback_renderingpass =0x122 # called just before the scene is rendered 190 | sim_message_eventcallback_bannerclicked =0x123 # called when a banner was clicked (aux[0]=banner ID) 191 | sim_message_eventcallback_menuitemselected =0x124 # auxiliaryData[0] indicates the handle of the item auxiliaryData[1] indicates the state of the item 192 | sim_message_eventcallback_refreshdialogs =0x125 # aux[0]=refresh degree (0=light 1=medium 2=full) 193 | sim_message_eventcallback_sceneloaded =0x126 194 | sim_message_eventcallback_modelloaded =0x127 195 | sim_message_eventcallback_instanceswitch =0x128 196 | sim_message_eventcallback_guipass =0x129 197 | sim_message_eventcallback_mainscriptabouttobecalled =0x12a 198 | sim_message_eventcallback_rmlposition =0x12b #the command simRMLPosition was called. The appropriate plugin should handle the call 199 | sim_message_eventcallback_rmlvelocity =0x12c # the command simRMLVelocity was called. The appropriate plugin should handle the call 200 | sim_message_simulation_start_resume_request =0x1000 201 | sim_message_simulation_pause_request =0x1001 202 | sim_message_simulation_stop_request =0x1002 203 | 204 | # Scene object properties. Combine with the | operator 205 | sim_objectproperty_reserved1 =0x0000 206 | sim_objectproperty_reserved2 =0x0001 207 | sim_objectproperty_reserved3 =0x0002 208 | sim_objectproperty_reserved4 =0x0003 209 | sim_objectproperty_reserved5 =0x0004 # formely sim_objectproperty_visible 210 | sim_objectproperty_reserved6 =0x0008 # formely sim_objectproperty_wireframe 211 | sim_objectproperty_collapsed =0x0010 212 | sim_objectproperty_selectable =0x0020 213 | sim_objectproperty_reserved7 =0x0040 214 | sim_objectproperty_selectmodelbaseinstead =0x0080 215 | sim_objectproperty_dontshowasinsidemodel =0x0100 216 | # reserved =0x0200 217 | sim_objectproperty_canupdatedna =0x0400 218 | sim_objectproperty_selectinvisible =0x0800 219 | sim_objectproperty_depthinvisible =0x1000 220 | 221 | 222 | # type of arguments (input and output) for custom lua commands 223 | sim_lua_arg_nil =0 224 | sim_lua_arg_bool =1 225 | sim_lua_arg_int =2 226 | sim_lua_arg_float =3 227 | sim_lua_arg_string =4 228 | sim_lua_arg_invalid =5 229 | sim_lua_arg_table =8 230 | 231 | # custom user interface properties. Values are serialized. 232 | sim_ui_property_visible =0x0001 233 | sim_ui_property_visibleduringsimulationonly =0x0002 234 | sim_ui_property_moveable =0x0004 235 | sim_ui_property_relativetoleftborder =0x0008 236 | sim_ui_property_relativetotopborder =0x0010 237 | sim_ui_property_fixedwidthfont =0x0020 238 | sim_ui_property_systemblock =0x0040 239 | sim_ui_property_settocenter =0x0080 240 | sim_ui_property_rolledup =0x0100 241 | sim_ui_property_selectassociatedobject =0x0200 242 | sim_ui_property_visiblewhenobjectselected =0x0400 243 | 244 | 245 | # button properties. Values are serialized. 246 | sim_buttonproperty_button =0x0000 247 | sim_buttonproperty_label =0x0001 248 | sim_buttonproperty_slider =0x0002 249 | sim_buttonproperty_editbox =0x0003 250 | sim_buttonproperty_staydown =0x0008 251 | sim_buttonproperty_enabled =0x0010 252 | sim_buttonproperty_borderless =0x0020 253 | sim_buttonproperty_horizontallycentered =0x0040 254 | sim_buttonproperty_ignoremouse =0x0080 255 | sim_buttonproperty_isdown =0x0100 256 | sim_buttonproperty_transparent =0x0200 257 | sim_buttonproperty_nobackgroundcolor =0x0400 258 | sim_buttonproperty_rollupaction =0x0800 259 | sim_buttonproperty_closeaction =0x1000 260 | sim_buttonproperty_verticallycentered =0x2000 261 | sim_buttonproperty_downupevent =0x4000 262 | 263 | 264 | # Simulation status 265 | sim_simulation_stopped =0x00 # Simulation is stopped 266 | sim_simulation_paused =0x08 # Simulation is paused 267 | sim_simulation_advancing =0x10 # Simulation is advancing 268 | sim_simulation_advancing_firstafterstop =sim_simulation_advancing|0x00 # First simulation pass (1x) 269 | sim_simulation_advancing_running =sim_simulation_advancing|0x01 # Normal simulation pass (>=1x) 270 | # reserved =sim_simulation_advancing|0x02 271 | sim_simulation_advancing_lastbeforepause =sim_simulation_advancing|0x03 # Last simulation pass before pause (1x) 272 | sim_simulation_advancing_firstafterpause =sim_simulation_advancing|0x04 # First simulation pass after pause (1x) 273 | sim_simulation_advancing_abouttostop =sim_simulation_advancing|0x05 # "Trying to stop" simulation pass (>=1x) 274 | sim_simulation_advancing_lastbeforestop =sim_simulation_advancing|0x06 # Last simulation pass (1x) 275 | 276 | 277 | # Script execution result (first return value) 278 | sim_script_no_error =0 279 | sim_script_main_script_nonexistent =1 280 | sim_script_main_script_not_called =2 281 | sim_script_reentrance_error =4 282 | sim_script_lua_error =8 283 | sim_script_call_error =16 284 | 285 | 286 | # Script types (serialized!) 287 | sim_scripttype_mainscript =0 288 | sim_scripttype_childscript =1 289 | sim_scripttype_jointctrlcallback =4 290 | sim_scripttype_contactcallback =5 291 | sim_scripttype_customizationscript =6 292 | sim_scripttype_generalcallback =7 293 | 294 | # API call error messages 295 | sim_api_errormessage_ignore =0 # does not memorize nor output errors 296 | sim_api_errormessage_report =1 # memorizes errors (default for C-API calls) 297 | sim_api_errormessage_output =2 # memorizes and outputs errors (default for Lua-API calls) 298 | 299 | 300 | # special argument of some functions 301 | sim_handle_all =-2 302 | sim_handle_all_except_explicit =-3 303 | sim_handle_self =-4 304 | sim_handle_main_script =-5 305 | sim_handle_tree =-6 306 | sim_handle_chain =-7 307 | sim_handle_single =-8 308 | sim_handle_default =-9 309 | sim_handle_all_except_self =-10 310 | sim_handle_parent =-11 311 | 312 | 313 | # special handle flags 314 | sim_handleflag_assembly =0x400000 315 | sim_handleflag_model =0x800000 316 | 317 | 318 | # distance calculation methods (serialized) 319 | sim_distcalcmethod_dl =0 320 | sim_distcalcmethod_dac =1 321 | sim_distcalcmethod_max_dl_dac =2 322 | sim_distcalcmethod_dl_and_dac =3 323 | sim_distcalcmethod_sqrt_dl2_and_dac2=4 324 | sim_distcalcmethod_dl_if_nonzero =5 325 | sim_distcalcmethod_dac_if_nonzero =6 326 | 327 | 328 | # Generic dialog styles 329 | sim_dlgstyle_message =0 330 | sim_dlgstyle_input =1 331 | sim_dlgstyle_ok =2 332 | sim_dlgstyle_ok_cancel =3 333 | sim_dlgstyle_yes_no =4 334 | 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 335 | 336 | # Generic dialog return values 337 | sim_dlgret_still_open =0 338 | sim_dlgret_ok =1 339 | sim_dlgret_cancel =2 340 | sim_dlgret_yes =3 341 | sim_dlgret_no =4 342 | 343 | 344 | # Path properties 345 | sim_pathproperty_show_line =0x0001 346 | sim_pathproperty_show_orientation =0x0002 347 | sim_pathproperty_closed_path =0x0004 348 | sim_pathproperty_automatic_orientation =0x0008 349 | sim_pathproperty_invert_velocity =0x0010 350 | sim_pathproperty_infinite_acceleration =0x0020 351 | sim_pathproperty_flat_path =0x0040 352 | sim_pathproperty_show_position =0x0080 353 | sim_pathproperty_auto_velocity_profile_translation =0x0100 354 | sim_pathproperty_auto_velocity_profile_rotation =0x0200 355 | sim_pathproperty_endpoints_at_zero =0x0400 356 | sim_pathproperty_keep_x_up =0x0800 357 | 358 | 359 | # drawing objects 360 | # following are mutually exclusive 361 | sim_drawing_points =0 # 3 values per point (point size in pixels) 362 | sim_drawing_lines =1 # 6 values per line (line size in pixels) 363 | sim_drawing_triangles =2 # 9 values per triangle 364 | sim_drawing_trianglepoints =3 # 6 values per point (3 for triangle position 3 for triangle normal vector) (triangle size in meters) 365 | sim_drawing_quadpoints =4 # 6 values per point (3 for quad position 3 for quad normal vector) (quad size in meters) 366 | sim_drawing_discpoints =5 # 6 values per point (3 for disc position 3 for disc normal vector) (disc size in meters) 367 | sim_drawing_cubepoints =6 # 6 values per point (3 for cube position 3 for cube normal vector) (cube size in meters) 368 | sim_drawing_spherepoints =7 # 3 values per point (sphere size in meters) 369 | 370 | # following can be or-combined 371 | sim_drawing_itemcolors =0x00020 # +3 values per item (each item has its own ambient color (rgb values)). 372 | # Mutually exclusive with sim_drawing_vertexcolors 373 | 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 374 | sim_drawing_itemsizes =0x00080 # +1 value per item (each item has its own size). Not for sim_drawing_triangles 375 | sim_drawing_backfaceculling =0x00100 # back faces are not displayed for all items 376 | sim_drawing_wireframe =0x00200 # all items displayed in wireframe 377 | sim_drawing_painttag =0x00400 # all items are tagged as paint (for additinal processing at a later stage) 378 | sim_drawing_followparentvisibility =0x00800 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible 379 | sim_drawing_cyclic =0x01000 # if the max item count was reached then the first items are overwritten. 380 | sim_drawing_50percenttransparency =0x02000 # the drawing object will be 50% transparent 381 | sim_drawing_25percenttransparency =0x04000 # the drawing object will be 25% transparent 382 | sim_drawing_12percenttransparency =0x08000 # the drawing object will be 12.5% transparent 383 | 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 384 | 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) 385 | sim_drawing_overlay =0x40000 # When specified objects are always drawn on top of "regular objects" 386 | sim_drawing_itemtransparency =0x80000 # +1 value per item (each item has its own transparency value (0-1)). Not compatible with sim_drawing_vertexcolors 387 | 388 | # banner values 389 | # following can be or-combined 390 | sim_banner_left =0x00001 # Banners display on the left of the specified point 391 | sim_banner_right =0x00002 # Banners display on the right of the specified point 392 | sim_banner_nobackground =0x00004 # Banners have no background rectangle 393 | sim_banner_overlay =0x00008 # When specified banners are always drawn on top of "regular objects" 394 | sim_banner_followparentvisibility =0x00010 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible 395 | sim_banner_clickselectsparent =0x00020 # if the object is associated with a scene object then clicking the banner will select the scene object 396 | sim_banner_clicktriggersevent =0x00040 # if the banner is clicked an event is triggered (sim_message_eventcallback_bannerclicked and sim_message_bannerclicked are generated) 397 | sim_banner_facingcamera =0x00080 # If specified the banner will always face the camera by rotating around the banner's vertical axis (y-axis) 398 | 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) 399 | sim_banner_backfaceculling =0x00200 # If specified the banner will only be visible from one side 400 | 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 401 | 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 402 | # to the specified position. Bitmap fonts are not clickable 403 | 404 | 405 | # particle objects following are mutually exclusive 406 | sim_particle_points1 =0 # 6 values per point (pt1 and pt2. Pt1 is start position pt2-pt1 is the initial velocity vector). i 407 | #Point is 1 pixel big. Only appearance is a point internally handled as a perfect sphere 408 | sim_particle_points2 =1 # 6 values per point. Point is 2 pixel big. Only appearance is a point internally handled as a perfect sphere 409 | sim_particle_points4 =2 # 6 values per point. Point is 4 pixel big. Only appearance is a point internally handled as a perfect sphere 410 | sim_particle_roughspheres =3 # 6 values per sphere. Only appearance is rough. Internally a perfect sphere 411 | sim_particle_spheres =4 # 6 values per sphere. Internally a perfect sphere 412 | 413 | 414 | 415 | 416 | # following can be or-combined 417 | 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) 418 | 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) 419 | sim_particle_particlerespondable =0x0080 # the particles are respondable against each other 420 | sim_particle_ignoresgravity =0x0100 # the particles ignore the effect of gravity. Not compatible with sim_particle_water 421 | sim_particle_invisible =0x0200 # the particles are invisible 422 | sim_particle_itemsizes =0x0400 # +1 value per particle (each particle can have a different size) 423 | sim_particle_itemdensities =0x0800 # +1 value per particle (each particle can have a different density) 424 | sim_particle_itemcolors =0x1000 # +3 values per particle (each particle can have a different color) 425 | sim_particle_cyclic =0x2000 # if the max item count was reached then the first items are overwritten. 426 | sim_particle_emissioncolor =0x4000 # When used in combination with sim_particle_itemcolors then the specified colors will be for the emissive component 427 | 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 428 | sim_particle_painttag =0x10000 # The particles can be seen by vision sensors (sim_particle_invisible must not be set) 429 | 430 | 431 | 432 | 433 | # custom user interface menu attributes 434 | sim_ui_menu_title =1 435 | sim_ui_menu_minimize =2 436 | sim_ui_menu_close =4 437 | sim_ui_menu_systemblock =8 438 | 439 | 440 | 441 | # Boolean parameters 442 | sim_boolparam_hierarchy_visible =0 443 | sim_boolparam_console_visible =1 444 | sim_boolparam_collision_handling_enabled =2 445 | sim_boolparam_distance_handling_enabled =3 446 | sim_boolparam_ik_handling_enabled =4 447 | sim_boolparam_gcs_handling_enabled =5 448 | sim_boolparam_dynamics_handling_enabled =6 449 | sim_boolparam_joint_motion_handling_enabled =7 450 | sim_boolparam_path_motion_handling_enabled =8 451 | sim_boolparam_proximity_sensor_handling_enabled =9 452 | sim_boolparam_vision_sensor_handling_enabled =10 453 | sim_boolparam_mill_handling_enabled =11 454 | sim_boolparam_browser_visible =12 455 | sim_boolparam_scene_and_model_load_messages =13 456 | sim_reserved0 =14 457 | sim_boolparam_shape_textures_are_visible =15 458 | sim_boolparam_display_enabled =16 459 | sim_boolparam_infotext_visible =17 460 | sim_boolparam_statustext_open =18 461 | sim_boolparam_fog_enabled =19 462 | sim_boolparam_rml2_available =20 463 | sim_boolparam_rml4_available =21 464 | sim_boolparam_mirrors_enabled =22 465 | sim_boolparam_aux_clip_planes_enabled =23 466 | sim_boolparam_full_model_copy_from_api =24 467 | sim_boolparam_realtime_simulation =25 468 | sim_boolparam_force_show_wireless_emission =27 469 | sim_boolparam_force_show_wireless_reception =28 470 | sim_boolparam_video_recording_triggered =29 471 | sim_boolparam_threaded_rendering_enabled =32 472 | sim_boolparam_fullscreen =33 473 | sim_boolparam_headless =34 474 | sim_boolparam_hierarchy_toolbarbutton_enabled =35 475 | sim_boolparam_browser_toolbarbutton_enabled =36 476 | sim_boolparam_objectshift_toolbarbutton_enabled =37 477 | sim_boolparam_objectrotate_toolbarbutton_enabled=38 478 | sim_boolparam_force_calcstruct_all_visible =39 479 | sim_boolparam_force_calcstruct_all =40 480 | sim_boolparam_exit_request =41 481 | sim_boolparam_play_toolbarbutton_enabled =42 482 | sim_boolparam_pause_toolbarbutton_enabled =43 483 | sim_boolparam_stop_toolbarbutton_enabled =44 484 | sim_boolparam_waiting_for_trigger =45 485 | 486 | 487 | # Integer parameters 488 | sim_intparam_error_report_mode =0 # Check sim_api_errormessage_... constants above for valid values 489 | sim_intparam_program_version =1 # e.g Version 2.1.4 --> 20104. Can only be read 490 | sim_intparam_instance_count =2 # do not use anymore (always returns 1 since V-REP 2.5.11) 491 | sim_intparam_custom_cmd_start_id =3 # can only be read 492 | sim_intparam_compilation_version =4 # 0=evaluation version 1=full version 2=player version. Can only be read 493 | sim_intparam_current_page =5 494 | sim_intparam_flymode_camera_handle =6 # can only be read 495 | sim_intparam_dynamic_step_divider =7 # can only be read 496 | sim_intparam_dynamic_engine =8 # 0=Bullet 1=ODE. 2=Vortex. 497 | sim_intparam_server_port_start =9 # can only be read 498 | sim_intparam_server_port_range =10 # can only be read 499 | sim_intparam_visible_layers =11 500 | sim_intparam_infotext_style =12 501 | sim_intparam_settings =13 502 | sim_intparam_edit_mode_type =14 # can only be read 503 | sim_intparam_server_port_next =15 # is initialized at sim_intparam_server_port_start 504 | sim_intparam_qt_version =16 # version of the used Qt framework 505 | sim_intparam_event_flags_read =17 # can only be read 506 | sim_intparam_event_flags_read_clear =18 # can only be read 507 | sim_intparam_platform =19 # can only be read 508 | sim_intparam_scene_unique_id =20 # can only be read 509 | sim_intparam_work_thread_count =21 510 | sim_intparam_mouse_x =22 511 | sim_intparam_mouse_y =23 512 | sim_intparam_core_count =24 513 | sim_intparam_work_thread_calc_time_ms =25 514 | sim_intparam_idle_fps =26 515 | sim_intparam_prox_sensor_select_down =27 516 | sim_intparam_prox_sensor_select_up =28 517 | sim_intparam_stop_request_counter =29 518 | sim_intparam_program_revision =30 519 | sim_intparam_mouse_buttons =31 520 | sim_intparam_dynamic_warning_disabled_mask =32 521 | sim_intparam_simulation_warning_disabled_mask =33 522 | sim_intparam_scene_index =34 523 | sim_intparam_motionplanning_seed =35 524 | sim_intparam_speedmodifier =36 525 | 526 | # Float parameters 527 | sim_floatparam_rand=0 # random value (0.0-1.0) 528 | sim_floatparam_simulation_time_step =1 529 | sim_floatparam_stereo_distance =2 530 | 531 | # String parameters 532 | sim_stringparam_application_path=0 # path of V-REP's executable 533 | sim_stringparam_video_filename=1 534 | sim_stringparam_app_arg1 =2 535 | sim_stringparam_app_arg2 =3 536 | sim_stringparam_app_arg3 =4 537 | sim_stringparam_app_arg4 =5 538 | sim_stringparam_app_arg5 =6 539 | sim_stringparam_app_arg6 =7 540 | sim_stringparam_app_arg7 =8 541 | sim_stringparam_app_arg8 =9 542 | sim_stringparam_app_arg9 =10 543 | sim_stringparam_scene_path_and_name =13 544 | 545 | # Array parameters 546 | sim_arrayparam_gravity =0 547 | sim_arrayparam_fog =1 548 | sim_arrayparam_fog_color =2 549 | sim_arrayparam_background_color1=3 550 | sim_arrayparam_background_color2=4 551 | sim_arrayparam_ambient_light =5 552 | sim_arrayparam_random_euler =6 553 | 554 | sim_objintparam_visibility_layer= 10 555 | sim_objfloatparam_abs_x_velocity= 11 556 | sim_objfloatparam_abs_y_velocity= 12 557 | sim_objfloatparam_abs_z_velocity= 13 558 | sim_objfloatparam_abs_rot_velocity= 14 559 | sim_objfloatparam_objbbox_min_x= 15 560 | sim_objfloatparam_objbbox_min_y= 16 561 | sim_objfloatparam_objbbox_min_z= 17 562 | sim_objfloatparam_objbbox_max_x= 18 563 | sim_objfloatparam_objbbox_max_y= 19 564 | sim_objfloatparam_objbbox_max_z= 20 565 | sim_objfloatparam_modelbbox_min_x= 21 566 | sim_objfloatparam_modelbbox_min_y= 22 567 | sim_objfloatparam_modelbbox_min_z= 23 568 | sim_objfloatparam_modelbbox_max_x= 24 569 | sim_objfloatparam_modelbbox_max_y= 25 570 | sim_objfloatparam_modelbbox_max_z= 26 571 | sim_objintparam_collection_self_collision_indicator= 27 572 | sim_objfloatparam_transparency_offset= 28 573 | sim_objintparam_child_role= 29 574 | sim_objintparam_parent_role= 30 575 | sim_objintparam_manipulation_permissions= 31 576 | sim_objintparam_illumination_handle= 32 577 | 578 | sim_visionfloatparam_near_clipping= 1000 579 | sim_visionfloatparam_far_clipping= 1001 580 | sim_visionintparam_resolution_x= 1002 581 | sim_visionintparam_resolution_y= 1003 582 | sim_visionfloatparam_perspective_angle= 1004 583 | sim_visionfloatparam_ortho_size= 1005 584 | sim_visionintparam_disabled_light_components= 1006 585 | sim_visionintparam_rendering_attributes= 1007 586 | sim_visionintparam_entity_to_render= 1008 587 | sim_visionintparam_windowed_size_x= 1009 588 | sim_visionintparam_windowed_size_y= 1010 589 | sim_visionintparam_windowed_pos_x= 1011 590 | sim_visionintparam_windowed_pos_y= 1012 591 | sim_visionintparam_pov_focal_blur= 1013 592 | sim_visionfloatparam_pov_blur_distance= 1014 593 | sim_visionfloatparam_pov_aperture= 1015 594 | sim_visionintparam_pov_blur_sampled= 1016 595 | sim_visionintparam_render_mode= 1017 596 | 597 | sim_jointintparam_motor_enabled= 2000 598 | sim_jointintparam_ctrl_enabled= 2001 599 | sim_jointfloatparam_pid_p= 2002 600 | sim_jointfloatparam_pid_i= 2003 601 | sim_jointfloatparam_pid_d= 2004 602 | sim_jointfloatparam_intrinsic_x= 2005 603 | sim_jointfloatparam_intrinsic_y= 2006 604 | sim_jointfloatparam_intrinsic_z= 2007 605 | sim_jointfloatparam_intrinsic_qx= 2008 606 | sim_jointfloatparam_intrinsic_qy= 2009 607 | sim_jointfloatparam_intrinsic_qz= 2010 608 | sim_jointfloatparam_intrinsic_qw= 2011 609 | sim_jointfloatparam_velocity= 2012 610 | sim_jointfloatparam_spherical_qx= 2013 611 | sim_jointfloatparam_spherical_qy= 2014 612 | sim_jointfloatparam_spherical_qz= 2015 613 | sim_jointfloatparam_spherical_qw= 2016 614 | sim_jointfloatparam_upper_limit= 2017 615 | sim_jointfloatparam_kc_k= 2018 616 | sim_jointfloatparam_kc_c= 2019 617 | sim_jointfloatparam_ik_weight= 2021 618 | sim_jointfloatparam_error_x= 2022 619 | sim_jointfloatparam_error_y= 2023 620 | sim_jointfloatparam_error_z= 2024 621 | sim_jointfloatparam_error_a= 2025 622 | sim_jointfloatparam_error_b= 2026 623 | sim_jointfloatparam_error_g= 2027 624 | sim_jointfloatparam_error_pos= 2028 625 | sim_jointfloatparam_error_angle= 2029 626 | sim_jointintparam_velocity_lock= 2030 627 | sim_jointintparam_vortex_dep_handle= 2031 628 | sim_jointfloatparam_vortex_dep_multiplication= 2032 629 | sim_jointfloatparam_vortex_dep_offset= 2033 630 | 631 | sim_shapefloatparam_init_velocity_x= 3000 632 | sim_shapefloatparam_init_velocity_y= 3001 633 | sim_shapefloatparam_init_velocity_z= 3002 634 | sim_shapeintparam_static= 3003 635 | sim_shapeintparam_respondable= 3004 636 | sim_shapefloatparam_mass= 3005 637 | sim_shapefloatparam_texture_x= 3006 638 | sim_shapefloatparam_texture_y= 3007 639 | sim_shapefloatparam_texture_z= 3008 640 | sim_shapefloatparam_texture_a= 3009 641 | sim_shapefloatparam_texture_b= 3010 642 | sim_shapefloatparam_texture_g= 3011 643 | sim_shapefloatparam_texture_scaling_x= 3012 644 | sim_shapefloatparam_texture_scaling_y= 3013 645 | sim_shapeintparam_culling= 3014 646 | sim_shapeintparam_wireframe= 3015 647 | sim_shapeintparam_compound= 3016 648 | sim_shapeintparam_convex= 3017 649 | sim_shapeintparam_convex_check= 3018 650 | sim_shapeintparam_respondable_mask= 3019 651 | sim_shapefloatparam_init_velocity_a= 3020 652 | sim_shapefloatparam_init_velocity_b= 3021 653 | sim_shapefloatparam_init_velocity_g= 3022 654 | sim_shapestringparam_color_name= 3023 655 | sim_shapeintparam_edge_visibility= 3024 656 | sim_shapefloatparam_shading_angle= 3025 657 | sim_shapefloatparam_edge_angle= 3026 658 | sim_shapeintparam_edge_borders_hidden= 3027 659 | 660 | sim_proxintparam_ray_invisibility= 4000 661 | 662 | sim_forcefloatparam_error_x= 5000 663 | sim_forcefloatparam_error_y= 5001 664 | sim_forcefloatparam_error_z= 5002 665 | sim_forcefloatparam_error_a= 5003 666 | sim_forcefloatparam_error_b= 5004 667 | sim_forcefloatparam_error_g= 5005 668 | sim_forcefloatparam_error_pos= 5006 669 | sim_forcefloatparam_error_angle= 5007 670 | 671 | sim_lightintparam_pov_casts_shadows= 8000 672 | 673 | sim_cameraintparam_disabled_light_components= 9000 674 | sim_camerafloatparam_perspective_angle= 9001 675 | sim_camerafloatparam_ortho_size= 9002 676 | sim_cameraintparam_rendering_attributes= 9003 677 | sim_cameraintparam_pov_focal_blur= 9004 678 | sim_camerafloatparam_pov_blur_distance= 9005 679 | sim_camerafloatparam_pov_aperture= 9006 680 | sim_cameraintparam_pov_blur_samples= 9007 681 | 682 | sim_dummyintparam_link_type= 10000 683 | 684 | sim_mirrorfloatparam_width= 12000 685 | sim_mirrorfloatparam_height= 12001 686 | sim_mirrorfloatparam_reflectance= 12002 687 | sim_mirrorintparam_enable= 12003 688 | 689 | sim_pplanfloatparam_x_min= 20000 690 | sim_pplanfloatparam_x_range= 20001 691 | sim_pplanfloatparam_y_min= 20002 692 | sim_pplanfloatparam_y_range= 20003 693 | sim_pplanfloatparam_z_min= 20004 694 | sim_pplanfloatparam_z_range= 20005 695 | sim_pplanfloatparam_delta_min= 20006 696 | sim_pplanfloatparam_delta_range= 20007 697 | 698 | sim_mplanintparam_nodes_computed= 25000 699 | sim_mplanintparam_prepare_nodes= 25001 700 | sim_mplanintparam_clear_nodes= 25002 701 | 702 | # User interface elements 703 | sim_gui_menubar =0x0001 704 | sim_gui_popups =0x0002 705 | sim_gui_toolbar1 =0x0004 706 | sim_gui_toolbar2 =0x0008 707 | sim_gui_hierarchy =0x0010 708 | sim_gui_infobar =0x0020 709 | sim_gui_statusbar =0x0040 710 | sim_gui_scripteditor =0x0080 711 | sim_gui_scriptsimulationparameters =0x0100 712 | sim_gui_dialogs =0x0200 713 | sim_gui_browser =0x0400 714 | sim_gui_all =0xffff 715 | 716 | 717 | # Joint modes 718 | sim_jointmode_passive =0 719 | sim_jointmode_motion =1 720 | sim_jointmode_ik =2 721 | sim_jointmode_ikdependent =3 722 | sim_jointmode_dependent =4 723 | sim_jointmode_force =5 724 | 725 | 726 | # Navigation and selection modes with the mouse. Lower byte values are mutually exclusive upper byte bits can be combined 727 | sim_navigation_passive =0x0000 728 | sim_navigation_camerashift =0x0001 729 | sim_navigation_camerarotate =0x0002 730 | sim_navigation_camerazoom =0x0003 731 | sim_navigation_cameratilt =0x0004 732 | sim_navigation_cameraangle =0x0005 733 | sim_navigation_camerafly =0x0006 734 | sim_navigation_objectshift =0x0007 735 | sim_navigation_objectrotate =0x0008 736 | sim_navigation_reserved2 =0x0009 737 | sim_navigation_reserved3 =0x000A 738 | sim_navigation_jointpathtest =0x000B 739 | sim_navigation_ikmanip =0x000C 740 | sim_navigation_objectmultipleselection =0x000D 741 | # Bit-combine following values and add them to one of above's values for a valid navigation mode 742 | sim_navigation_reserved4 =0x0100 743 | sim_navigation_clickselection =0x0200 744 | sim_navigation_ctrlselection =0x0400 745 | sim_navigation_shiftselection =0x0800 746 | sim_navigation_camerazoomwheel =0x1000 747 | sim_navigation_camerarotaterightbutton =0x2000 748 | 749 | 750 | 751 | #Remote API constants 752 | SIMX_VERSION =0 753 | # Remote API message header structure 754 | SIMX_HEADER_SIZE =18 755 | simx_headeroffset_crc =0 # 1 simxUShort. Generated by the client or server. The CRC for the message 756 | simx_headeroffset_version =2 # 1 byte. Generated by the client or server. The version of the remote API software 757 | simx_headeroffset_message_id =3 # 1 simxInt. Generated by the client (and used in a reply by the server) 758 | simx_headeroffset_client_time =7 # 1 simxInt. Client time stamp generated by the client (and sent back by the server) 759 | simx_headeroffset_server_time =11 # 1 simxInt. Generated by the server when a reply is generated. The server timestamp 760 | simx_headeroffset_scene_id =15 # 1 simxUShort. Generated by the server. A unique ID identifying the scene currently displayed 761 | 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) 762 | 763 | # Remote API command header 764 | SIMX_SUBHEADER_SIZE =26 765 | simx_cmdheaderoffset_mem_size =0 # 1 simxInt. Generated by the client or server. The buffer size of the command. 766 | 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). 767 | simx_cmdheaderoffset_pdata_offset0 =8 # 1 simxUShort. Generated by the client or server. The amount of data that is part of the command identification. 768 | 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). 769 | 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. 770 | 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). 771 | 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) 772 | 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 773 | simx_cmdheaderoffset_reserved =25 # 1 byte. Not yet used 774 | 775 | 776 | 777 | 778 | 779 | # Regular operation modes 780 | simx_opmode_oneshot =0x000000 # sends command as one chunk. Reply will also come as one chunk. Doesn't wait for the reply. 781 | 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). 782 | 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). 783 | simx_opmode_continuous =0x020000 784 | simx_opmode_streaming =0x020000 # sends command as one chunk. Command will be stored on the server and always executed 785 | #(every x ms (as far as possible) where x can be 0-65535. just add x to opmode_continuous). 786 | # A reply will be sent continuously each time as one chunk. Doesn't wait for the reply. 787 | 788 | # Operation modes for heavy data 789 | 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. 790 | simx_opmode_continuous_split =0x040000 791 | 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. 792 | 793 | # Special operation modes 794 | simx_opmode_discontinue =0x050000 # removes and cancels all commands stored on the client or server side (also continuous commands) 795 | 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) 796 | 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) 797 | 798 | 799 | # Command return codes 800 | simx_return_ok =0x000000 801 | simx_return_novalue_flag =0x000001 # input buffer doesn't contain the specified command 802 | simx_return_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode 803 | simx_return_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode 804 | simx_return_remote_error_flag =0x000008 # command caused an error on the server side 805 | simx_return_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) 806 | simx_return_local_error_flag =0x000020 # command caused an error on the client side 807 | simx_return_initialize_error_flag =0x000040 # simxStart was not yet called 808 | 809 | # Following for backward compatibility (same as above) 810 | simx_error_noerror =0x000000 811 | simx_error_novalue_flag =0x000001 # input buffer doesn't contain the specified command 812 | simx_error_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode 813 | simx_error_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode 814 | simx_error_remote_error_flag =0x000008 # command caused an error on the server side 815 | simx_error_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) 816 | simx_error_local_error_flag =0x000020 # command caused an error on the client side 817 | simx_error_initialize_error_flag =0x000040 # simxStart was not yet called 818 | 819 | # vrepConst.py 820 | 821 | #load library 822 | libsimx = None 823 | try: 824 | file_extension = '.so' 825 | if platform.system() =='cli': 826 | file_extension = '.dll' 827 | elif platform.system() =='Windows': 828 | file_extension = '.dll' 829 | elif platform.system() == 'Darwin': 830 | file_extension = '.dylib' 831 | else: 832 | file_extension = '.so' 833 | libfullpath = os.path.join(os.path.dirname(__file__), 'remoteApi' + file_extension) 834 | libsimx = ct.CDLL(libfullpath) 835 | except: 836 | print ('----------------------------------------------------') 837 | print ('The remoteApi library could not be loaded. Make sure') 838 | print ('it is located in the same folder as "vrep.py", or') 839 | print ('appropriately adjust the file "vrep.py"') 840 | print ('----------------------------------------------------') 841 | print ('') 842 | 843 | #ctypes wrapper prototypes 844 | c_GetJointPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointPosition", libsimx)) 845 | c_SetJointPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointPosition", libsimx)) 846 | c_GetJointMatrix = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointMatrix", libsimx)) 847 | c_SetSphericalJointMatrix = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetSphericalJointMatrix", libsimx)) 848 | c_SetJointTargetVelocity = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointTargetVelocity", libsimx)) 849 | c_SetJointTargetPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointTargetPosition", libsimx)) 850 | c_GetJointForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointForce", libsimx)) 851 | c_SetJointForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointForce", libsimx)) 852 | c_ReadForceSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.c_int32)(("simxReadForceSensor", libsimx)) 853 | c_BreakForceSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxBreakForceSensor", libsimx)) 854 | c_ReadVisionSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.POINTER(ct.c_float)), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxReadVisionSensor", libsimx)) 855 | c_GetObjectHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectHandle", libsimx)) 856 | c_GetVisionSensorImage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_byte)), ct.c_ubyte, ct.c_int32)(("simxGetVisionSensorImage", libsimx)) 857 | c_SetVisionSensorImage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_byte), ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetVisionSensorImage", libsimx)) 858 | c_GetVisionSensorDepthBuffer= ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)), ct.c_int32)(("simxGetVisionSensorDepthBuffer", libsimx)) 859 | c_GetObjectChild = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectChild", libsimx)) 860 | c_GetObjectParent = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectParent", libsimx)) 861 | c_ReadProximitySensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_float), ct.c_int32)(("simxReadProximitySensor", libsimx)) 862 | c_LoadModel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.POINTER(ct.c_int32), ct.c_int32)(("simxLoadModel", libsimx)) 863 | c_LoadUI = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxLoadUI", libsimx)) 864 | c_LoadScene = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.c_int32)(("simxLoadScene", libsimx)) 865 | c_StartSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxStartSimulation", libsimx)) 866 | c_PauseSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxPauseSimulation", libsimx)) 867 | c_StopSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxStopSimulation", libsimx)) 868 | c_GetUIHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIHandle", libsimx)) 869 | c_GetUISlider = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUISlider", libsimx)) 870 | c_SetUISlider = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetUISlider", libsimx)) 871 | c_GetUIEventButton = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIEventButton", libsimx)) 872 | c_GetUIButtonProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIButtonProperty", libsimx)) 873 | c_SetUIButtonProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetUIButtonProperty", libsimx)) 874 | c_AddStatusbarMessage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxAddStatusbarMessage", libsimx)) 875 | c_AuxiliaryConsoleOpen = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.c_int32)(("simxAuxiliaryConsoleOpen", libsimx)) 876 | c_AuxiliaryConsoleClose = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxAuxiliaryConsoleClose", libsimx)) 877 | c_AuxiliaryConsolePrint = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxAuxiliaryConsolePrint", libsimx)) 878 | c_AuxiliaryConsoleShow = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxAuxiliaryConsoleShow", libsimx)) 879 | c_GetObjectOrientation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectOrientation", libsimx)) 880 | c_GetObjectPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectPosition", libsimx)) 881 | c_SetObjectOrientation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectOrientation", libsimx)) 882 | c_SetObjectPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectPosition", libsimx)) 883 | c_SetObjectParent = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetObjectParent", libsimx)) 884 | c_SetUIButtonLabel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32)(("simxSetUIButtonLabel", libsimx)) 885 | c_GetLastErrors = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetLastErrors", libsimx)) 886 | c_GetArrayParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetArrayParameter", libsimx)) 887 | c_SetArrayParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetArrayParameter", libsimx)) 888 | c_GetBooleanParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxGetBooleanParameter", libsimx)) 889 | c_SetBooleanParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetBooleanParameter", libsimx)) 890 | c_GetIntegerParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetIntegerParameter", libsimx)) 891 | c_SetIntegerParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetIntegerParameter", libsimx)) 892 | c_GetFloatingParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetFloatingParameter", libsimx)) 893 | c_SetFloatingParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetFloatingParameter", libsimx)) 894 | c_GetStringParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetStringParameter", libsimx)) 895 | c_GetCollisionHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetCollisionHandle", libsimx)) 896 | c_GetDistanceHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetDistanceHandle", libsimx)) 897 | c_GetCollectionHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetCollectionHandle", libsimx)) 898 | c_ReadCollision = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxReadCollision", libsimx)) 899 | c_ReadDistance = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxReadDistance", libsimx)) 900 | c_RemoveObject = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveObject", libsimx)) 901 | c_RemoveModel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveModel", libsimx)) 902 | c_RemoveUI = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveUI", libsimx)) 903 | c_CloseScene = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxCloseScene", libsimx)) 904 | c_GetObjects = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxGetObjects", libsimx)) 905 | c_DisplayDialog = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.c_int32)(("simxDisplayDialog", libsimx)) 906 | c_EndDialog = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxEndDialog", libsimx)) 907 | c_GetDialogInput = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetDialogInput", libsimx)) 908 | c_GetDialogResult = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetDialogResult", libsimx)) 909 | c_CopyPasteObjects = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32, ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxCopyPasteObjects", libsimx)) 910 | c_GetObjectSelection = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectSelection", libsimx)) 911 | c_SetObjectSelection = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32, ct.c_int32)(("simxSetObjectSelection", libsimx)) 912 | c_ClearFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearFloatSignal", libsimx)) 913 | c_ClearIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearIntegerSignal", libsimx)) 914 | c_ClearStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearStringSignal", libsimx)) 915 | c_GetFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_float), ct.c_int32)(("simxGetFloatSignal", libsimx)) 916 | c_GetIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetIntegerSignal", libsimx)) 917 | c_GetStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetStringSignal", libsimx)) 918 | c_SetFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_float, ct.c_int32)(("simxSetFloatSignal", libsimx)) 919 | c_SetIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32)(("simxSetIntegerSignal", libsimx)) 920 | c_SetStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxSetStringSignal", libsimx)) 921 | c_AppendStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxAppendStringSignal", libsimx)) 922 | c_WriteStringStream = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxWriteStringStream", libsimx)) 923 | c_GetObjectFloatParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectFloatParameter", libsimx)) 924 | c_SetObjectFloatParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetObjectFloatParameter", libsimx)) 925 | c_GetObjectIntParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectIntParameter", libsimx)) 926 | c_SetObjectIntParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetObjectIntParameter", libsimx)) 927 | c_GetModelProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetModelProperty", libsimx)) 928 | c_SetModelProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetModelProperty", libsimx)) 929 | c_Start = ct.CFUNCTYPE(ct.c_int32,ct.POINTER(ct.c_char), ct.c_int32, ct.c_ubyte, ct.c_ubyte, ct.c_int32, ct.c_int32)(("simxStart", libsimx)) 930 | c_Finish = ct.CFUNCTYPE(None, ct.c_int32)(("simxFinish", libsimx)) 931 | c_GetPingTime = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetPingTime", libsimx)) 932 | c_GetLastCmdTime = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxGetLastCmdTime", libsimx)) 933 | c_SynchronousTrigger = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxSynchronousTrigger", libsimx)) 934 | c_Synchronous = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_ubyte)(("simxSynchronous", libsimx)) 935 | c_PauseCommunication = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_ubyte)(("simxPauseCommunication", libsimx)) 936 | c_GetInMessageInfo = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetInMessageInfo", libsimx)) 937 | c_GetOutMessageInfo = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetOutMessageInfo", libsimx)) 938 | c_GetConnectionId = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxGetConnectionId", libsimx)) 939 | c_CreateBuffer = ct.CFUNCTYPE(ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxCreateBuffer", libsimx)) 940 | c_ReleaseBuffer = ct.CFUNCTYPE(None, ct.c_void_p)(("simxReleaseBuffer", libsimx)) 941 | c_TransferFile = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32)(("simxTransferFile", libsimx)) 942 | c_EraseFile = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxEraseFile", libsimx)) 943 | c_GetAndClearStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetAndClearStringSignal", libsimx)) 944 | c_ReadStringStream = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxReadStringStream", libsimx)) 945 | c_CreateDummy = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_float, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_int32), ct.c_int32)(("simxCreateDummy", libsimx)) 946 | c_Query = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxQuery", libsimx)) 947 | c_GetObjectGroupData = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetObjectGroupData", libsimx)) 948 | c_GetObjectVelocity = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectVelocity", libsimx)) 949 | c_CallScriptFunction = ct.CFUNCTYPE(ct.c_int32,ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_int32),ct.c_int32,ct.POINTER(ct.c_float),ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_ubyte),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_ubyte)),ct.c_int32)(("simxCallScriptFunction", libsimx)) 950 | 951 | #API functions 952 | def simxGetJointPosition(clientID, jointHandle, operationMode): 953 | ''' 954 | Please have a look at the function description/documentation in the V-REP user manual 955 | ''' 956 | position = ct.c_float() 957 | return c_GetJointPosition(clientID, jointHandle, ct.byref(position), operationMode), position.value 958 | 959 | def simxSetJointPosition(clientID, jointHandle, position, operationMode): 960 | ''' 961 | Please have a look at the function description/documentation in the V-REP user manual 962 | ''' 963 | 964 | return c_SetJointPosition(clientID, jointHandle, position, operationMode) 965 | 966 | def simxGetJointMatrix(clientID, jointHandle, operationMode): 967 | ''' 968 | Please have a look at the function description/documentation in the V-REP user manual 969 | ''' 970 | matrix = (ct.c_float*12)() 971 | ret = c_GetJointMatrix(clientID, jointHandle, matrix, operationMode) 972 | arr = [] 973 | for i in range(12): 974 | arr.append(matrix[i]) 975 | return ret, arr 976 | 977 | def simxSetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode): 978 | ''' 979 | Please have a look at the function description/documentation in the V-REP user manual 980 | ''' 981 | matrix = (ct.c_float*12)(*matrix) 982 | return c_SetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode) 983 | 984 | def simxSetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode): 985 | ''' 986 | Please have a look at the function description/documentation in the V-REP user manual 987 | ''' 988 | 989 | return c_SetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode) 990 | 991 | def simxSetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode): 992 | ''' 993 | Please have a look at the function description/documentation in the V-REP user manual 994 | ''' 995 | 996 | return c_SetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode) 997 | 998 | def simxJointGetForce(clientID, jointHandle, operationMode): 999 | ''' 1000 | Please have a look at the function description/documentation in the V-REP user manual 1001 | ''' 1002 | force = ct.c_float() 1003 | return c_GetJointForce(clientID, jointHandle, ct.byref(force), operationMode), force.value 1004 | 1005 | def simxGetJointForce(clientID, jointHandle, operationMode): 1006 | ''' 1007 | Please have a look at the function description/documentation in the V-REP user manual 1008 | ''' 1009 | force = ct.c_float() 1010 | return c_GetJointForce(clientID, jointHandle, ct.byref(force), operationMode), force.value 1011 | 1012 | def simxSetJointForce(clientID, jointHandle, force, operationMode): 1013 | ''' 1014 | Please have a look at the function description/documentation in the V-REP user manual 1015 | ''' 1016 | return c_SetJointForce(clientID, jointHandle, force, operationMode) 1017 | 1018 | def simxReadForceSensor(clientID, forceSensorHandle, operationMode): 1019 | ''' 1020 | Please have a look at the function description/documentation in the V-REP user manual 1021 | ''' 1022 | state = ct.c_ubyte() 1023 | forceVector = (ct.c_float*3)() 1024 | torqueVector = (ct.c_float*3)() 1025 | ret = c_ReadForceSensor(clientID, forceSensorHandle, ct.byref(state), forceVector, torqueVector, operationMode) 1026 | arr1 = [] 1027 | for i in range(3): 1028 | arr1.append(forceVector[i]) 1029 | arr2 = [] 1030 | for i in range(3): 1031 | arr2.append(torqueVector[i]) 1032 | #if sys.version_info[0] == 3: 1033 | # state=state.value 1034 | #else: 1035 | # state=ord(state.value) 1036 | return ret, state.value, arr1, arr2 1037 | 1038 | def simxBreakForceSensor(clientID, forceSensorHandle, operationMode): 1039 | ''' 1040 | Please have a look at the function description/documentation in the V-REP user manual 1041 | ''' 1042 | return c_BreakForceSensor(clientID, forceSensorHandle, operationMode) 1043 | 1044 | def simxReadVisionSensor(clientID, sensorHandle, operationMode): 1045 | ''' 1046 | Please have a look at the function description/documentation in the V-REP user manual 1047 | ''' 1048 | 1049 | detectionState = ct.c_ubyte() 1050 | auxValues = ct.POINTER(ct.c_float)() 1051 | auxValuesCount = ct.POINTER(ct.c_int)() 1052 | ret = c_ReadVisionSensor(clientID, sensorHandle, ct.byref(detectionState), ct.byref(auxValues), ct.byref(auxValuesCount), operationMode) 1053 | 1054 | auxValues2 = [] 1055 | if ret == 0: 1056 | s = 0 1057 | for i in range(auxValuesCount[0]): 1058 | auxValues2.append(auxValues[s:s+auxValuesCount[i+1]]) 1059 | s += auxValuesCount[i+1] 1060 | 1061 | #free C buffers 1062 | c_ReleaseBuffer(auxValues) 1063 | c_ReleaseBuffer(auxValuesCount) 1064 | 1065 | return ret, bool(detectionState.value!=0), auxValues2 1066 | 1067 | def simxGetObjectHandle(clientID, objectName, operationMode): 1068 | ''' 1069 | Please have a look at the function description/documentation in the V-REP user manual 1070 | ''' 1071 | handle = ct.c_int() 1072 | if (sys.version_info[0] == 3) and (type(objectName) is str): 1073 | objectName=objectName.encode('utf-8') 1074 | return c_GetObjectHandle(clientID, objectName, ct.byref(handle), operationMode), handle.value 1075 | 1076 | def simxGetVisionSensorImage(clientID, sensorHandle, options, operationMode): 1077 | ''' 1078 | Please have a look at the function description/documentation in the V-REP user manual 1079 | ''' 1080 | 1081 | resolution = (ct.c_int*2)() 1082 | c_image = ct.POINTER(ct.c_byte)() 1083 | bytesPerPixel = 3 1084 | if (options and 1) != 0: 1085 | bytesPerPixel = 1 1086 | ret = c_GetVisionSensorImage(clientID, sensorHandle, resolution, ct.byref(c_image), options, operationMode) 1087 | 1088 | reso = [] 1089 | image = [] 1090 | if (ret == 0): 1091 | image = [None]*resolution[0]*resolution[1]*bytesPerPixel 1092 | for i in range(resolution[0] * resolution[1] * bytesPerPixel): 1093 | image[i] = c_image[i] 1094 | for i in range(2): 1095 | reso.append(resolution[i]) 1096 | return ret, reso, image 1097 | 1098 | def simxSetVisionSensorImage(clientID, sensorHandle, image, options, operationMode): 1099 | ''' 1100 | Please have a look at the function description/documentation in the V-REP user manual 1101 | ''' 1102 | size = len(image) 1103 | image_bytes = (ct.c_byte*size)(*image) 1104 | return c_SetVisionSensorImage(clientID, sensorHandle, image_bytes, size, options, operationMode) 1105 | 1106 | def simxGetVisionSensorDepthBuffer(clientID, sensorHandle, operationMode): 1107 | ''' 1108 | Please have a look at the function description/documentation in the V-REP user manual 1109 | ''' 1110 | c_buffer = ct.POINTER(ct.c_float)() 1111 | resolution = (ct.c_int*2)() 1112 | ret = c_GetVisionSensorDepthBuffer(clientID, sensorHandle, resolution, ct.byref(c_buffer), operationMode) 1113 | reso = [] 1114 | buffer = [] 1115 | if (ret == 0): 1116 | buffer = [None]*resolution[0]*resolution[1] 1117 | for i in range(resolution[0] * resolution[1]): 1118 | buffer[i] = c_buffer[i] 1119 | for i in range(2): 1120 | reso.append(resolution[i]) 1121 | return ret, reso, buffer 1122 | 1123 | def simxGetObjectChild(clientID, parentObjectHandle, childIndex, operationMode): 1124 | ''' 1125 | Please have a look at the function description/documentation in the V-REP user manual 1126 | ''' 1127 | childObjectHandle = ct.c_int() 1128 | return c_GetObjectChild(clientID, parentObjectHandle, childIndex, ct.byref(childObjectHandle), operationMode), childObjectHandle.value 1129 | 1130 | def simxGetObjectParent(clientID, childObjectHandle, operationMode): 1131 | ''' 1132 | Please have a look at the function description/documentation in the V-REP user manual 1133 | ''' 1134 | 1135 | parentObjectHandle = ct.c_int() 1136 | return c_GetObjectParent(clientID, childObjectHandle, ct.byref(parentObjectHandle), operationMode), parentObjectHandle.value 1137 | 1138 | def simxReadProximitySensor(clientID, sensorHandle, operationMode): 1139 | ''' 1140 | Please have a look at the function description/documentation in the V-REP user manual 1141 | ''' 1142 | 1143 | detectionState = ct.c_ubyte() 1144 | detectedObjectHandle = ct.c_int() 1145 | detectedPoint = (ct.c_float*3)() 1146 | detectedSurfaceNormalVector = (ct.c_float*3)() 1147 | ret = c_ReadProximitySensor(clientID, sensorHandle, ct.byref(detectionState), detectedPoint, ct.byref(detectedObjectHandle), detectedSurfaceNormalVector, operationMode) 1148 | arr1 = [] 1149 | for i in range(3): 1150 | arr1.append(detectedPoint[i]) 1151 | arr2 = [] 1152 | for i in range(3): 1153 | arr2.append(detectedSurfaceNormalVector[i]) 1154 | return ret, bool(detectionState.value!=0), arr1, detectedObjectHandle.value, arr2 1155 | 1156 | def simxLoadModel(clientID, modelPathAndName, options, operationMode): 1157 | ''' 1158 | Please have a look at the function description/documentation in the V-REP user manual 1159 | ''' 1160 | baseHandle = ct.c_int() 1161 | if (sys.version_info[0] == 3) and (type(modelPathAndName) is str): 1162 | modelPathAndName=modelPathAndName.encode('utf-8') 1163 | return c_LoadModel(clientID, modelPathAndName, options, ct.byref(baseHandle), operationMode), baseHandle.value 1164 | 1165 | def simxLoadUI(clientID, uiPathAndName, options, operationMode): 1166 | ''' 1167 | Please have a look at the function description/documentation in the V-REP user manual 1168 | ''' 1169 | 1170 | count = ct.c_int() 1171 | uiHandles = ct.POINTER(ct.c_int)() 1172 | if (sys.version_info[0] == 3) and (type(uiPathAndName) is str): 1173 | uiPathAndName=uiPathAndName.encode('utf-8') 1174 | ret = c_LoadUI(clientID, uiPathAndName, options, ct.byref(count), ct.byref(uiHandles), operationMode) 1175 | 1176 | handles = [] 1177 | if ret == 0: 1178 | for i in range(count.value): 1179 | handles.append(uiHandles[i]) 1180 | #free C buffers 1181 | c_ReleaseBuffer(uiHandles) 1182 | 1183 | return ret, handles 1184 | 1185 | def simxLoadScene(clientID, scenePathAndName, options, operationMode): 1186 | ''' 1187 | Please have a look at the function description/documentation in the V-REP user manual 1188 | ''' 1189 | 1190 | if (sys.version_info[0] == 3) and (type(scenePathAndName) is str): 1191 | scenePathAndName=scenePathAndName.encode('utf-8') 1192 | return c_LoadScene(clientID, scenePathAndName, options, operationMode) 1193 | 1194 | def simxStartSimulation(clientID, operationMode): 1195 | ''' 1196 | Please have a look at the function description/documentation in the V-REP user manual 1197 | ''' 1198 | 1199 | return c_StartSimulation(clientID, operationMode) 1200 | 1201 | def simxPauseSimulation(clientID, operationMode): 1202 | ''' 1203 | Please have a look at the function description/documentation in the V-REP user manual 1204 | ''' 1205 | 1206 | return c_PauseSimulation(clientID, operationMode) 1207 | 1208 | def simxStopSimulation(clientID, operationMode): 1209 | ''' 1210 | Please have a look at the function description/documentation in the V-REP user manual 1211 | ''' 1212 | 1213 | return c_StopSimulation(clientID, operationMode) 1214 | 1215 | def simxGetUIHandle(clientID, uiName, operationMode): 1216 | ''' 1217 | Please have a look at the function description/documentation in the V-REP user manual 1218 | ''' 1219 | 1220 | handle = ct.c_int() 1221 | if (sys.version_info[0] == 3) and (type(uiName) is str): 1222 | uiName=uiName.encode('utf-8') 1223 | return c_GetUIHandle(clientID, uiName, ct.byref(handle), operationMode), handle.value 1224 | 1225 | def simxGetUISlider(clientID, uiHandle, uiButtonID, operationMode): 1226 | ''' 1227 | Please have a look at the function description/documentation in the V-REP user manual 1228 | ''' 1229 | 1230 | position = ct.c_int() 1231 | return c_GetUISlider(clientID, uiHandle, uiButtonID, ct.byref(position), operationMode), position.value 1232 | 1233 | def simxSetUISlider(clientID, uiHandle, uiButtonID, position, operationMode): 1234 | ''' 1235 | Please have a look at the function description/documentation in the V-REP user manual 1236 | ''' 1237 | 1238 | return c_SetUISlider(clientID, uiHandle, uiButtonID, position, operationMode) 1239 | 1240 | def simxGetUIEventButton(clientID, uiHandle, operationMode): 1241 | ''' 1242 | Please have a look at the function description/documentation in the V-REP user manual 1243 | ''' 1244 | 1245 | uiEventButtonID = ct.c_int() 1246 | auxValues = (ct.c_int*2)() 1247 | ret = c_GetUIEventButton(clientID, uiHandle, ct.byref(uiEventButtonID), auxValues, operationMode) 1248 | arr = [] 1249 | for i in range(2): 1250 | arr.append(auxValues[i]) 1251 | return ret, uiEventButtonID.value, arr 1252 | 1253 | def simxGetUIButtonProperty(clientID, uiHandle, uiButtonID, operationMode): 1254 | ''' 1255 | Please have a look at the function description/documentation in the V-REP user manual 1256 | ''' 1257 | 1258 | prop = ct.c_int() 1259 | return c_GetUIButtonProperty(clientID, uiHandle, uiButtonID, ct.byref(prop), operationMode), prop.value 1260 | 1261 | def simxSetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode): 1262 | ''' 1263 | Please have a look at the function description/documentation in the V-REP user manual 1264 | ''' 1265 | 1266 | return c_SetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode) 1267 | 1268 | def simxAddStatusbarMessage(clientID, message, operationMode): 1269 | ''' 1270 | Please have a look at the function description/documentation in the V-REP user manual 1271 | ''' 1272 | 1273 | if (sys.version_info[0] == 3) and (type(message) is str): 1274 | message=message.encode('utf-8') 1275 | return c_AddStatusbarMessage(clientID, message, operationMode) 1276 | 1277 | def simxAuxiliaryConsoleOpen(clientID, title, maxLines, mode, position, size, textColor, backgroundColor, operationMode): 1278 | ''' 1279 | Please have a look at the function description/documentation in the V-REP user manual 1280 | ''' 1281 | 1282 | consoleHandle = ct.c_int() 1283 | if (sys.version_info[0] == 3) and (type(title) is str): 1284 | title=title.encode('utf-8') 1285 | if position != None: 1286 | c_position = (ct.c_int*2)(*position) 1287 | else: 1288 | c_position = None 1289 | if size != None: 1290 | c_size = (ct.c_int*2)(*size) 1291 | else: 1292 | c_size = None 1293 | if textColor != None: 1294 | c_textColor = (ct.c_float*3)(*textColor) 1295 | else: 1296 | c_textColor = None 1297 | if backgroundColor != None: 1298 | c_backgroundColor = (ct.c_float*3)(*backgroundColor) 1299 | else: 1300 | c_backgroundColor = None 1301 | return c_AuxiliaryConsoleOpen(clientID, title, maxLines, mode, c_position, c_size, c_textColor, c_backgroundColor, ct.byref(consoleHandle), operationMode), consoleHandle.value 1302 | 1303 | def simxAuxiliaryConsoleClose(clientID, consoleHandle, operationMode): 1304 | ''' 1305 | Please have a look at the function description/documentation in the V-REP user manual 1306 | ''' 1307 | 1308 | return c_AuxiliaryConsoleClose(clientID, consoleHandle, operationMode) 1309 | 1310 | def simxAuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode): 1311 | ''' 1312 | Please have a look at the function description/documentation in the V-REP user manual 1313 | ''' 1314 | 1315 | if (sys.version_info[0] == 3) and (type(txt) is str): 1316 | txt=txt.encode('utf-8') 1317 | return c_AuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode) 1318 | 1319 | def simxAuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode): 1320 | ''' 1321 | Please have a look at the function description/documentation in the V-REP user manual 1322 | ''' 1323 | 1324 | return c_AuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode) 1325 | 1326 | def simxGetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, operationMode): 1327 | ''' 1328 | Please have a look at the function description/documentation in the V-REP user manual 1329 | ''' 1330 | eulerAngles = (ct.c_float*3)() 1331 | ret = c_GetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode) 1332 | arr = [] 1333 | for i in range(3): 1334 | arr.append(eulerAngles[i]) 1335 | return ret, arr 1336 | 1337 | def simxGetObjectPosition(clientID, objectHandle, relativeToObjectHandle, operationMode): 1338 | ''' 1339 | Please have a look at the function description/documentation in the V-REP user manual 1340 | ''' 1341 | position = (ct.c_float*3)() 1342 | ret = c_GetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode) 1343 | arr = [] 1344 | for i in range(3): 1345 | arr.append(position[i]) 1346 | return ret, arr 1347 | 1348 | def simxSetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode): 1349 | ''' 1350 | Please have a look at the function description/documentation in the V-REP user manual 1351 | ''' 1352 | 1353 | angles = (ct.c_float*3)(*eulerAngles) 1354 | return c_SetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, angles, operationMode) 1355 | 1356 | def simxSetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode): 1357 | ''' 1358 | Please have a look at the function description/documentation in the V-REP user manual 1359 | ''' 1360 | 1361 | c_position = (ct.c_float*3)(*position) 1362 | return c_SetObjectPosition(clientID, objectHandle, relativeToObjectHandle, c_position, operationMode) 1363 | 1364 | def simxSetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode): 1365 | ''' 1366 | Please have a look at the function description/documentation in the V-REP user manual 1367 | ''' 1368 | 1369 | return c_SetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode) 1370 | 1371 | def simxSetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode): 1372 | ''' 1373 | Please have a look at the function description/documentation in the V-REP user manual 1374 | ''' 1375 | 1376 | if sys.version_info[0] == 3: 1377 | if type(upStateLabel) is str: 1378 | upStateLabel=upStateLabel.encode('utf-8') 1379 | if type(downStateLabel) is str: 1380 | downStateLabel=downStateLabel.encode('utf-8') 1381 | return c_SetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode) 1382 | 1383 | def simxGetLastErrors(clientID, operationMode): 1384 | ''' 1385 | Please have a look at the function description/documentation in the V-REP user manual 1386 | ''' 1387 | errors =[] 1388 | errorCnt = ct.c_int() 1389 | errorStrings = ct.POINTER(ct.c_char)() 1390 | ret = c_GetLastErrors(clientID, ct.byref(errorCnt), ct.byref(errorStrings), operationMode) 1391 | if ret == 0: 1392 | s = 0 1393 | for i in range(errorCnt.value): 1394 | a = bytearray() 1395 | while errorStrings[s] != b'\0': 1396 | if sys.version_info[0] == 3: 1397 | a.append(int.from_bytes(errorStrings[s],'big')) 1398 | else: 1399 | a.append(errorStrings[s]) 1400 | s += 1 1401 | s += 1 #skip null 1402 | if sys.version_info[0] == 3: 1403 | errors.append(str(a,'utf-8')) 1404 | else: 1405 | errors.append(str(a)) 1406 | 1407 | return ret, errors 1408 | 1409 | def simxGetArrayParameter(clientID, paramIdentifier, operationMode): 1410 | ''' 1411 | Please have a look at the function description/documentation in the V-REP user manual 1412 | ''' 1413 | paramValues = (ct.c_float*3)() 1414 | ret = c_GetArrayParameter(clientID, paramIdentifier, paramValues, operationMode) 1415 | arr = [] 1416 | for i in range(3): 1417 | arr.append(paramValues[i]) 1418 | return ret, arr 1419 | 1420 | def simxSetArrayParameter(clientID, paramIdentifier, paramValues, operationMode): 1421 | ''' 1422 | Please have a look at the function description/documentation in the V-REP user manual 1423 | ''' 1424 | 1425 | c_paramValues = (ct.c_float*3)(*paramValues) 1426 | return c_SetArrayParameter(clientID, paramIdentifier, c_paramValues, operationMode) 1427 | 1428 | def simxGetBooleanParameter(clientID, paramIdentifier, operationMode): 1429 | ''' 1430 | Please have a look at the function description/documentation in the V-REP user manual 1431 | ''' 1432 | 1433 | paramValue = ct.c_ubyte() 1434 | return c_GetBooleanParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), bool(paramValue.value!=0) 1435 | 1436 | def simxSetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode): 1437 | ''' 1438 | Please have a look at the function description/documentation in the V-REP user manual 1439 | ''' 1440 | 1441 | return c_SetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode) 1442 | 1443 | def simxGetIntegerParameter(clientID, paramIdentifier, operationMode): 1444 | ''' 1445 | Please have a look at the function description/documentation in the V-REP user manual 1446 | ''' 1447 | 1448 | paramValue = ct.c_int() 1449 | return c_GetIntegerParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), paramValue.value 1450 | 1451 | def simxSetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode): 1452 | ''' 1453 | Please have a look at the function description/documentation in the V-REP user manual 1454 | ''' 1455 | 1456 | return c_SetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode) 1457 | 1458 | def simxGetFloatingParameter(clientID, paramIdentifier, operationMode): 1459 | ''' 1460 | Please have a look at the function description/documentation in the V-REP user manual 1461 | ''' 1462 | 1463 | paramValue = ct.c_float() 1464 | return c_GetFloatingParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), paramValue.value 1465 | 1466 | def simxSetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode): 1467 | ''' 1468 | Please have a look at the function description/documentation in the V-REP user manual 1469 | ''' 1470 | 1471 | return c_SetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode) 1472 | 1473 | def simxGetStringParameter(clientID, paramIdentifier, operationMode): 1474 | ''' 1475 | Please have a look at the function description/documentation in the V-REP user manual 1476 | ''' 1477 | paramValue = ct.POINTER(ct.c_char)() 1478 | ret = c_GetStringParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode) 1479 | 1480 | a = bytearray() 1481 | if ret == 0: 1482 | i = 0 1483 | while paramValue[i] != b'\0': 1484 | if sys.version_info[0] == 3: 1485 | a.append(int.from_bytes(paramValue[i],'big')) 1486 | else: 1487 | a.append(paramValue[i]) 1488 | i=i+1 1489 | if sys.version_info[0] == 3: 1490 | a=str(a,'utf-8') 1491 | else: 1492 | a=str(a) 1493 | return ret, a 1494 | 1495 | def simxGetCollisionHandle(clientID, collisionObjectName, operationMode): 1496 | ''' 1497 | Please have a look at the function description/documentation in the V-REP user manual 1498 | ''' 1499 | 1500 | handle = ct.c_int() 1501 | if (sys.version_info[0] == 3) and (type(collisionObjectName) is str): 1502 | collisionObjectName=collisionObjectName.encode('utf-8') 1503 | return c_GetCollisionHandle(clientID, collisionObjectName, ct.byref(handle), operationMode), handle.value 1504 | 1505 | def simxGetCollectionHandle(clientID, collectionName, operationMode): 1506 | ''' 1507 | Please have a look at the function description/documentation in the V-REP user manual 1508 | ''' 1509 | 1510 | handle = ct.c_int() 1511 | if (sys.version_info[0] == 3) and (type(collectionName) is str): 1512 | collectionName=collectionName.encode('utf-8') 1513 | return c_GetCollectionHandle(clientID, collectionName, ct.byref(handle), operationMode), handle.value 1514 | 1515 | def simxGetDistanceHandle(clientID, distanceObjectName, operationMode): 1516 | ''' 1517 | Please have a look at the function description/documentation in the V-REP user manual 1518 | ''' 1519 | 1520 | handle = ct.c_int() 1521 | if (sys.version_info[0] == 3) and (type(distanceObjectName) is str): 1522 | distanceObjectName=distanceObjectName.encode('utf-8') 1523 | return c_GetDistanceHandle(clientID, distanceObjectName, ct.byref(handle), operationMode), handle.value 1524 | 1525 | def simxReadCollision(clientID, collisionObjectHandle, operationMode): 1526 | ''' 1527 | Please have a look at the function description/documentation in the V-REP user manual 1528 | ''' 1529 | collisionState = ct.c_ubyte() 1530 | return c_ReadCollision(clientID, collisionObjectHandle, ct.byref(collisionState), operationMode), bool(collisionState.value!=0) 1531 | 1532 | def simxReadDistance(clientID, distanceObjectHandle, operationMode): 1533 | ''' 1534 | Please have a look at the function description/documentation in the V-REP user manual 1535 | ''' 1536 | 1537 | minimumDistance = ct.c_float() 1538 | return c_ReadDistance(clientID, distanceObjectHandle, ct.byref(minimumDistance), operationMode), minimumDistance.value 1539 | 1540 | def simxRemoveObject(clientID, objectHandle, operationMode): 1541 | ''' 1542 | Please have a look at the function description/documentation in the V-REP user manual 1543 | ''' 1544 | 1545 | return c_RemoveObject(clientID, objectHandle, operationMode) 1546 | 1547 | def simxRemoveModel(clientID, objectHandle, operationMode): 1548 | ''' 1549 | Please have a look at the function description/documentation in the V-REP user manual 1550 | ''' 1551 | 1552 | return c_RemoveModel(clientID, objectHandle, operationMode) 1553 | 1554 | def simxRemoveUI(clientID, uiHandle, operationMode): 1555 | ''' 1556 | Please have a look at the function description/documentation in the V-REP user manual 1557 | ''' 1558 | 1559 | return c_RemoveUI(clientID, uiHandle, operationMode) 1560 | 1561 | def simxCloseScene(clientID, operationMode): 1562 | ''' 1563 | Please have a look at the function description/documentation in the V-REP user manual 1564 | ''' 1565 | 1566 | return c_CloseScene(clientID, operationMode) 1567 | 1568 | def simxGetObjects(clientID, objectType, operationMode): 1569 | ''' 1570 | Please have a look at the function description/documentation in the V-REP user manual 1571 | ''' 1572 | 1573 | objectCount = ct.c_int() 1574 | objectHandles = ct.POINTER(ct.c_int)() 1575 | 1576 | ret = c_GetObjects(clientID, objectType, ct.byref(objectCount), ct.byref(objectHandles), operationMode) 1577 | handles = [] 1578 | if ret == 0: 1579 | for i in range(objectCount.value): 1580 | handles.append(objectHandles[i]) 1581 | 1582 | return ret, handles 1583 | 1584 | 1585 | def simxDisplayDialog(clientID, titleText, mainText, dialogType, initialText, titleColors, dialogColors, operationMode): 1586 | ''' 1587 | Please have a look at the function description/documentation in the V-REP user manual 1588 | ''' 1589 | if titleColors != None: 1590 | c_titleColors = (ct.c_float*6)(*titleColors) 1591 | else: 1592 | c_titleColors = None 1593 | if dialogColors != None: 1594 | c_dialogColors = (ct.c_float*6)(*dialogColors) 1595 | else: 1596 | c_dialogColors = None 1597 | 1598 | c_dialogHandle = ct.c_int() 1599 | c_uiHandle = ct.c_int() 1600 | if sys.version_info[0] == 3: 1601 | if type(titleText) is str: 1602 | titleText=titleText.encode('utf-8') 1603 | if type(mainText) is str: 1604 | mainText=mainText.encode('utf-8') 1605 | if type(initialText) is str: 1606 | initialText=initialText.encode('utf-8') 1607 | return c_DisplayDialog(clientID, titleText, mainText, dialogType, initialText, c_titleColors, c_dialogColors, ct.byref(c_dialogHandle), ct.byref(c_uiHandle), operationMode), c_dialogHandle.value, c_uiHandle.value 1608 | 1609 | def simxEndDialog(clientID, dialogHandle, operationMode): 1610 | ''' 1611 | Please have a look at the function description/documentation in the V-REP user manual 1612 | ''' 1613 | 1614 | return c_EndDialog(clientID, dialogHandle, operationMode) 1615 | 1616 | def simxGetDialogInput(clientID, dialogHandle, operationMode): 1617 | ''' 1618 | Please have a look at the function description/documentation in the V-REP user manual 1619 | ''' 1620 | inputText = ct.POINTER(ct.c_char)() 1621 | ret = c_GetDialogInput(clientID, dialogHandle, ct.byref(inputText), operationMode) 1622 | 1623 | a = bytearray() 1624 | if ret == 0: 1625 | i = 0 1626 | while inputText[i] != b'\0': 1627 | if sys.version_info[0] == 3: 1628 | a.append(int.from_bytes(inputText[i],'big')) 1629 | else: 1630 | a.append(inputText[i]) 1631 | i = i+1 1632 | 1633 | if sys.version_info[0] == 3: 1634 | a=str(a,'utf-8') 1635 | else: 1636 | a=str(a) 1637 | return ret, a 1638 | 1639 | 1640 | def simxGetDialogResult(clientID, dialogHandle, operationMode): 1641 | ''' 1642 | Please have a look at the function description/documentation in the V-REP user manual 1643 | ''' 1644 | result = ct.c_int() 1645 | return c_GetDialogResult(clientID, dialogHandle, ct.byref(result), operationMode), result.value 1646 | 1647 | def simxCopyPasteObjects(clientID, objectHandles, operationMode): 1648 | ''' 1649 | Please have a look at the function description/documentation in the V-REP user manual 1650 | ''' 1651 | c_objectHandles = (ct.c_int*len(objectHandles))(*objectHandles) 1652 | c_objectHandles = ct.cast(c_objectHandles,ct.POINTER(ct.c_int)) # IronPython needs this 1653 | newObjectCount = ct.c_int() 1654 | newObjectHandles = ct.POINTER(ct.c_int)() 1655 | ret = c_CopyPasteObjects(clientID, c_objectHandles, len(objectHandles), ct.byref(newObjectHandles), ct.byref(newObjectCount), operationMode) 1656 | 1657 | newobj = [] 1658 | if ret == 0: 1659 | for i in range(newObjectCount.value): 1660 | newobj.append(newObjectHandles[i]) 1661 | 1662 | return ret, newobj 1663 | 1664 | 1665 | def simxGetObjectSelection(clientID, operationMode): 1666 | ''' 1667 | Please have a look at the function description/documentation in the V-REP user manual 1668 | ''' 1669 | objectCount = ct.c_int() 1670 | objectHandles = ct.POINTER(ct.c_int)() 1671 | ret = c_GetObjectSelection(clientID, ct.byref(objectHandles), ct.byref(objectCount), operationMode) 1672 | 1673 | newobj = [] 1674 | if ret == 0: 1675 | for i in range(objectCount.value): 1676 | newobj.append(objectHandles[i]) 1677 | 1678 | return ret, newobj 1679 | 1680 | 1681 | 1682 | def simxSetObjectSelection(clientID, objectHandles, operationMode): 1683 | ''' 1684 | Please have a look at the function description/documentation in the V-REP user manual 1685 | ''' 1686 | 1687 | c_objectHandles = (ct.c_int*len(objectHandles))(*objectHandles) 1688 | return c_SetObjectSelection(clientID, c_objectHandles, len(objectHandles), operationMode) 1689 | 1690 | def simxClearFloatSignal(clientID, signalName, operationMode): 1691 | ''' 1692 | Please have a look at the function description/documentation in the V-REP user manual 1693 | ''' 1694 | 1695 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1696 | signalName=signalName.encode('utf-8') 1697 | return c_ClearFloatSignal(clientID, signalName, operationMode) 1698 | 1699 | def simxClearIntegerSignal(clientID, signalName, operationMode): 1700 | ''' 1701 | Please have a look at the function description/documentation in the V-REP user manual 1702 | ''' 1703 | 1704 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1705 | signalName=signalName.encode('utf-8') 1706 | return c_ClearIntegerSignal(clientID, signalName, operationMode) 1707 | 1708 | def simxClearStringSignal(clientID, signalName, operationMode): 1709 | ''' 1710 | Please have a look at the function description/documentation in the V-REP user manual 1711 | ''' 1712 | 1713 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1714 | signalName=signalName.encode('utf-8') 1715 | return c_ClearStringSignal(clientID, signalName, operationMode) 1716 | 1717 | def simxGetFloatSignal(clientID, signalName, operationMode): 1718 | ''' 1719 | Please have a look at the function description/documentation in the V-REP user manual 1720 | ''' 1721 | 1722 | signalValue = ct.c_float() 1723 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1724 | signalName=signalName.encode('utf-8') 1725 | return c_GetFloatSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value 1726 | 1727 | def simxGetIntegerSignal(clientID, signalName, operationMode): 1728 | ''' 1729 | Please have a look at the function description/documentation in the V-REP user manual 1730 | ''' 1731 | 1732 | signalValue = ct.c_int() 1733 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1734 | signalName=signalName.encode('utf-8') 1735 | return c_GetIntegerSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value 1736 | 1737 | def simxGetStringSignal(clientID, signalName, operationMode): 1738 | ''' 1739 | Please have a look at the function description/documentation in the V-REP user manual 1740 | ''' 1741 | 1742 | signalLength = ct.c_int(); 1743 | signalValue = ct.POINTER(ct.c_ubyte)() 1744 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1745 | signalName=signalName.encode('utf-8') 1746 | ret = c_GetStringSignal(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) 1747 | 1748 | a = bytearray() 1749 | if ret == 0: 1750 | for i in range(signalLength.value): 1751 | a.append(signalValue[i]) 1752 | if sys.version_info[0] != 3: 1753 | a=str(a) 1754 | 1755 | return ret, a 1756 | 1757 | def simxGetAndClearStringSignal(clientID, signalName, operationMode): 1758 | ''' 1759 | Please have a look at the function description/documentation in the V-REP user manual 1760 | ''' 1761 | 1762 | signalLength = ct.c_int(); 1763 | signalValue = ct.POINTER(ct.c_ubyte)() 1764 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1765 | signalName=signalName.encode('utf-8') 1766 | ret = c_GetAndClearStringSignal(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) 1767 | 1768 | a = bytearray() 1769 | if ret == 0: 1770 | for i in range(signalLength.value): 1771 | a.append(signalValue[i]) 1772 | if sys.version_info[0] != 3: 1773 | a=str(a) 1774 | 1775 | return ret, a 1776 | 1777 | def simxReadStringStream(clientID, signalName, operationMode): 1778 | ''' 1779 | Please have a look at the function description/documentation in the V-REP user manual 1780 | ''' 1781 | 1782 | signalLength = ct.c_int(); 1783 | signalValue = ct.POINTER(ct.c_ubyte)() 1784 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1785 | signalName=signalName.encode('utf-8') 1786 | ret = c_ReadStringStream(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) 1787 | 1788 | a = bytearray() 1789 | if ret == 0: 1790 | for i in range(signalLength.value): 1791 | a.append(signalValue[i]) 1792 | if sys.version_info[0] != 3: 1793 | a=str(a) 1794 | 1795 | return ret, a 1796 | 1797 | def simxSetFloatSignal(clientID, signalName, signalValue, operationMode): 1798 | ''' 1799 | Please have a look at the function description/documentation in the V-REP user manual 1800 | ''' 1801 | 1802 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1803 | signalName=signalName.encode('utf-8') 1804 | return c_SetFloatSignal(clientID, signalName, signalValue, operationMode) 1805 | 1806 | def simxSetIntegerSignal(clientID, signalName, signalValue, operationMode): 1807 | ''' 1808 | Please have a look at the function description/documentation in the V-REP user manual 1809 | ''' 1810 | 1811 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1812 | signalName=signalName.encode('utf-8') 1813 | return c_SetIntegerSignal(clientID, signalName, signalValue, operationMode) 1814 | 1815 | def simxSetStringSignal(clientID, signalName, signalValue, operationMode): 1816 | ''' 1817 | Please have a look at the function description/documentation in the V-REP user manual 1818 | ''' 1819 | 1820 | sigV=signalValue 1821 | if sys.version_info[0] == 3: 1822 | if type(signalName) is str: 1823 | signalName=signalName.encode('utf-8') 1824 | if type(signalValue) is bytearray: 1825 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1826 | if type(signalValue) is str: 1827 | signalValue=signalValue.encode('utf-8') 1828 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1829 | else: 1830 | if type(signalValue) is bytearray: 1831 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1832 | if type(signalValue) is str: 1833 | signalValue=bytearray(signalValue) 1834 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1835 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1836 | return c_SetStringSignal(clientID, signalName, sigV, len(signalValue), operationMode) 1837 | 1838 | def simxAppendStringSignal(clientID, signalName, signalValue, operationMode): 1839 | ''' 1840 | Please have a look at the function description/documentation in the V-REP user manual 1841 | ''' 1842 | 1843 | sigV=signalValue 1844 | if sys.version_info[0] == 3: 1845 | if type(signalName) is str: 1846 | signalName=signalName.encode('utf-8') 1847 | if type(signalValue) is bytearray: 1848 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1849 | if type(signalValue) is str: 1850 | signalValue=signalValue.encode('utf-8') 1851 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1852 | else: 1853 | if type(signalValue) is bytearray: 1854 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1855 | if type(signalValue) is str: 1856 | signalValue=bytearray(signalValue) 1857 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1858 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1859 | return c_AppendStringSignal(clientID, signalName, sigV, len(signalValue), operationMode) 1860 | 1861 | def simxWriteStringStream(clientID, signalName, signalValue, operationMode): 1862 | ''' 1863 | Please have a look at the function description/documentation in the V-REP user manual 1864 | ''' 1865 | 1866 | sigV=signalValue 1867 | if sys.version_info[0] == 3: 1868 | if type(signalName) is str: 1869 | signalName=signalName.encode('utf-8') 1870 | if type(signalValue) is bytearray: 1871 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1872 | if type(signalValue) is str: 1873 | signalValue=signalValue.encode('utf-8') 1874 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1875 | else: 1876 | if type(signalValue) is bytearray: 1877 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1878 | if type(signalValue) is str: 1879 | signalValue=bytearray(signalValue) 1880 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1881 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1882 | return c_WriteStringStream(clientID, signalName, sigV, len(signalValue), operationMode) 1883 | 1884 | def simxGetObjectFloatParameter(clientID, objectHandle, parameterID, operationMode): 1885 | ''' 1886 | Please have a look at the function description/documentation in the V-REP user manual 1887 | ''' 1888 | 1889 | parameterValue = ct.c_float() 1890 | return c_GetObjectFloatParameter(clientID, objectHandle, parameterID, ct.byref(parameterValue), operationMode), parameterValue.value 1891 | 1892 | def simxSetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): 1893 | ''' 1894 | Please have a look at the function description/documentation in the V-REP user manual 1895 | ''' 1896 | 1897 | return c_SetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) 1898 | 1899 | def simxGetObjectIntParameter(clientID, objectHandle, parameterID, operationMode): 1900 | ''' 1901 | Please have a look at the function description/documentation in the V-REP user manual 1902 | ''' 1903 | 1904 | parameterValue = ct.c_int() 1905 | return c_GetObjectIntParameter(clientID, objectHandle, parameterID, ct.byref(parameterValue), operationMode), parameterValue.value 1906 | 1907 | def simxSetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): 1908 | ''' 1909 | Please have a look at the function description/documentation in the V-REP user manual 1910 | ''' 1911 | 1912 | return c_SetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) 1913 | 1914 | def simxGetModelProperty(clientID, objectHandle, operationMode): 1915 | ''' 1916 | Please have a look at the function description/documentation in the V-REP user manual 1917 | ''' 1918 | prop = ct.c_int() 1919 | return c_GetModelProperty(clientID, objectHandle, ct.byref(prop), operationMode), prop.value 1920 | 1921 | def simxSetModelProperty(clientID, objectHandle, prop, operationMode): 1922 | ''' 1923 | Please have a look at the function description/documentation in the V-REP user manual 1924 | ''' 1925 | 1926 | return c_SetModelProperty(clientID, objectHandle, prop, operationMode) 1927 | 1928 | def simxStart(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs): 1929 | ''' 1930 | Please have a look at the function description/documentation in the V-REP user manual 1931 | ''' 1932 | 1933 | if (sys.version_info[0] == 3) and (type(connectionAddress) is str): 1934 | connectionAddress=connectionAddress.encode('utf-8') 1935 | return c_Start(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs) 1936 | 1937 | def simxFinish(clientID): 1938 | ''' 1939 | Please have a look at the function description/documentation in the V-REP user manual 1940 | ''' 1941 | 1942 | return c_Finish(clientID) 1943 | 1944 | def simxGetPingTime(clientID): 1945 | ''' 1946 | Please have a look at the function description/documentation in the V-REP user manual 1947 | ''' 1948 | pingTime = ct.c_int() 1949 | return c_GetPingTime(clientID, ct.byref(pingTime)), pingTime.value 1950 | 1951 | def simxGetLastCmdTime(clientID): 1952 | ''' 1953 | Please have a look at the function description/documentation in the V-REP user manual 1954 | ''' 1955 | 1956 | return c_GetLastCmdTime(clientID) 1957 | 1958 | def simxSynchronousTrigger(clientID): 1959 | ''' 1960 | Please have a look at the function description/documentation in the V-REP user manual 1961 | ''' 1962 | 1963 | return c_SynchronousTrigger(clientID) 1964 | 1965 | def simxSynchronous(clientID, enable): 1966 | ''' 1967 | Please have a look at the function description/documentation in the V-REP user manual 1968 | ''' 1969 | 1970 | return c_Synchronous(clientID, enable) 1971 | 1972 | def simxPauseCommunication(clientID, enable): 1973 | ''' 1974 | Please have a look at the function description/documentation in the V-REP user manual 1975 | ''' 1976 | 1977 | return c_PauseCommunication(clientID, enable) 1978 | 1979 | def simxGetInMessageInfo(clientID, infoType): 1980 | ''' 1981 | Please have a look at the function description/documentation in the V-REP user manual 1982 | ''' 1983 | info = ct.c_int() 1984 | return c_GetInMessageInfo(clientID, infoType, ct.byref(info)), info.value 1985 | 1986 | def simxGetOutMessageInfo(clientID, infoType): 1987 | ''' 1988 | Please have a look at the function description/documentation in the V-REP user manual 1989 | ''' 1990 | info = ct.c_int() 1991 | return c_GetOutMessageInfo(clientID, infoType, ct.byref(info)), info.value 1992 | 1993 | def simxGetConnectionId(clientID): 1994 | ''' 1995 | Please have a look at the function description/documentation in the V-REP user manual 1996 | ''' 1997 | 1998 | return c_GetConnectionId(clientID) 1999 | 2000 | def simxCreateBuffer(bufferSize): 2001 | ''' 2002 | Please have a look at the function description/documentation in the V-REP user manual 2003 | ''' 2004 | 2005 | return c_CreateBuffer(bufferSize) 2006 | 2007 | def simxReleaseBuffer(buffer): 2008 | ''' 2009 | Please have a look at the function description/documentation in the V-REP user manual 2010 | ''' 2011 | 2012 | return c_ReleaseBuffer(buffer) 2013 | 2014 | def simxTransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode): 2015 | ''' 2016 | Please have a look at the function description/documentation in the V-REP user manual 2017 | ''' 2018 | 2019 | if (sys.version_info[0] == 3) and (type(filePathAndName) is str): 2020 | filePathAndName=filePathAndName.encode('utf-8') 2021 | return c_TransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode) 2022 | 2023 | def simxEraseFile(clientID, fileName_serverSide, operationMode): 2024 | ''' 2025 | Please have a look at the function description/documentation in the V-REP user manual 2026 | ''' 2027 | 2028 | if (sys.version_info[0] == 3) and (type(fileName_serverSide) is str): 2029 | fileName_serverSide=fileName_serverSide.encode('utf-8') 2030 | return c_EraseFile(clientID, fileName_serverSide, operationMode) 2031 | 2032 | def simxCreateDummy(clientID, size, color, operationMode): 2033 | ''' 2034 | Please have a look at the function description/documentation in the V-REP user manual 2035 | ''' 2036 | 2037 | handle = ct.c_int() 2038 | if color != None: 2039 | c_color = (ct.c_ubyte*12)(*color) 2040 | else: 2041 | c_color = None 2042 | return c_CreateDummy(clientID, size, c_color, ct.byref(handle), operationMode), handle.value 2043 | 2044 | def simxQuery(clientID, signalName, signalValue, retSignalName, timeOutInMs): 2045 | ''' 2046 | Please have a look at the function description/documentation in the V-REP user manual 2047 | ''' 2048 | 2049 | retSignalLength = ct.c_int(); 2050 | retSignalValue = ct.POINTER(ct.c_ubyte)() 2051 | 2052 | sigV=signalValue 2053 | if sys.version_info[0] == 3: 2054 | if type(signalName) is str: 2055 | signalName=signalName.encode('utf-8') 2056 | if type(retSignalName) is str: 2057 | retSignalName=retSignalName.encode('utf-8') 2058 | if type(signalValue) is bytearray: 2059 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 2060 | if type(signalValue) is str: 2061 | signalValue=signalValue.encode('utf-8') 2062 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 2063 | else: 2064 | if type(signalValue) is bytearray: 2065 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 2066 | if type(signalValue) is str: 2067 | signalValue=bytearray(signalValue) 2068 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 2069 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 2070 | 2071 | ret = c_Query(clientID, signalName, sigV, len(signalValue), retSignalName, ct.byref(retSignalValue), ct.byref(retSignalLength), timeOutInMs) 2072 | 2073 | a = bytearray() 2074 | if ret == 0: 2075 | for i in range(retSignalLength.value): 2076 | a.append(retSignalValue[i]) 2077 | if sys.version_info[0] != 3: 2078 | a=str(a) 2079 | 2080 | return ret, a 2081 | 2082 | def simxGetObjectGroupData(clientID, objectType, dataType, operationMode): 2083 | ''' 2084 | Please have a look at the function description/documentation in the V-REP user manual 2085 | ''' 2086 | 2087 | handles =[] 2088 | intData =[] 2089 | floatData =[] 2090 | stringData =[] 2091 | handlesC = ct.c_int() 2092 | handlesP = ct.POINTER(ct.c_int)() 2093 | intDataC = ct.c_int() 2094 | intDataP = ct.POINTER(ct.c_int)() 2095 | floatDataC = ct.c_int() 2096 | floatDataP = ct.POINTER(ct.c_float)() 2097 | stringDataC = ct.c_int() 2098 | stringDataP = ct.POINTER(ct.c_char)() 2099 | ret = c_GetObjectGroupData(clientID, objectType, dataType, ct.byref(handlesC), ct.byref(handlesP), ct.byref(intDataC), ct.byref(intDataP), ct.byref(floatDataC), ct.byref(floatDataP), ct.byref(stringDataC), ct.byref(stringDataP), operationMode) 2100 | 2101 | if ret == 0: 2102 | for i in range(handlesC.value): 2103 | handles.append(handlesP[i]) 2104 | for i in range(intDataC.value): 2105 | intData.append(intDataP[i]) 2106 | for i in range(floatDataC.value): 2107 | floatData.append(floatDataP[i]) 2108 | s = 0 2109 | for i in range(stringDataC.value): 2110 | a = bytearray() 2111 | while stringDataP[s] != b'\0': 2112 | if sys.version_info[0] == 3: 2113 | a.append(int.from_bytes(stringDataP[s],'big')) 2114 | else: 2115 | a.append(stringDataP[s]) 2116 | s += 1 2117 | s += 1 #skip null 2118 | if sys.version_info[0] == 3: 2119 | a=str(a,'utf-8') 2120 | else: 2121 | a=str(a) 2122 | stringData.append(a) 2123 | 2124 | return ret, handles, intData, floatData, stringData 2125 | 2126 | def simxCallScriptFunction(clientID, scriptDescription, options, functionName, inputInts, inputFloats, inputStrings, inputBuffer, operationMode): 2127 | ''' 2128 | Please have a look at the function description/documentation in the V-REP user manual 2129 | ''' 2130 | 2131 | inputBufferV=inputBuffer 2132 | if sys.version_info[0] == 3: 2133 | if type(scriptDescription) is str: 2134 | scriptDescription=scriptDescription.encode('utf-8') 2135 | if type(functionName) is str: 2136 | functionName=functionName.encode('utf-8') 2137 | if type(inputBuffer) is bytearray: 2138 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 2139 | if type(inputBuffer) is str: 2140 | inputBuffer=inputBuffer.encode('utf-8') 2141 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 2142 | else: 2143 | if type(inputBuffer) is bytearray: 2144 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 2145 | if type(inputBuffer) is str: 2146 | inputBuffer=bytearray(inputBuffer) 2147 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 2148 | inputBufferV=ct.cast(inputBufferV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 2149 | 2150 | c_inInts = (ct.c_int*len(inputInts))(*inputInts) 2151 | c_inInts = ct.cast(c_inInts,ct.POINTER(ct.c_int)) # IronPython needs this 2152 | c_inFloats = (ct.c_float*len(inputFloats))(*inputFloats) 2153 | c_inFloats = ct.cast(c_inFloats,ct.POINTER(ct.c_float)) # IronPython needs this 2154 | 2155 | concatStr=''.encode('utf-8') 2156 | for i in range(len(inputStrings)): 2157 | a=inputStrings[i] 2158 | a=a+'\0' 2159 | if type(a) is str: 2160 | a=a.encode('utf-8') 2161 | concatStr=concatStr+a 2162 | c_inStrings = (ct.c_char*len(concatStr))(*concatStr) 2163 | 2164 | intDataOut =[] 2165 | floatDataOut =[] 2166 | stringDataOut =[] 2167 | bufferOut =bytearray() 2168 | 2169 | intDataC = ct.c_int() 2170 | intDataP = ct.POINTER(ct.c_int)() 2171 | floatDataC = ct.c_int() 2172 | floatDataP = ct.POINTER(ct.c_float)() 2173 | stringDataC = ct.c_int() 2174 | stringDataP = ct.POINTER(ct.c_char)() 2175 | bufferS = ct.c_int() 2176 | bufferP = ct.POINTER(ct.c_ubyte)() 2177 | 2178 | ret = c_CallScriptFunction(clientID,scriptDescription,options,functionName,len(inputInts),c_inInts,len(inputFloats),c_inFloats,len(inputStrings),c_inStrings,len(inputBuffer),inputBufferV,ct.byref(intDataC),ct.byref(intDataP),ct.byref(floatDataC),ct.byref(floatDataP),ct.byref(stringDataC),ct.byref(stringDataP),ct.byref(bufferS),ct.byref(bufferP),operationMode) 2179 | 2180 | if ret == 0: 2181 | for i in range(intDataC.value): 2182 | intDataOut.append(intDataP[i]) 2183 | for i in range(floatDataC.value): 2184 | floatDataOut.append(floatDataP[i]) 2185 | s = 0 2186 | for i in range(stringDataC.value): 2187 | a = bytearray() 2188 | while stringDataP[s] != b'\0': 2189 | if sys.version_info[0] == 3: 2190 | a.append(int.from_bytes(stringDataP[s],'big')) 2191 | else: 2192 | a.append(stringDataP[s]) 2193 | s += 1 2194 | s += 1 #skip null 2195 | if sys.version_info[0] == 3: 2196 | a=str(a,'utf-8') 2197 | else: 2198 | a=str(a) 2199 | stringDataOut.append(a) 2200 | for i in range(bufferS.value): 2201 | bufferOut.append(bufferP[i]) 2202 | if sys.version_info[0] != 3: 2203 | bufferOut=str(bufferOut) 2204 | 2205 | return ret, intDataOut, floatDataOut, stringDataOut, bufferOut 2206 | 2207 | def simxGetObjectVelocity(clientID, objectHandle, operationMode): 2208 | ''' 2209 | Please have a look at the function description/documentation in the V-REP user manual 2210 | ''' 2211 | linearVel = (ct.c_float*3)() 2212 | angularVel = (ct.c_float*3)() 2213 | ret = c_GetObjectVelocity(clientID, objectHandle, linearVel, angularVel, operationMode) 2214 | arr1 = [] 2215 | for i in range(3): 2216 | arr1.append(linearVel[i]) 2217 | arr2 = [] 2218 | for i in range(3): 2219 | arr2.append(angularVel[i]) 2220 | return ret, arr1, arr2 2221 | 2222 | def simxPackInts(intList): 2223 | ''' 2224 | Please have a look at the function description/documentation in the V-REP user manual 2225 | ''' 2226 | 2227 | if sys.version_info[0] == 3: 2228 | s=bytes() 2229 | for i in range(len(intList)): 2230 | s=s+struct.pack('