├── .gitignore ├── LICENSE ├── README.md ├── controller.py ├── loadscene.py ├── pid.py ├── reinforcementLearner.py ├── rl_simulation.py ├── simulation.py ├── tune.sh ├── twiddle.py ├── util.py ├── vrep.py ├── vrep ├── segway.ttt ├── segway_big.ttt ├── segway_big_body.ttt └── segway_middle.ttt └── vrepConst.py /.gitignore: -------------------------------------------------------------------------------- 1 | # V-REP libs 2 | *.so 3 | *.dylib 4 | 5 | # Python temporaries 6 | *.pyc 7 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2015 Pascal Becker, Tarmo Räntilä 4 | Copyright (c) 2018 Pascal Becker 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # roboticsControl 2 | 3 | Repository for the class 'Software Technology Course with a Varying Content' and the topic 'Introduction in Artificial Intelligence'. 4 | 5 | This repository contains two different controlling algorithms. PID and Reinforcement learning (Q-Learning). The purpose of the lecture was to control a segway-like platform and compare the controlling behavior. 6 | 7 | The result is that the PID controller is able to control the model. But it takes a lot of optimization to get the best parameters. 8 | Also the reinforcement learning agent is able to control the model. It is learning really fast and after four to five failures it balances the segway multiple minutes in place. 9 | 10 | Some useful links: 11 | 12 | - [V-Rep Doku](http://coppeliarobotics.com/helpFiles/index.html) 13 | 14 | # Installation 15 | 16 | 1. Download, extract and install the education version of VREP ([Download-Link](http://www.coppeliarobotics.com/downloads.html)) 17 | 2. Clone this repository 18 | 3. Start VREP and open one of the scenes in the folder *vrep* 19 | - There are three different sizes of inverted pendulums. All work but have different behavior as mass and height changes 20 | 4. Run *python rl_simulation.py* from a console 21 | 5. See how the the reinforcement learning agent learns to balance the inverted pendulum 22 | 23 | # More Information 24 | If you want to have more information of the reinforcement agent, change the debug flag at line 14 in the file *rl_simulation.py* from 0 to 1 25 | 26 | # PID 27 | In this section you find the evaluated values for a good controlling with a PID controller 28 | 29 | ## PID tunings 30 | 31 | A few different tunings found out to work more or less well. 32 | 33 | ### Bullet engine 34 | 35 | #### |a|: 36 | - 50ms - 13.7 0.199 1286 37 | - 10ms - 56.25, 2.736, 1389 38 | 39 | #### a**2 + v**2: 40 | - 50ms - 13.76 0.1990 1609 41 | 42 | #### a**2 + |x| 43 | - 50ms - 13.7 0.2197 1194 44 | 45 | #### a**2 + x**2 46 | - 50ms - 36.72, 0.5333, 2036 (bad) 47 | - 10ms - ? 48 | 49 | #### a**2 + x**2 / t**2 50 | - 10ms - 26.11, 1.971, 2925 51 | 52 | #### |a| + |x| + |v| / t^2 53 | - 10ms - 12.73 0.3582 3042 54 | 55 | #### Pitch-delta 56 | - Doomed to drift... 57 | - 50ms - 0.5, 0.005, 12.5 58 | 59 | #### Oldies but goldies 60 | - 50ms: 21.000000 9.009500 16.550000 61 | - 10ms: 79.0 19.9 41.4 62 | 63 | ### ODE engine 64 | 65 | #### a**2 + x**2 / t**2 66 | - 10ms - 324 3.24 4500 67 | 68 | #### |a| + |x| + |v| / t^2 69 | - 10ms - 80.77 0.5011 330.0 70 | 71 | ## Reinforcement Learning / Q-Learning 72 | 73 | The basic Q-Learning approach is used to implement an easy and fast solution to control the segway. 74 | -------------------------------------------------------------------------------- /controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | from math import sqrt, log10, pi 4 | from vrep import * 5 | from util import * 6 | from pid import PID 7 | 8 | M_PI = 3.14159265359 9 | 10 | class SegwayController(object): 11 | 12 | def __init__(self, client): 13 | self.client = client 14 | self.current_position = 0 15 | 16 | def setup(self, body_name="body", left_motor_name="leftMotor", right_motor_name="rightMotor"): 17 | self.setup_body(body_name) 18 | self.setup_motors(left_motor_name, right_motor_name) 19 | self.setup_streaming() 20 | 21 | def setup_body(self, body_name): 22 | err, self.body = simxGetObjectHandle(self.client, body_name, simx_opmode_oneshot_wait) 23 | if err: 24 | log(self.client, 'ERROR GetObjectHandle code %d' % err) 25 | 26 | def setup_motors(self, left_motor_name, right_motor_name): 27 | err_l, self.left_motor = simxGetObjectHandle(self.client, left_motor_name, simx_opmode_oneshot_wait) 28 | err_r, self.right_motor = simxGetObjectHandle(self.client, right_motor_name, simx_opmode_oneshot_wait) 29 | err = err_l or err_r 30 | if err: 31 | log(self.client, 'ERROR GetObjectHandle code %d' % err) 32 | self._send_target_velocities(0.0, 0.0, simx_opmode_oneshot_wait) 33 | 34 | def setup_wheels(self, left_wheel, right_wheel): 35 | err_l, self.left_wheel = simxGetObjectHandle(self.client, left_wheel, simx_opmode_oneshot_wait) 36 | err_r, self.right_wheel = simxGetObjectHandle(self.client, right_wheel, simx_opmode_oneshot_wait) 37 | err = err_l or err_r 38 | if err: 39 | log(self.client, 'ERROR GetObjectHandle code %d' % err) 40 | 41 | def setup_sensors(self, gyro, height): 42 | # Placeholder 43 | pass 44 | 45 | def setup_streaming(self): 46 | # Setup V-REP streaming 47 | simxGetObjectOrientation(self.client, self.body, -1, simx_opmode_streaming) 48 | simxGetObjectVelocity(self.client, self.body, simx_opmode_streaming) 49 | simxGetObjectPosition(self.client, self.body, -1, simx_opmode_streaming) 50 | if 'left_wheel' in locals(): 51 | simxGetObjectVelocity(self.client, self.left_wheel, simx_opmode_streaming) 52 | simxGetObjectVelocity(self.client, self.right_wheel, simx_opmode_streaming) 53 | 54 | def set_target_velocities(self, left_vel, right_vel): 55 | # Pause comms to sync the orders 56 | simxPauseCommunication(self.client, True) 57 | self._send_target_velocities(left_vel, right_vel, simx_opmode_streaming) 58 | # Re-enable comms to push the commands 59 | simxPauseCommunication(self.client, False) 60 | 61 | def _send_target_velocities(self, left_vel, right_vel, opmode): 62 | err_l = None 63 | err_r = None 64 | if left_vel is not None: 65 | err_l = simxSetJointTargetVelocity(self.client, self.left_motor, 66 | left_vel, opmode) 67 | if right_vel is not None: 68 | err_r = simxSetJointTargetVelocity(self.client, self.right_motor, 69 | right_vel, opmode) 70 | err = err_l or err_r 71 | if err > 1: 72 | log(self.client, 'ERROR SetJointTargetVelocity code %d' % err) 73 | 74 | def setup_control(self, balance_controller): 75 | self.balance_controller = balance_controller 76 | 77 | ##### END CONDITIONS ######################################################### 78 | 79 | def zero_velocity_condition(self, lin_vel, rot_vel): 80 | # Check if velocity is near zero (could cause issues on first cycle!) 81 | vel_tot = sqrt(reduce(lambda total, value: total + value**2, 82 | lin_vel, 83 | 0.0)) 84 | log(self.client, 'Total velocity: %f' % vel_tot) 85 | return vel_tot > 10.0 ** -5 86 | 87 | def body_height_condition(self, body_pos): 88 | x, y, z = body_pos 89 | return z > 0.04 # Wheel radius is 0.08m 90 | 91 | def get_current_angle(self): 92 | err_or, angle = simxGetObjectOrientation(self.client, self.body, -1, simx_opmode_streaming) 93 | if err_or > 0: 94 | print "Error while getting angle" 95 | # Returns angle (RPY) 96 | return angle 97 | 98 | def get_current_position(self): 99 | err_pos, pos = simxGetObjectPosition(self.client, self.body, -1, simx_opmode_streaming) 100 | if err_pos > 0: 101 | print "Error while getting position" 102 | return pos 103 | 104 | def get_current_ground_speed(self): 105 | err_vel, lin_vel, rot_vel = simxGetObjectVelocity(self.client, self.body, simx_opmode_streaming) 106 | if err_vel > 0: 107 | print "Error while getting velocity" 108 | return lin_vel 109 | 110 | def get_current_angle_speed(self, part_name="rightMotor"): 111 | err_vel, lin_vel, rot_vel = simxGetObjectVelocity(self.client, self.body, simx_opmode_streaming) 112 | if err_vel > 0: 113 | print "Error while getting velocity" 114 | return rot_vel 115 | 116 | def simulation_run_condition(self, simulation_time, body_pos): 117 | if simulation_time < 100: 118 | return True 119 | else: 120 | x, y, z = body_pos 121 | # Wheel radius 0.25m, box length 1.5m -> pos @1m, time in ms 122 | height_condition = 0.3 < z < 1.1 123 | drive_condition = sqrt(x**2 + y**2) < 2.0 124 | time_condition = simulation_time < 30000 125 | return height_condition and drive_condition and time_condition 126 | 127 | def run(self, condition=None): 128 | # Default condition to something sensible 129 | condition = condition if condition else self.simulation_run_condition 130 | 131 | simulation_time_current = 0 132 | simulation_time_previous = 0 # ms 133 | cost = 0.0 134 | ok = True 135 | 136 | self.setup_streaming() 137 | 138 | while ok and simxGetConnectionId(self.client) != -1: 139 | simxPauseCommunication(self.client, True) 140 | err_rot, euler_angles = simxGetObjectOrientation(self.client, self.body, -1, simx_opmode_buffer) 141 | err_vel, lin_vel, rot_vel = simxGetObjectVelocity(self.client, self.body, simx_opmode_buffer) 142 | err_pos, position = simxGetObjectPosition(self.client, self.body, -1, simx_opmode_buffer) 143 | err_wheel_left_vel, wheel_left_lin_vel, wheel_left_rot_vel = simxGetObjectVelocity(self.client, self.left_wheel, simx_opmode_streaming) 144 | err_wheel_right_vel, wheel_right_lin_vel, wheel_right_rot_vel = simxGetObjectVelocity(self.client, self.right_wheel, simx_opmode_streaming) 145 | simxPauseCommunication(self.client, False) 146 | 147 | err = err_rot or err_vel or err_pos or err_wheel_left_vel or err_wheel_right_vel 148 | if err > 1: 149 | print "-- No data right now!" 150 | continue 151 | 152 | # Check whether new commands have been executed 153 | simulation_time_current = simxGetLastCmdTime(self.client) 154 | if simulation_time_previous == simulation_time_current: 155 | continue 156 | # Calculate dt now that we have times available 157 | dt = simulation_time_current - simulation_time_previous 158 | # Store the time spent until last fetch'd value 159 | simulation_time_previous = simulation_time_current 160 | 161 | # Calculate and set control 162 | roll, pitch, yaw = euler_angles 163 | droll, dpitch, dyaw = rot_vel 164 | vx, vy, vz = lin_vel 165 | x, y, z = position 166 | dtilt_left = tilt_from_rp(wheel_left_rot_vel[0], wheel_left_rot_vel[1]) 167 | dtilt_right = tilt_from_rp(wheel_right_rot_vel[0], wheel_right_rot_vel[1]) 168 | dtilt = (dtilt_left + dtilt_right) / 2 169 | control = self.balance_controller.control(pitch, dt) 170 | self.set_target_velocities(control, control) 171 | 172 | # Calculcate the current cost and sum it to the total only after 100ms to prevent a weird spike on first cycles 173 | if simulation_time_current > 100: 174 | cost += abs(tilt_from_rp(roll, pitch)) + abs((pi/2)*x) + abs(dtilt) 175 | 176 | # Check for continuing 177 | ok = condition(simulation_time_current, position) 178 | 179 | return (log10(cost / max(simulation_time_current, 1)**2), simulation_time_current) 180 | 181 | 182 | # log(self.client, 'Euler angles: ' + str(euler_angles)) 183 | # log(self.client, 'Control value: ' + str(control)) 184 | # log(self.client, 'Cost on cycle: ' + str(cost)) 185 | # log(self.client, 'Cost (final): ' + str(cost)) 186 | # log(self.client, 'Cost (final 2): ' + str(cost / niterations)) 187 | 188 | if __name__ == '__main__': 189 | print '-- Please use simulation.py instead!' 190 | -------------------------------------------------------------------------------- /loadscene.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from vrep import * 3 | 4 | print '-- Loading scene for V-REP' 5 | 6 | simxFinish(-1) 7 | client = simxStart('127.0.0.1', 19997, True, True, 5000, 5) 8 | 9 | if client != -1: 10 | simxStopSimulation(client, simx_opmode_oneshot_wait) 11 | 12 | # 1 for path relative to client 13 | err = simxLoadScene(client, 'vrep/segway.ttt', 1, simx_opmode_oneshot_wait) 14 | if err: 15 | print '-- ERROR loading scene: %d' % (err) 16 | else: 17 | print '-- Failed connecting to remote API server' 18 | 19 | print '-- Scene loaded!' 20 | -------------------------------------------------------------------------------- /pid.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | class PID: 5 | """ 6 | A PID controller sporting an option to do accumulator min/max anti-windup. 7 | """ 8 | 9 | def __init__(self, Kp=1.0, Ki=0.0, Kd=0.0, reference=None, initial=None): 10 | self.Kp = Kp 11 | self.Ki = Ki 12 | self.Kd = Kd 13 | self.reference = reference 14 | self.previous_error = 0.0 if (reference is None or initial is None) else (reference - initial) 15 | self.accumulated_error = 0.0 16 | self.anti_windup = False 17 | 18 | def control(self, input, dt=1, reference=None): 19 | """ 20 | Compute a control value for @input. If no @reference is used, fall 21 | back to self.reference. If no @dt is provided, fall back to discrete 1. 22 | """ 23 | # Calculate new error and accumulate 24 | error = (self.reference if reference is None else reference) - input 25 | self.accumulated_error += error * dt 26 | error_diff = (error - self.previous_error) / dt 27 | # Check for accumulator limits 28 | if (self.anti_windup): 29 | if self.accumulated_error < self.accumulator_min: 30 | self.accumulated_error = self.accumulator_min 31 | elif self.accumulated_error > self.accumulator_max: 32 | self.accumulated_error = self.accumulated_max 33 | # Calculate control output 34 | P_term = self.Kp * error 35 | D_term = self.Kd * error_diff 36 | I_term = self.Ki * self.accumulated_error 37 | control = P_term + I_term + D_term 38 | # Store current error 39 | self.previous_error = error 40 | # Return control value 41 | return control 42 | 43 | def anti_windup(self, acc_min, acc_max=None): 44 | """ 45 | @acc_min false for disabling 46 | @acc_max defaults to -@acc_min 47 | """ 48 | self.anti_windup = True if acc_min is not False else False 49 | self.accumulator_min = acc_min 50 | self.accumulator_max = acc_max if acc_max is not None else -acc_min 51 | 52 | 53 | if __name__ == "__main__": 54 | from math import * 55 | import random 56 | 57 | class robot: 58 | """ 59 | This is the robot class from Udacity 60 | """ 61 | 62 | def __init__(self, length = 20.0): 63 | """ 64 | creates robot and initializes location/orientation to 0, 0, 0 65 | """ 66 | self.x = 0.0 67 | self.y = 0.0 68 | self.orientation = 0.0 69 | self.length = length 70 | self.steering_noise = 0.0 71 | self.distance_noise = 0.0 72 | self.steering_drift = 0.0 73 | 74 | def set(self, new_x, new_y, new_orientation): 75 | """ 76 | sets a robot coordinate 77 | """ 78 | self.x = float(new_x) 79 | self.y = float(new_y) 80 | self.orientation = float(new_orientation) % (2.0 * pi) 81 | 82 | def set_noise(self, new_s_noise, new_d_noise): 83 | """ 84 | sets the noise parameters 85 | makes it possible to change the noise parameters 86 | this is often useful in particle filters 87 | """ 88 | self.steering_noise = float(new_s_noise) 89 | self.distance_noise = float(new_d_noise) 90 | 91 | def set_steering_drift(self, drift): 92 | """ 93 | sets the systematical steering drift parameter 94 | """ 95 | self.steering_drift = drift 96 | 97 | def move(self, steering, distance, 98 | tolerance=0.001, max_steering_angle=pi/4.0): 99 | """ 100 | steering = front wheel steering angle, limited by max_steering_angle 101 | distance = total distance driven, most be non-negative 102 | """ 103 | if steering > max_steering_angle: 104 | steering = max_steering_angle 105 | if steering < -max_steering_angle: 106 | steering = -max_steering_angle 107 | if distance < 0.0: 108 | distance = 0.0 109 | 110 | # make a new copy 111 | res = robot() 112 | res.length = self.length 113 | res.steering_noise = self.steering_noise 114 | res.distance_noise = self.distance_noise 115 | res.steering_drift = self.steering_drift 116 | 117 | # apply noise 118 | steering2 = random.gauss(steering, self.steering_noise) 119 | distance2 = random.gauss(distance, self.distance_noise) 120 | 121 | # apply steering drift 122 | steering2 += self.steering_drift 123 | 124 | # Execute motion 125 | turn = tan(steering2) * distance2 / res.length 126 | 127 | if abs(turn) < tolerance: 128 | # approximate by straight line motion 129 | res.x = self.x + (distance2 * cos(self.orientation)) 130 | res.y = self.y + (distance2 * sin(self.orientation)) 131 | res.orientation = (self.orientation + turn) % (2.0 * pi) 132 | else: 133 | # approximate bicycle model for motion 134 | radius = distance2 / turn 135 | cx = self.x - (sin(self.orientation) * radius) 136 | cy = self.y + (cos(self.orientation) * radius) 137 | res.orientation = (self.orientation + turn) % (2.0 * pi) 138 | res.x = cx + (sin(res.orientation) * radius) 139 | res.y = cy - (cos(res.orientation) * radius) 140 | 141 | return res 142 | 143 | def __repr__(self): 144 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 145 | 146 | 147 | def run(param1, param2, param3): 148 | # Robot 149 | myrobot = robot() 150 | myrobot.set(0.0, 1.0, 0.0) 151 | speed = 1.0 # motion distance is equal to speed (we assume time = 1) 152 | N = 100 153 | myrobot.set_steering_drift(10.0 / 180.0 * pi) 154 | 155 | # PID & results 156 | results = [] 157 | pid = PID(param1, param3, param2, 0.0, myrobot.y) 158 | 159 | # Loop some 160 | for i in range(N): 161 | steering = pid.control(myrobot.y) 162 | myrobot = myrobot.move(steering, speed) 163 | results.append(myrobot.y) 164 | 165 | return results 166 | 167 | # Call your function with parameters of (0.2, 3.0, and 0.004) 168 | results = run(0.2, 3.0, 0.004) 169 | 170 | if results[len(results)-1] > 0.06: 171 | print("It' broken, man! %.2f / 0.06" % (results[len(results)-1])) 172 | else: 173 | print("Goog enough, man!") 174 | -------------------------------------------------------------------------------- /reinforcementLearner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | one_degree = 0.0174532 # 2pi/360 4 | six_degrees = 0.1047192 5 | twelve_degrees = 0.2094384 6 | fifty_degrees = 0.87266 7 | 8 | # approach based on the example from 'Reinforcement Learning: An Introduction' by Richard S. Sutton and Andrew G. Barto (1998) 9 | # Q-Learning with look-up table 10 | 11 | class ReinforcementLearner(): 12 | 13 | # initialize a new ReinforcementLearner with some default parameters 14 | def __init__(self, controller, alpha=1000, beta=0.5, gamma=0.95, lambda_w=0.9, lambda_v=0.8, max_failures=50, max_steps=1000000, max_distance=2.4, max_speed=1, max_angle_factor=12): 15 | self.n_states = 162 # 3x3x6x3 = 162 states 16 | self.alpha = alpha # learning rate for action weights 17 | self.beta = beta # learning rate for critic weights 18 | self.gamma = gamma # discount factor for critic 19 | self.lambda_w = lambda_w # decay rate for action weights 20 | self.lambda_v = lambda_v # decay rate for critic weights 21 | self.max_failures = max_failures 22 | self.max_steps = max_steps 23 | 24 | self.max_distance = max_distance 25 | self.max_speed = max_speed 26 | self.max_angle = max_angle_factor * one_degree 27 | 28 | self.action_weights = [0] * self.n_states # action weights 29 | self.critic_weights = [0] * self.n_states # critic weights 30 | self.action_weights_elig = [0] * self.n_states # action weight eligibilities 31 | self.critic_weights_elig = [0] * self.n_states # critic weight eligibilities 32 | 33 | #position, velocity, angle, angle velocity 34 | self.x, self.dx, self.t, self.dt = 0, 0, 0, 0 35 | 36 | self.controller = controller 37 | 38 | # matches the current state to an integer between 1 and n_states 39 | # 3 states for position x: -max_distance < x < -0.8, -0.8 < x < 0.8, 0.8 < x < 2.4 40 | # 3 states for velocity dx: dx < -0.5, -0.5 < dx < 0.5, 0.5 < dx 41 | # 6 states for angle t: t < -6, -6 < t < -1, -1 < t < 0, 0 < t < 1, 1 < t < 6, t < 6 42 | # 3 states for angle velocity dt: dt < -50, -50 < dt < 50, 50 < dt 43 | # --> 3x3x6x3 = 162 states for the look-up table 44 | def get_state(self): 45 | state = 0 46 | 47 | # failed 48 | if self.x < -self.max_distance or self.x > self.max_distance or self.t < -self.max_angle or self.t > self.max_angle: 49 | return -1 50 | 51 | #position 52 | if self.x < -0.8: 53 | state = 0 54 | elif self.x < 0.8: 55 | state = 1 56 | else: 57 | state = 2 58 | 59 | #velocity 60 | if self.dx < -self.max_speed: 61 | state += 0 62 | elif self.dx < 0.5: 63 | state += 3 64 | else: 65 | state += 6 66 | 67 | #angle 68 | if self.t < -six_degrees: 69 | state += 0 70 | elif self.t < -one_degree: 71 | state += 9 72 | elif self.t < 0: 73 | state += 18 74 | elif self.t < one_degree: 75 | state += 27 76 | elif self.t < six_degrees: 77 | state += 36 78 | else: 79 | state += 45 80 | 81 | #angle velocity 82 | if self.dt < -fifty_degrees: 83 | state += 0 84 | elif self.dt < fifty_degrees: 85 | state += 54 86 | else: 87 | state += 108 88 | 89 | return state 90 | 91 | # read the variables x, dx, t and dt from vrep 92 | def read_variables(self): 93 | self.x = self.controller.get_current_position()[0] 94 | self.dx = self.controller.get_current_ground_speed()[0] 95 | self.t = self.controller.get_current_angle()[1] 96 | self.dt = self.controller.get_current_angle_speed()[1] 97 | 98 | # executes action and updates x, dx, t, dt 99 | # action size is two: left and right 100 | def do_action(self, action): 101 | if action == True: 102 | self.controller.set_target_velocities(self.max_speed,self.max_speed) 103 | else: 104 | self.controller.set_target_velocities(-self.max_speed,-self.max_speed) 105 | 106 | # update all weights or reset them when failed 107 | def update_all_weights(self, rhat, failed): 108 | for i in range(self.n_states): 109 | self.action_weights[i] += self.alpha * rhat * self.action_weights_elig[i] 110 | self.critic_weights[i] += self.beta * rhat * self.critic_weights_elig[i] 111 | 112 | if self.critic_weights[i] < -1.0: 113 | self.critic_weights[i] = self.critic_weights[i] 114 | 115 | if failed == True: 116 | self.action_weights_elig[i] = 0 117 | self.critic_weights_elig[i] = 0 118 | else: 119 | self.action_weights_elig[i] = self.action_weights_elig[i] * self.lambda_w 120 | self.critic_weights_elig[i] = self.critic_weights_elig[i] * self.lambda_v -------------------------------------------------------------------------------- /rl_simulation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import random 3 | import math 4 | import sys 5 | from time import * 6 | 7 | from controller import * 8 | from reinforcementLearner import * 9 | from util import * 10 | from vrep import * 11 | 12 | # Python file to launch simulation and handling vrep 13 | 14 | debug = 0 15 | 16 | if __name__ == '__main__': 17 | 18 | print '-- Starting master client' 19 | simxFinish(-1) # just in case, close all opened connections 20 | 21 | # localhost:19997 connects to V-REP global remote API 22 | addr = '127.0.0.1' 23 | port = 19997 24 | print '-- Connecting to %s:%d' % (addr, port) 25 | client = simxStart(addr, port, True, True, 5000, 5) 26 | 27 | if client != -1: 28 | log(client, 'Master client connected to client %d at port %d' % (client, port)) 29 | 30 | if debug == 1: 31 | err, objs = simxGetObjects(client, sim_handle_all, simx_opmode_oneshot_wait) 32 | if err == simx_return_ok: 33 | log(client, 'Number of objects in the scene: %d' % len(objs)) 34 | else: 35 | log(client, 'ERROR GetObjects code %d' % err) 36 | 37 | # initialize controller for handling vrep connection 38 | controller = SegwayController(client) 39 | controller.setup("body", "leftMotor", "rightMotor") 40 | 41 | # create a new reinforcement learner 42 | cart = ReinforcementLearner(controller) 43 | 44 | # initialize values 45 | p, oldp, rhat, r = 0, 0, 0, 0 46 | state, i, y, steps, failures, failed, startSim = 0, 0, 0, 0, 0, False, True 47 | 48 | while steps < cart.max_steps and failures < cart.max_failures: 49 | # start simulation in the first step 50 | if startSim == True: 51 | err = simxStartSimulation(controller.client, simx_opmode_oneshot_wait) 52 | now = int(time()) 53 | # get start state 54 | cart.read_variables() 55 | state = cart.get_state() 56 | startSim = False 57 | if debug == 1: 58 | print "----------------------- RESTART --------------------------" 59 | 60 | # guess new action depending on the weight of the current state 61 | action = (random.random()/((2**31) - 1) < (1.0 / (1.0 + math.exp(-max(-50, min(cart.action_weights[state], 50)))))) 62 | if debug == 1: 63 | print "action: " + str(action) 64 | 65 | #update traces 66 | cart.action_weights_elig[state] += (1 - cart.lambda_w) * (y - 0.5) 67 | cart.critic_weights_elig[state] += (1 - cart.lambda_v) 68 | oldp = cart.critic_weights[state] # remember prediction for the current state 69 | cart.do_action(action) # do action 70 | cart.read_variables() # read new values TODO maybe a bit to close after doing action?! 71 | state = cart.get_state() # get new x, dx, t, dt 72 | if debug == 1: 73 | print "state: " + str(state) + " x: " + str(cart.x) + " t: " + str(cart.t) 74 | 75 | # failure 76 | if state < 0: 77 | failed = True 78 | failures += 1 79 | print "Trial " + str(failures) + " was " + str(steps) + " steps or " + str(int(time()) - now) + " seconds" 80 | steps = 0 81 | # restart simulation and get initial start values 82 | err = simxStopSimulation(controller.client, simx_opmode_oneshot_wait) 83 | sleep(0.5) 84 | state = cart.get_state() 85 | cart.read_variables() 86 | r = -1 # reward = -1 87 | p = 0 # prediction of failure 88 | startSim = True 89 | else: # no failure 90 | failed = False 91 | r = 0 # reward = 0 92 | p = cart.critic_weights[state] 93 | 94 | rhat = r + cart.gamma * p - oldp 95 | 96 | # update all weights 97 | cart.update_all_weights(rhat, failed) 98 | 99 | # going to next step 100 | steps += 1 101 | 102 | # finished the loop 103 | if failures == cart.max_failures: 104 | print "Pole not balanced. Stopping after " + str(failures) + " failures \n" 105 | else: 106 | print "Pole balanced successfully for at least " + str(steps) + " steps \n" 107 | print "critic weights: " + str(cart.critic_weights) 108 | print "action weights: " + str(cart.action_weights) -------------------------------------------------------------------------------- /simulation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import argparse 4 | from time import sleep 5 | from vrep import * 6 | from util import * 7 | from controller import * 8 | from twiddle import * 9 | 10 | 11 | class SimulationController(object): 12 | """ 13 | A class for running the simulation. Mostly useful for holding data such as 14 | the client object. 15 | """ 16 | 17 | def __init__(self, client, controller_class=SegwayController, tuner_class=Twiddle): 18 | self.client = client 19 | # Instantiate controller and tuner objects from the provided classes 20 | self.controller = controller_class(client) 21 | self.tuner = tuner_class() 22 | 23 | def setup(self, body="Payload", left_motor="MotorLeft", right_motor="MotorRight", 24 | left_wheel="WheelLeft", right_wheel="WheelRight"): 25 | """ 26 | Setup object handles and possible other V-REP related things. 27 | """ 28 | self.controller.setup_body(body) 29 | self.controller.setup_motors(left_motor, right_motor) 30 | self.controller.setup_wheels(left_wheel, right_wheel) 31 | 32 | def setup_tuner(self, **kwargs): 33 | self.tuner.setup(**kwargs) 34 | 35 | def single_run(self, parameters): 36 | """ 37 | A function to pass for the twiddle implementation. [P, I, D] -> error 38 | """ 39 | # Create a PID by parameters 40 | P, I, D = parameters 41 | balance_PID = PID(P, I, D, 0.0, 0.0) 42 | self.controller.setup_control(balance_PID) 43 | log(self.client, 'New simulation with (%f, %f, %f)' % (P, I, D)) 44 | err = simxStartSimulation(self.client, simx_opmode_oneshot_wait) 45 | if err > 1: 46 | log(self.client, 'ERROR StartSimulation code %d' % err) 47 | # Do the control, returns when end condition is reached 48 | cost, simulation_time = self.controller.run() 49 | # Stop the simulation (e.g. fell down, time up) 50 | err = simxStopSimulation(self.client, simx_opmode_oneshot_wait) 51 | if err > 1: 52 | log(self.client, 'ERROR StopSimulation code %d' % err) 53 | log(self.client, 'Simulation results to cost %f (#%d)' % (cost, simulation_time)) 54 | # Wait some time to prevent V-REP lagging behind 55 | sleep(0.1) 56 | return cost, simulation_time 57 | 58 | def run(self): 59 | """ 60 | Run the simulation continuously and tune the parameters. Ctrl-Cs are 61 | caught on the self.tuner.tune function. Returns the best parameters. 62 | """ 63 | return self.tuner.tune(self.single_run) 64 | 65 | 66 | ############################################################################## 67 | # MAIN 68 | ############################################################################## 69 | if __name__ == '__main__': 70 | # Parse args 71 | parser = argparse.ArgumentParser() 72 | parser.add_argument("-o", "--one-shot", action="store_true", help="Do a single") 73 | parser.add_argument("-p", "--params", nargs=3, type=float, metavar="P", help="PID gains in a list: [KP, KI, KD]", default=[13.7, 0.199, 1286]) 74 | parser.add_argument("-d", "--deltas", nargs=3, type=float, metavar="dP", help="Twiddle PID gain deltas in a list: [dKP, dKI, dKD]", default=None) 75 | args = parser.parse_args() 76 | 77 | print '-- Starting master client' 78 | 79 | # Close all connection, just in case 80 | simxFinish(-1) 81 | 82 | # localhost:19997 connects to V-REP global remote API 83 | addr = '127.0.0.1' 84 | port = 19997 85 | print '-- Connecting to %s:%d' % (addr, port) 86 | client = simxStart(addr, port, True, True, 5000, 5) 87 | 88 | if client != -1: 89 | log(client, 'Master client connected to client %d at port %d' % (client, port)) 90 | 91 | print '-- Halt pending simulations' 92 | simxStopSimulation(client, simx_opmode_oneshot_wait) 93 | 94 | # Create a simulation controller to run the tuning 95 | simulation_controller = SimulationController(client) 96 | # Defaults will do for the setup unless we change the model 97 | simulation_controller.setup() 98 | 99 | # Tuning run 100 | if not args.one_shot: 101 | # Setup twiddle 102 | deltas = args.deltas if args.deltas else map(lambda x: 0.8*x, args.params[:]) 103 | simulation_controller.setup_tuner(params=args.params, deltas=deltas) 104 | # Run tuner 105 | best_params, best_cost, best_cost_time = simulation_controller.run() 106 | print "--- RESULTS ---" 107 | print "Best params (cost) #ms:" 108 | print str(best_params) + " (" + str(best_cost) + ") #" + str(best_cost_time) 109 | 110 | # One shot 111 | else: 112 | simulation_controller.single_run(map(float, args.params)) # [sys.argv[1], sys.argv[2], sys.argv[3]] 113 | 114 | # Force stop of simulation under all circumstances 115 | print "-- Try to stop simulation (it is difficult!)" 116 | simxStopSimulation(client, simx_opmode_oneshot_wait) 117 | else: 118 | print '-- Failed connecting to remote API server' 119 | 120 | print '-- Terminating master client' 121 | -------------------------------------------------------------------------------- /tune.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # Script directory 3 | DIR=$( cd "$( dirname "$0" )" && pwd ) 4 | VREP_WAIT=5s 5 | SCENE_WAIT=2s 6 | # Execute headless V-REP 7 | cd $DIR/V-REP/ 8 | echo ">> Launching V-REP" 9 | ./vrep.sh -h & 10 | echo ">> V-REP launching, wait for $VREP_WAIT" 11 | sleep $VREP_WAIT 12 | echo ">> Waited for $VREP_WAIT!" 13 | cd $DIR 14 | echo ">> Load V-REP scene" 15 | python loadscene.py 16 | echo ">> Done loading scene" 17 | echo ">> Wait for $SCENE_WAIT" 18 | sleep $SCENE_WAIT 19 | echo ">> Start twiddling" 20 | ./simulation.py "$@" 21 | echo ">> End of twiddling" 22 | echo ">> Now kill all!" 23 | #pkill -TERM -P $$ 24 | pkill -TERM -P $(pgrep -P $$) 25 | sleep 0.1s 26 | echo ">> All dead" 27 | exit 28 | -------------------------------------------------------------------------------- /twiddle.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | class Twiddle: 5 | """ 6 | Twiddler class 7 | """ 8 | 9 | def __init__(self, params=[0, 0, 0], deltas=[2, 1, 1], tolerance=10**-6): 10 | self.params = params 11 | self.deltas = deltas 12 | self.tolerance = tolerance 13 | 14 | def setup(self, **kwargs): 15 | self.params = kwargs.get("params", self.params) 16 | self.deltas = kwargs.get("deltas", self.deltas) 17 | self.tolerance = kwargs.get("tolerance", self.tolerance) 18 | 19 | def tune(self, error_function): 20 | """ 21 | Calculate the tuned parameters 22 | """ 23 | 24 | # Catch ctrl-Cs here since the interesting data (best params) are here 25 | try: 26 | # Initialize a buffered vector for the best parameters found for 27 | # cleaner keyboard interrupt handling 28 | best_params = self.params[:] 29 | # Initial run 30 | error_min, error_min_time = error_function(self.params) 31 | # Coordinate descent -> tolerance will be on the param deltas 32 | while sum(map(abs, self.deltas)) > self.tolerance: # error_min > self.tolerance 33 | for i in range(len(self.params)): 34 | self.params[i] += self.deltas[i] 35 | error, sim_time = error_function(self.params) 36 | if error < error_min: 37 | # New best result 38 | error_min = float(error) 39 | error_min_time = int(sim_time) 40 | best_params = self.params[:] # Deep-ish copy 41 | self.deltas[i] *= 1.1 42 | else: 43 | # Was not better, try other way around 44 | self.params[i] -= 2*self.deltas[i] 45 | error, sim_time = error_function(self.params) 46 | if error < error_min: 47 | # Was best when going the other way 48 | error_min = float(error) 49 | error_min_time = int(sim_time) 50 | best_params = self.params[:] # Deep-ish copy 51 | self.deltas[i] *= 1.1 52 | else: 53 | # Didn't get better results using previous delta. 54 | # Thus, make it smaller and try again 55 | self.params[i] += self.deltas[i] 56 | self.deltas[i] *= 0.9 57 | except KeyboardInterrupt: 58 | print "ctrl-C!" # Do nothing and then pass the params 59 | return (best_params, error_min, error_min_time) 60 | -------------------------------------------------------------------------------- /util.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | from vrep import * 4 | from math import sqrt, cos, acos 5 | 6 | def log(client, message_, send=False): 7 | message = '-- %s' % message_ 8 | print message 9 | if send: 10 | msg(client, message) 11 | 12 | 13 | def msg(client, message): 14 | err = simxAddStatusbarMessage(client, message, simx_opmode_oneshot_wait) 15 | if err: 16 | print 'ERROR AddStatusBarMessage code %d' % err 17 | 18 | 19 | def square(val): 20 | return val**2.0 21 | 22 | def distN(n, vec, ref): 23 | return sqrt(sum(map(square, map(lambda zipd: zipd[0]-zipd[1], zip(vec[0:n], ref[0:n]))))) 24 | 25 | def dist2(vec, ref=[0.0, 0.0]): 26 | return distN(2, vec, ref) 27 | 28 | 29 | def tilt_from_rp(roll, pitch): 30 | return acos(cos(roll)*cos(pitch)) 31 | -------------------------------------------------------------------------------- /vrep.py: -------------------------------------------------------------------------------- 1 | # This file is part of the REMOTE API 2 | # 3 | # Copyright 2006-2014 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.1.3 on Sept. 30th 2014 28 | 29 | import platform 30 | import struct 31 | from ctypes import * 32 | from vrepConst import * 33 | 34 | #load library 35 | libsimx = None 36 | if platform.system() =='Windows': 37 | libsimx = CDLL("./remoteApi.dll") 38 | elif platform.system() == 'Darwin': 39 | libsimx = CDLL("./remoteApi.dylib") 40 | else: 41 | libsimx = CDLL("./remoteApi.so") 42 | 43 | #ctypes wrapper prototypes 44 | c_GetJointPosition = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetJointPosition", libsimx)) 45 | c_SetJointPosition = CFUNCTYPE(c_int32,c_int32, c_int32, c_float, c_int32)(("simxSetJointPosition", libsimx)) 46 | c_GetJointMatrix = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetJointMatrix", libsimx)) 47 | c_SetSphericalJointMatrix = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxSetSphericalJointMatrix", libsimx)) 48 | c_SetJointTargetVelocity = CFUNCTYPE(c_int32,c_int32, c_int32, c_float, c_int32)(("simxSetJointTargetVelocity", libsimx)) 49 | c_SetJointTargetPosition = CFUNCTYPE(c_int32,c_int32, c_int32, c_float, c_int32)(("simxSetJointTargetPosition", libsimx)) 50 | c_GetJointForce = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetJointForce", libsimx)) 51 | c_SetJointForce = CFUNCTYPE(c_int32,c_int32, c_int32, c_float, c_int32)(("simxSetJointForce", libsimx)) 52 | c_ReadForceSensor = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_ubyte), POINTER(c_float), POINTER(c_float), c_int32)(("simxReadForceSensor", libsimx)) 53 | c_BreakForceSensor = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxBreakForceSensor", libsimx)) 54 | c_ReadVisionSensor = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_ubyte), POINTER(POINTER(c_float)), POINTER(POINTER(c_int32)), c_int32)(("simxReadVisionSensor", libsimx)) 55 | c_GetObjectHandle = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_int32), c_int32)(("simxGetObjectHandle", libsimx)) 56 | c_GetVisionSensorImage = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), POINTER(POINTER(c_byte)), c_ubyte, c_int32)(("simxGetVisionSensorImage", libsimx)) 57 | c_SetVisionSensorImage = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_byte), c_int32, c_ubyte, c_int32)(("simxSetVisionSensorImage", libsimx)) 58 | c_GetVisionSensorDepthBuffer= CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), POINTER(POINTER(c_float)), c_int32)(("simxGetVisionSensorDepthBuffer", libsimx)) 59 | c_GetObjectChild = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetObjectChild", libsimx)) 60 | c_GetObjectParent = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetObjectParent", libsimx)) 61 | c_ReadProximitySensor = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_ubyte), POINTER(c_float), POINTER(c_int32), POINTER(c_float), c_int32)(("simxReadProximitySensor", libsimx)) 62 | c_LoadModel = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_ubyte, POINTER(c_int32), c_int32)(("simxLoadModel", libsimx)) 63 | c_LoadUI = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_ubyte, POINTER(c_int32), POINTER(POINTER(c_int32)), c_int32)(("simxLoadUI", libsimx)) 64 | c_LoadScene = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_ubyte, c_int32)(("simxLoadScene", libsimx)) 65 | c_StartSimulation = CFUNCTYPE(c_int32,c_int32, c_int32)(("simxStartSimulation", libsimx)) 66 | c_PauseSimulation = CFUNCTYPE(c_int32,c_int32, c_int32)(("simxPauseSimulation", libsimx)) 67 | c_StopSimulation = CFUNCTYPE(c_int32,c_int32, c_int32)(("simxStopSimulation", libsimx)) 68 | c_GetUIHandle = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_int32), c_int32)(("simxGetUIHandle", libsimx)) 69 | c_GetUISlider = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetUISlider", libsimx)) 70 | c_SetUISlider = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_int32, c_int32)(("simxSetUISlider", libsimx)) 71 | c_GetUIEventButton = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), POINTER(c_int32), c_int32)(("simxGetUIEventButton", libsimx)) 72 | c_GetUIButtonProperty = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetUIButtonProperty", libsimx)) 73 | c_SetUIButtonProperty = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_int32, c_int32)(("simxSetUIButtonProperty", libsimx)) 74 | c_AddStatusbarMessage = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32)(("simxAddStatusbarMessage", libsimx)) 75 | c_AuxiliaryConsoleOpen = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32, c_int32, POINTER(c_int32), POINTER(c_int32), POINTER(c_float), POINTER(c_float), POINTER(c_int32), c_int32)(("simxAuxiliaryConsoleOpen", libsimx)) 76 | c_AuxiliaryConsoleClose = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxAuxiliaryConsoleClose", libsimx)) 77 | c_AuxiliaryConsolePrint = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_char), c_int32)(("simxAuxiliaryConsolePrint", libsimx)) 78 | c_AuxiliaryConsoleShow = CFUNCTYPE(c_int32,c_int32, c_int32, c_ubyte, c_int32)(("simxAuxiliaryConsoleShow", libsimx)) 79 | c_GetObjectOrientation = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetObjectOrientation", libsimx)) 80 | c_GetObjectPosition = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetObjectPosition", libsimx)) 81 | c_SetObjectOrientation = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_float), c_int32)(("simxSetObjectOrientation", libsimx)) 82 | c_SetObjectPosition = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_float), c_int32)(("simxSetObjectPosition", libsimx)) 83 | c_SetObjectParent = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_ubyte, c_int32)(("simxSetObjectParent", libsimx)) 84 | c_SetUIButtonLabel = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_char), POINTER(c_char), c_int32)(("simxSetUIButtonLabel", libsimx)) 85 | c_GetLastErrors = CFUNCTYPE(c_int32,c_int32, POINTER(c_int32), POINTER(POINTER(c_char)), c_int32)(("simxGetLastErrors", libsimx)) 86 | c_GetArrayParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetArrayParameter", libsimx)) 87 | c_SetArrayParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxSetArrayParameter", libsimx)) 88 | c_GetBooleanParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_ubyte), c_int32)(("simxGetBooleanParameter", libsimx)) 89 | c_SetBooleanParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_ubyte, c_int32)(("simxSetBooleanParameter", libsimx)) 90 | c_GetIntegerParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetIntegerParameter", libsimx)) 91 | c_SetIntegerParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_int32)(("simxSetIntegerParameter", libsimx)) 92 | c_GetFloatingParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetFloatingParameter", libsimx)) 93 | c_SetFloatingParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_float, c_int32)(("simxSetFloatingParameter", libsimx)) 94 | c_GetStringParameter = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(POINTER(c_char)), c_int32)(("simxGetStringParameter", libsimx)) 95 | c_GetCollisionHandle = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_int32), c_int32)(("simxGetCollisionHandle", libsimx)) 96 | c_GetDistanceHandle = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_int32), c_int32)(("simxGetDistanceHandle", libsimx)) 97 | c_ReadCollision = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_ubyte), c_int32)(("simxReadCollision", libsimx)) 98 | c_ReadDistance = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), c_int32)(("simxReadDistance", libsimx)) 99 | c_RemoveObject = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxRemoveObject", libsimx)) 100 | c_RemoveModel = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxRemoveModel", libsimx)) 101 | c_RemoveUI = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxRemoveUI", libsimx)) 102 | c_CloseScene = CFUNCTYPE(c_int32,c_int32, c_int32)(("simxCloseScene", libsimx)) 103 | c_GetObjects = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), POINTER(POINTER(c_int32)), c_int32)(("simxGetObjects", libsimx)) 104 | c_DisplayDialog = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_char), c_int32, POINTER(c_char), POINTER(c_float), POINTER(c_float), POINTER(c_int32), POINTER(c_int32), c_int32)(("simxDisplayDialog", libsimx)) 105 | c_EndDialog = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32)(("simxEndDialog", libsimx)) 106 | c_GetDialogInput = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(POINTER(c_char)), c_int32)(("simxGetDialogInput", libsimx)) 107 | c_GetDialogResult = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetDialogResult", libsimx)) 108 | c_CopyPasteObjects = CFUNCTYPE(c_int32,c_int32, POINTER(c_int32), c_int32, POINTER(POINTER(c_int32)), POINTER(c_int32), c_int32)(("simxCopyPasteObjects", libsimx)) 109 | c_GetObjectSelection = CFUNCTYPE(c_int32,c_int32, POINTER(POINTER(c_int32)), POINTER(c_int32), c_int32)(("simxGetObjectSelection", libsimx)) 110 | c_SetObjectSelection = CFUNCTYPE(c_int32,c_int32, POINTER(c_int32), c_int32, c_int32)(("simxSetObjectSelection", libsimx)) 111 | c_ClearFloatSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32)(("simxClearFloatSignal", libsimx)) 112 | c_ClearIntegerSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32)(("simxClearIntegerSignal", libsimx)) 113 | c_ClearStringSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32)(("simxClearStringSignal", libsimx)) 114 | c_GetFloatSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_float), c_int32)(("simxGetFloatSignal", libsimx)) 115 | c_GetIntegerSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_int32), c_int32)(("simxGetIntegerSignal", libsimx)) 116 | c_GetStringSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(POINTER(c_ubyte)), POINTER(c_int32), c_int32)(("simxGetStringSignal", libsimx)) 117 | c_SetFloatSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_float, c_int32)(("simxSetFloatSignal", libsimx)) 118 | c_SetIntegerSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32, c_int32)(("simxSetIntegerSignal", libsimx)) 119 | c_SetStringSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_ubyte), c_int32, c_int32)(("simxSetStringSignal", libsimx)) 120 | c_AppendStringSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_ubyte), c_int32, c_int32)(("simxAppendStringSignal", libsimx)) 121 | c_WriteStringStream = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_ubyte), c_int32, c_int32)(("simxWriteStringStream", libsimx)) 122 | c_GetObjectFloatParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_float), c_int32)(("simxGetObjectFloatParameter", libsimx)) 123 | c_SetObjectFloatParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_float, c_int32)(("simxSetObjectFloatParameter", libsimx)) 124 | c_GetObjectIntParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetObjectIntParameter", libsimx)) 125 | c_SetObjectIntParameter = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_int32, c_int32)(("simxSetObjectIntParameter", libsimx)) 126 | c_GetModelProperty = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32), c_int32)(("simxGetModelProperty", libsimx)) 127 | c_SetModelProperty = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, c_int32)(("simxSetModelProperty", libsimx)) 128 | c_Start = CFUNCTYPE(c_int32,POINTER(c_char), c_int32, c_ubyte, c_ubyte, c_int32, c_int32)(("simxStart", libsimx)) 129 | c_Finish = CFUNCTYPE(None, c_int32)(("simxFinish", libsimx)) 130 | c_GetPingTime = CFUNCTYPE(c_int32,c_int32, POINTER(c_int32))(("simxGetPingTime", libsimx)) 131 | c_GetLastCmdTime = CFUNCTYPE(c_int32,c_int32)(("simxGetLastCmdTime", libsimx)) 132 | c_SynchronousTrigger = CFUNCTYPE(c_int32,c_int32)(("simxSynchronousTrigger", libsimx)) 133 | c_Synchronous = CFUNCTYPE(c_int32,c_int32, c_ubyte)(("simxSynchronous", libsimx)) 134 | c_PauseCommunication = CFUNCTYPE(c_int32,c_int32, c_ubyte)(("simxPauseCommunication", libsimx)) 135 | c_GetInMessageInfo = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32))(("simxGetInMessageInfo", libsimx)) 136 | c_GetOutMessageInfo = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_int32))(("simxGetOutMessageInfo", libsimx)) 137 | c_GetConnectionId = CFUNCTYPE(c_int32,c_int32)(("simxGetConnectionId", libsimx)) 138 | c_CreateBuffer = CFUNCTYPE(POINTER(c_ubyte), c_int32)(("simxCreateBuffer", libsimx)) 139 | c_ReleaseBuffer = CFUNCTYPE(None, c_void_p)(("simxReleaseBuffer", libsimx)) 140 | c_TransferFile = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_char), c_int32, c_int32)(("simxTransferFile", libsimx)) 141 | c_EraseFile = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), c_int32)(("simxEraseFile", libsimx)) 142 | c_GetAndClearStringSignal = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(POINTER(c_ubyte)), POINTER(c_int32), c_int32)(("simxGetAndClearStringSignal", libsimx)) 143 | c_ReadStringStream = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(POINTER(c_ubyte)), POINTER(c_int32), c_int32)(("simxReadStringStream", libsimx)) 144 | c_CreateDummy = CFUNCTYPE(c_int32,c_int32, c_float, POINTER(c_ubyte), POINTER(c_int32), c_int32)(("simxCreateDummy", libsimx)) 145 | c_Query = CFUNCTYPE(c_int32,c_int32, POINTER(c_char), POINTER(c_ubyte), c_int32, POINTER(c_char), POINTER(POINTER(c_ubyte)), POINTER(c_int32), c_int32)(("simxQuery", libsimx)) 146 | c_GetObjectGroupData = CFUNCTYPE(c_int32,c_int32, c_int32, c_int32, POINTER(c_int32), POINTER(POINTER(c_int32)), POINTER(c_int32), POINTER(POINTER(c_int32)), POINTER(c_int32), POINTER(POINTER(c_float)), POINTER(c_int32), POINTER(POINTER(c_char)), c_int32)(("simxGetObjectGroupData", libsimx)) 147 | c_GetObjectVelocity = CFUNCTYPE(c_int32,c_int32, c_int32, POINTER(c_float), POINTER(c_float), c_int32)(("simxGetObjectVelocity", libsimx)) 148 | 149 | 150 | #API functions 151 | def simxGetJointPosition(clientID, jointHandle, operationMode): 152 | ''' 153 | Please have a look at the function description/documentation in the V-REP user manual 154 | ''' 155 | position = c_float() 156 | return c_GetJointPosition(clientID, jointHandle, byref(position), operationMode), position.value 157 | 158 | def simxSetJointPosition(clientID, jointHandle, position, operationMode): 159 | ''' 160 | Please have a look at the function description/documentation in the V-REP user manual 161 | ''' 162 | 163 | return c_SetJointPosition(clientID, jointHandle, position, operationMode) 164 | 165 | def simxGetJointMatrix(clientID, jointHandle, operationMode): 166 | ''' 167 | Please have a look at the function description/documentation in the V-REP user manual 168 | ''' 169 | matrix = (c_float*12)() 170 | ret = c_GetJointMatrix(clientID, jointHandle, matrix, operationMode) 171 | arr = [] 172 | for i in range(12): 173 | arr.append(matrix[i]) 174 | return ret, arr 175 | 176 | def simxSetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode): 177 | ''' 178 | Please have a look at the function description/documentation in the V-REP user manual 179 | ''' 180 | matrix = (c_float*12)(*matrix) 181 | return c_SetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode) 182 | 183 | def simxSetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode): 184 | ''' 185 | Please have a look at the function description/documentation in the V-REP user manual 186 | ''' 187 | 188 | return c_SetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode) 189 | 190 | def simxSetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode): 191 | ''' 192 | Please have a look at the function description/documentation in the V-REP user manual 193 | ''' 194 | 195 | return c_SetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode) 196 | 197 | def simxJointGetForce(clientID, jointHandle, operationMode): 198 | ''' 199 | Please have a look at the function description/documentation in the V-REP user manual 200 | ''' 201 | force = c_float() 202 | return c_GetJointForce(clientID, jointHandle, byref(force), operationMode), force.value 203 | 204 | def simxGetJointForce(clientID, jointHandle, operationMode): 205 | ''' 206 | Please have a look at the function description/documentation in the V-REP user manual 207 | ''' 208 | force = c_float() 209 | return c_GetJointForce(clientID, jointHandle, byref(force), operationMode), force.value 210 | 211 | def simxSetJointForce(clientID, jointHandle, force, operationMode): 212 | ''' 213 | Please have a look at the function description/documentation in the V-REP user manual 214 | ''' 215 | return c_SetJointForce(clientID, jointHandle, force, operationMode) 216 | 217 | def simxReadForceSensor(clientID, forceSensorHandle, operationMode): 218 | ''' 219 | Please have a look at the function description/documentation in the V-REP user manual 220 | ''' 221 | state = c_ubyte() 222 | forceVector = (c_float*3)() 223 | torqueVector = (c_float*3)() 224 | ret = c_ReadForceSensor(clientID, forceSensorHandle, byref(state), forceVector, torqueVector, operationMode) 225 | arr1 = [] 226 | for i in range(3): 227 | arr1.append(forceVector[i]) 228 | arr2 = [] 229 | for i in range(3): 230 | arr2.append(torqueVector[i]) 231 | return ret, ord(state.value), arr1, arr2 232 | 233 | def simxBreakForceSensor(clientID, forceSensorHandle, operationMode): 234 | ''' 235 | Please have a look at the function description/documentation in the V-REP user manual 236 | ''' 237 | return c_BreakForceSensor(clientID, forceSensorHandle, operationMode) 238 | 239 | def simxReadVisionSensor(clientID, sensorHandle, operationMode): 240 | ''' 241 | Please have a look at the function description/documentation in the V-REP user manual 242 | ''' 243 | 244 | detectionState = c_ubyte() 245 | auxValues = pointer(c_float()) 246 | auxValuesCount = pointer(c_int()) 247 | ret = c_ReadVisionSensor(clientID, sensorHandle, byref(detectionState), byref(auxValues), byref(auxValuesCount), operationMode) 248 | 249 | auxValues2 = [] 250 | if ret == 0: 251 | s = 0 252 | for i in range(auxValuesCount[0]): 253 | auxValues2.append(auxValues[s:s+auxValuesCount[i+1]]) 254 | s += auxValuesCount[i+1] 255 | 256 | #free C buffers 257 | c_ReleaseBuffer(auxValues) 258 | c_ReleaseBuffer(auxValuesCount) 259 | 260 | return ret, bool(detectionState.value!=0), auxValues2 261 | 262 | def simxGetObjectHandle(clientID, objectName, operationMode): 263 | ''' 264 | Please have a look at the function description/documentation in the V-REP user manual 265 | ''' 266 | handle = c_int() 267 | return c_GetObjectHandle(clientID, objectName, byref(handle), operationMode), handle.value 268 | 269 | def simxGetVisionSensorImage(clientID, sensorHandle, options, operationMode): 270 | ''' 271 | Please have a look at the function description/documentation in the V-REP user manual 272 | ''' 273 | 274 | resolution = (c_int*2)() 275 | c_image = pointer(c_byte()) 276 | bytesPerPixel = 3 277 | if (options and 1) != 0: 278 | bytesPerPixel = 1 279 | ret = c_GetVisionSensorImage(clientID, sensorHandle, resolution, byref(c_image), options, operationMode) 280 | 281 | reso = [] 282 | image = [] 283 | if (ret == 0): 284 | image = [None]*resolution[0]*resolution[1]*bytesPerPixel 285 | for i in range(resolution[0] * resolution[1] * bytesPerPixel): 286 | image[i] = c_image[i] 287 | for i in range(2): 288 | reso.append(resolution[i]) 289 | return ret, reso, image 290 | 291 | def simxSetVisionSensorImage(clientID, sensorHandle, image, options, operationMode): 292 | ''' 293 | Please have a look at the function description/documentation in the V-REP user manual 294 | ''' 295 | size = len(image) 296 | image_bytes = (c_byte*size)(*image) 297 | return c_SetVisionSensorImage(clientID, sensorHandle, image_bytes, size, options, operationMode) 298 | 299 | def simxGetVisionSensorDepthBuffer(clientID, sensorHandle, operationMode): 300 | ''' 301 | Please have a look at the function description/documentation in the V-REP user manual 302 | ''' 303 | c_buffer = pointer(c_float()) 304 | resolution = (c_int*2)() 305 | ret = c_GetVisionSensorDepthBuffer(clientID, sensorHandle, resolution, byref(c_buffer), operationMode) 306 | reso = [] 307 | buffer = [] 308 | if (ret == 0): 309 | buffer = [None]*resolution[0]*resolution[1] 310 | for i in range(resolution[0] * resolution[1]): 311 | buffer[i] = c_buffer[i] 312 | for i in range(2): 313 | reso.append(resolution[i]) 314 | return ret, reso, buffer 315 | 316 | def simxGetObjectChild(clientID, parentObjectHandle, childIndex, operationMode): 317 | ''' 318 | Please have a look at the function description/documentation in the V-REP user manual 319 | ''' 320 | childObjectHandle = c_int() 321 | return c_GetObjectChild(clientID, parentObjectHandle, childIndex, byref(childObjectHandle), operationMode), childObjectHandle.value 322 | 323 | def simxGetObjectParent(clientID, childObjectHandle, operationMode): 324 | ''' 325 | Please have a look at the function description/documentation in the V-REP user manual 326 | ''' 327 | 328 | parentObjectHandle = c_int() 329 | return c_GetObjectParent(clientID, childObjectHandle, byref(parentObjectHandle), operationMode), parentObjectHandle.value 330 | 331 | def simxReadProximitySensor(clientID, sensorHandle, operationMode): 332 | ''' 333 | Please have a look at the function description/documentation in the V-REP user manual 334 | ''' 335 | 336 | detectionState = c_ubyte() 337 | detectedObjectHandle = c_int() 338 | detectedPoint = (c_float*3)() 339 | detectedSurfaceNormalVector = (c_float*3)() 340 | ret = c_ReadProximitySensor(clientID, sensorHandle, byref(detectionState), detectedPoint, byref(detectedObjectHandle), detectedSurfaceNormalVector, operationMode) 341 | arr1 = [] 342 | for i in range(3): 343 | arr1.append(detectedPoint[i]) 344 | arr2 = [] 345 | for i in range(3): 346 | arr2.append(detectedSurfaceNormalVector[i]) 347 | return ret, bool(detectionState.value!=0), arr1, detectedObjectHandle.value, arr2 348 | 349 | def simxLoadModel(clientID, modelPathAndName, options, operationMode): 350 | ''' 351 | Please have a look at the function description/documentation in the V-REP user manual 352 | ''' 353 | baseHandle = c_int() 354 | return c_LoadModel(clientID, modelPathAndName, options, byref(baseHandle), operationMode), baseHandle.value 355 | 356 | def simxLoadUI(clientID, uiPathAndName, options, operationMode): 357 | ''' 358 | Please have a look at the function description/documentation in the V-REP user manual 359 | ''' 360 | 361 | count = c_int() 362 | uiHandles = pointer(c_int()) 363 | ret = c_LoadUI(clientID, uiPathAndName, options, byref(count), byref(uiHandles), operationMode) 364 | 365 | handles = [] 366 | if ret == 0: 367 | for i in range(count.value): 368 | handles.append(uiHandles[i]) 369 | #free C buffers 370 | c_ReleaseBuffer(uiHandles) 371 | 372 | return ret, handles 373 | 374 | def simxLoadScene(clientID, scenePathAndName, options, operationMode): 375 | ''' 376 | Please have a look at the function description/documentation in the V-REP user manual 377 | ''' 378 | 379 | return c_LoadScene(clientID, scenePathAndName, options, operationMode) 380 | 381 | def simxStartSimulation(clientID, operationMode): 382 | ''' 383 | Please have a look at the function description/documentation in the V-REP user manual 384 | ''' 385 | 386 | return c_StartSimulation(clientID, operationMode) 387 | 388 | def simxPauseSimulation(clientID, operationMode): 389 | ''' 390 | Please have a look at the function description/documentation in the V-REP user manual 391 | ''' 392 | 393 | return c_PauseSimulation(clientID, operationMode) 394 | 395 | def simxStopSimulation(clientID, operationMode): 396 | ''' 397 | Please have a look at the function description/documentation in the V-REP user manual 398 | ''' 399 | 400 | return c_StopSimulation(clientID, operationMode) 401 | 402 | def simxGetUIHandle(clientID, uiName, operationMode): 403 | ''' 404 | Please have a look at the function description/documentation in the V-REP user manual 405 | ''' 406 | 407 | handle = c_int() 408 | return c_GetUIHandle(clientID, uiName, byref(handle), operationMode), handle.value 409 | 410 | def simxGetUISlider(clientID, uiHandle, uiButtonID, operationMode): 411 | ''' 412 | Please have a look at the function description/documentation in the V-REP user manual 413 | ''' 414 | 415 | position = c_int() 416 | return c_GetUISlider(clientID, uiHandle, uiButtonID, byref(position), operationMode), position.value 417 | 418 | def simxSetUISlider(clientID, uiHandle, uiButtonID, position, operationMode): 419 | ''' 420 | Please have a look at the function description/documentation in the V-REP user manual 421 | ''' 422 | 423 | return c_SetUISlider(clientID, uiHandle, uiButtonID, position, operationMode) 424 | 425 | def simxGetUIEventButton(clientID, uiHandle, operationMode): 426 | ''' 427 | Please have a look at the function description/documentation in the V-REP user manual 428 | ''' 429 | 430 | uiEventButtonID = c_int() 431 | auxValues = (c_int*2)() 432 | ret = c_GetUIEventButton(clientID, uiHandle, byref(uiEventButtonID), auxValues, operationMode) 433 | arr = [] 434 | for i in range(2): 435 | arr.append(auxValues[i]) 436 | return ret, uiEventButtonID.value, arr 437 | 438 | def simxGetUIButtonProperty(clientID, uiHandle, uiButtonID, operationMode): 439 | ''' 440 | Please have a look at the function description/documentation in the V-REP user manual 441 | ''' 442 | 443 | prop = c_int() 444 | return c_GetUIButtonProperty(clientID, uiHandle, uiButtonID, byref(prop), operationMode), prop.value 445 | 446 | def simxSetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode): 447 | ''' 448 | Please have a look at the function description/documentation in the V-REP user manual 449 | ''' 450 | 451 | return c_SetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode) 452 | 453 | def simxAddStatusbarMessage(clientID, message, operationMode): 454 | ''' 455 | Please have a look at the function description/documentation in the V-REP user manual 456 | ''' 457 | 458 | return c_AddStatusbarMessage(clientID, message, operationMode) 459 | 460 | def simxAuxiliaryConsoleOpen(clientID, title, maxLines, mode, position, size, textColor, backgroundColor, operationMode): 461 | ''' 462 | Please have a look at the function description/documentation in the V-REP user manual 463 | ''' 464 | 465 | consoleHandle = c_int() 466 | if position != None: 467 | c_position = (c_int*2)(*position) 468 | else: 469 | c_position = None 470 | if size != None: 471 | c_size = (c_int*2)(*size) 472 | else: 473 | c_size = None 474 | if textColor != None: 475 | c_textColor = (c_float*3)(*textColor) 476 | else: 477 | c_textColor = None 478 | if backgroundColor != None: 479 | c_backgroundColor = (c_float*3)(*backgroundColor) 480 | else: 481 | c_backgroundColor = None 482 | return c_AuxiliaryConsoleOpen(clientID, title, maxLines, mode, c_position, c_size, c_textColor, c_backgroundColor, byref(consoleHandle), operationMode), consoleHandle.value 483 | 484 | def simxAuxiliaryConsoleClose(clientID, consoleHandle, operationMode): 485 | ''' 486 | Please have a look at the function description/documentation in the V-REP user manual 487 | ''' 488 | 489 | return c_AuxiliaryConsoleClose(clientID, consoleHandle, operationMode) 490 | 491 | def simxAuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode): 492 | ''' 493 | Please have a look at the function description/documentation in the V-REP user manual 494 | ''' 495 | 496 | return c_AuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode) 497 | 498 | def simxAuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode): 499 | ''' 500 | Please have a look at the function description/documentation in the V-REP user manual 501 | ''' 502 | 503 | return c_AuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode) 504 | 505 | def simxGetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, operationMode): 506 | ''' 507 | Please have a look at the function description/documentation in the V-REP user manual 508 | ''' 509 | eulerAngles = (c_float*3)() 510 | ret = c_GetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode) 511 | arr = [] 512 | for i in range(3): 513 | arr.append(eulerAngles[i]) 514 | return ret, arr 515 | 516 | def simxGetObjectPosition(clientID, objectHandle, relativeToObjectHandle, operationMode): 517 | ''' 518 | Please have a look at the function description/documentation in the V-REP user manual 519 | ''' 520 | position = (c_float*3)() 521 | ret = c_GetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode) 522 | arr = [] 523 | for i in range(3): 524 | arr.append(position[i]) 525 | return ret, arr 526 | 527 | def simxSetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode): 528 | ''' 529 | Please have a look at the function description/documentation in the V-REP user manual 530 | ''' 531 | 532 | angles = (c_float*3)(*eulerAngles) 533 | return c_SetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, angles, operationMode) 534 | 535 | def simxSetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode): 536 | ''' 537 | Please have a look at the function description/documentation in the V-REP user manual 538 | ''' 539 | 540 | c_position = (c_float*3)(*position) 541 | return c_SetObjectPosition(clientID, objectHandle, relativeToObjectHandle, c_position, operationMode) 542 | 543 | def simxSetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode): 544 | ''' 545 | Please have a look at the function description/documentation in the V-REP user manual 546 | ''' 547 | 548 | return c_SetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode) 549 | 550 | def simxSetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode): 551 | ''' 552 | Please have a look at the function description/documentation in the V-REP user manual 553 | ''' 554 | 555 | return c_SetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode) 556 | 557 | def simxGetLastErrors(clientID, operationMode): 558 | ''' 559 | Please have a look at the function description/documentation in the V-REP user manual 560 | ''' 561 | errors =[] 562 | errorCnt = c_int() 563 | errorStrings = pointer(c_char()) 564 | ret = c_GetLastErrors(clientID, byref(errorCnt), byref(errorStrings), operationMode) 565 | 566 | if ret == 0: 567 | s = 0 568 | for i in range(errorCnt.value): 569 | a = bytearray() 570 | while errorStrings[s] != '\0': 571 | a.append(errorStrings[s]) 572 | s += 1 573 | 574 | s += 1 #skip null 575 | errors.append(str(a)) 576 | 577 | return ret, errors 578 | 579 | def simxGetArrayParameter(clientID, paramIdentifier, operationMode): 580 | ''' 581 | Please have a look at the function description/documentation in the V-REP user manual 582 | ''' 583 | paramValues = (c_float*3)() 584 | ret = c_GetArrayParameter(clientID, paramIdentifier, paramValues, operationMode) 585 | arr = [] 586 | for i in range(3): 587 | arr.append(paramValues[i]) 588 | return ret, arr 589 | 590 | def simxSetArrayParameter(clientID, paramIdentifier, paramValues, operationMode): 591 | ''' 592 | Please have a look at the function description/documentation in the V-REP user manual 593 | ''' 594 | 595 | c_paramValues = (c_float*3)(*paramValues) 596 | return c_SetArrayParameter(clientID, paramIdentifier, c_paramValues, operationMode) 597 | 598 | def simxGetBooleanParameter(clientID, paramIdentifier, operationMode): 599 | ''' 600 | Please have a look at the function description/documentation in the V-REP user manual 601 | ''' 602 | 603 | paramValue = c_ubyte() 604 | return c_GetBooleanParameter(clientID, paramIdentifier, byref(paramValue), operationMode), bool(paramValue.value!=0) 605 | 606 | def simxSetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode): 607 | ''' 608 | Please have a look at the function description/documentation in the V-REP user manual 609 | ''' 610 | 611 | return c_SetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode) 612 | 613 | def simxGetIntegerParameter(clientID, paramIdentifier, operationMode): 614 | ''' 615 | Please have a look at the function description/documentation in the V-REP user manual 616 | ''' 617 | 618 | paramValue = c_int() 619 | return c_GetIntegerParameter(clientID, paramIdentifier, byref(paramValue), operationMode), paramValue.value 620 | 621 | def simxSetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode): 622 | ''' 623 | Please have a look at the function description/documentation in the V-REP user manual 624 | ''' 625 | 626 | return c_SetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode) 627 | 628 | def simxGetFloatingParameter(clientID, paramIdentifier, operationMode): 629 | ''' 630 | Please have a look at the function description/documentation in the V-REP user manual 631 | ''' 632 | 633 | paramValue = c_float() 634 | return c_GetFloatingParameter(clientID, paramIdentifier, byref(paramValue), operationMode), paramValue.value 635 | 636 | def simxSetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode): 637 | ''' 638 | Please have a look at the function description/documentation in the V-REP user manual 639 | ''' 640 | 641 | return c_SetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode) 642 | 643 | def simxGetStringParameter(clientID, paramIdentifier, operationMode): 644 | ''' 645 | Please have a look at the function description/documentation in the V-REP user manual 646 | ''' 647 | paramValue = pointer(c_char()) 648 | ret = c_GetStringParameter(clientID, paramIdentifier, byref(paramValue), operationMode) 649 | 650 | a = bytearray() 651 | if ret == 0: 652 | i = 0 653 | while paramValue[i] != '\0': 654 | a.append(paramValue[i]) 655 | i=i+1 656 | 657 | return ret, str(a) 658 | 659 | def simxGetCollisionHandle(clientID, collisionObjectName, operationMode): 660 | ''' 661 | Please have a look at the function description/documentation in the V-REP user manual 662 | ''' 663 | 664 | handle = c_int() 665 | return c_GetCollisionHandle(clientID, collisionObjectName, byref(handle), operationMode), handle.value 666 | 667 | def simxGetDistanceHandle(clientID, distanceObjectName, operationMode): 668 | ''' 669 | Please have a look at the function description/documentation in the V-REP user manual 670 | ''' 671 | 672 | handle = c_int() 673 | return c_GetDistanceHandle(clientID, distanceObjectName, byref(handle), operationMode), handle.value 674 | 675 | def simxReadCollision(clientID, collisionObjectHandle, operationMode): 676 | ''' 677 | Please have a look at the function description/documentation in the V-REP user manual 678 | ''' 679 | collisionState = c_ubyte() 680 | return c_ReadCollision(clientID, collisionObjectHandle, byref(collisionState), operationMode), bool(collisionState.value!=0) 681 | 682 | def simxReadDistance(clientID, distanceObjectHandle, operationMode): 683 | ''' 684 | Please have a look at the function description/documentation in the V-REP user manual 685 | ''' 686 | 687 | minimumDistance = c_float() 688 | return c_ReadDistance(clientID, distanceObjectHandle, byref(minimumDistance), operationMode), minimumDistance.value 689 | 690 | def simxRemoveObject(clientID, objectHandle, operationMode): 691 | ''' 692 | Please have a look at the function description/documentation in the V-REP user manual 693 | ''' 694 | 695 | return c_RemoveObject(clientID, objectHandle, operationMode) 696 | 697 | def simxRemoveModel(clientID, objectHandle, operationMode): 698 | ''' 699 | Please have a look at the function description/documentation in the V-REP user manual 700 | ''' 701 | 702 | return c_RemoveModel(clientID, objectHandle, operationMode) 703 | 704 | def simxRemoveUI(clientID, uiHandle, operationMode): 705 | ''' 706 | Please have a look at the function description/documentation in the V-REP user manual 707 | ''' 708 | 709 | return c_RemoveUI(clientID, uiHandle, operationMode) 710 | 711 | def simxCloseScene(clientID, operationMode): 712 | ''' 713 | Please have a look at the function description/documentation in the V-REP user manual 714 | ''' 715 | 716 | return c_CloseScene(clientID, operationMode) 717 | 718 | def simxGetObjects(clientID, objectType, operationMode): 719 | ''' 720 | Please have a look at the function description/documentation in the V-REP user manual 721 | ''' 722 | 723 | objectCount = c_int() 724 | objectHandles = pointer(c_int()) 725 | 726 | ret = c_GetObjects(clientID, objectType, byref(objectCount), byref(objectHandles), operationMode) 727 | handles = [] 728 | if ret == 0: 729 | for i in range(objectCount.value): 730 | handles.append(objectHandles[i]) 731 | 732 | return ret, handles 733 | 734 | 735 | def simxDisplayDialog(clientID, titleText, mainText, dialogType, initialText, titleColors, dialogColors, operationMode): 736 | ''' 737 | Please have a look at the function description/documentation in the V-REP user manual 738 | ''' 739 | if titleColors != None: 740 | c_titleColors = (c_float*6)(*titleColors) 741 | else: 742 | c_titleColors = None 743 | if dialogColors != None: 744 | c_dialogColors = (c_float*6)(*dialogColors) 745 | else: 746 | c_dialogColors = None 747 | 748 | c_dialogHandle = c_int() 749 | c_uiHandle = c_int() 750 | return c_DisplayDialog(clientID, titleText, mainText, dialogType, initialText, c_titleColors, c_dialogColors, byref(c_dialogHandle), byref(c_uiHandle), operationMode), c_dialogHandle.value, c_uiHandle.value 751 | 752 | def simxEndDialog(clientID, dialogHandle, operationMode): 753 | ''' 754 | Please have a look at the function description/documentation in the V-REP user manual 755 | ''' 756 | 757 | return c_EndDialog(clientID, dialogHandle, operationMode) 758 | 759 | def simxGetDialogInput(clientID, dialogHandle, operationMode): 760 | ''' 761 | Please have a look at the function description/documentation in the V-REP user manual 762 | ''' 763 | inputText = pointer(c_char()) 764 | ret = c_GetDialogInput(clientID, dialogHandle, byref(inputText), operationMode) 765 | 766 | a = bytearray() 767 | if ret == 0: 768 | i = 0 769 | while inputText[i] != '\0': 770 | a.append(inputText[i]) 771 | i = i+1 772 | 773 | return ret, str(a) 774 | 775 | 776 | def simxGetDialogResult(clientID, dialogHandle, operationMode): 777 | ''' 778 | Please have a look at the function description/documentation in the V-REP user manual 779 | ''' 780 | result = c_int() 781 | return c_GetDialogResult(clientID, dialogHandle, byref(result), operationMode), result.value 782 | 783 | def simxCopyPasteObjects(clientID, objectHandles, operationMode): 784 | ''' 785 | Please have a look at the function description/documentation in the V-REP user manual 786 | ''' 787 | c_objectHandles = (c_int*len(objectHandles))(*objectHandles) 788 | newObjectCount = c_int() 789 | newObjectHandles = pointer(c_int()) 790 | ret = c_CopyPasteObjects(clientID, c_objectHandles, len(objectHandles), byref(newObjectHandles), byref(newObjectCount), operationMode) 791 | 792 | newobj = [] 793 | if ret == 0: 794 | for i in range(newObjectCount.value): 795 | newobj.append(newObjectHandles[i]) 796 | 797 | return ret, newobj 798 | 799 | 800 | def simxGetObjectSelection(clientID, operationMode): 801 | ''' 802 | Please have a look at the function description/documentation in the V-REP user manual 803 | ''' 804 | objectCount = c_int() 805 | objectHandles = pointer(c_int()) 806 | ret = c_GetObjectSelection(clientID, byref(objectHandles), byref(objectCount), operationMode) 807 | 808 | newobj = [] 809 | if ret == 0: 810 | for i in range(objectCount.value): 811 | newobj.append(objectHandles[i]) 812 | 813 | return ret, newobj 814 | 815 | 816 | 817 | def simxSetObjectSelection(clientID, objectHandles, operationMode): 818 | ''' 819 | Please have a look at the function description/documentation in the V-REP user manual 820 | ''' 821 | 822 | c_objectHandles = (c_int*len(objectHandles))(*objectHandles) 823 | return c_SetObjectSelection(clientID, c_objectHandles, len(objectHandles), operationMode) 824 | 825 | def simxClearFloatSignal(clientID, signalName, operationMode): 826 | ''' 827 | Please have a look at the function description/documentation in the V-REP user manual 828 | ''' 829 | 830 | return c_ClearFloatSignal(clientID, signalName, operationMode) 831 | 832 | def simxClearIntegerSignal(clientID, signalName, operationMode): 833 | ''' 834 | Please have a look at the function description/documentation in the V-REP user manual 835 | ''' 836 | 837 | return c_ClearIntegerSignal(clientID, signalName, operationMode) 838 | 839 | def simxClearStringSignal(clientID, signalName, operationMode): 840 | ''' 841 | Please have a look at the function description/documentation in the V-REP user manual 842 | ''' 843 | 844 | return c_ClearStringSignal(clientID, signalName, operationMode) 845 | 846 | def simxGetFloatSignal(clientID, signalName, operationMode): 847 | ''' 848 | Please have a look at the function description/documentation in the V-REP user manual 849 | ''' 850 | 851 | signalValue = c_float() 852 | return c_GetFloatSignal(clientID, signalName, byref(signalValue), operationMode), signalValue.value 853 | 854 | def simxGetIntegerSignal(clientID, signalName, operationMode): 855 | ''' 856 | Please have a look at the function description/documentation in the V-REP user manual 857 | ''' 858 | 859 | signalValue = c_int() 860 | return c_GetIntegerSignal(clientID, signalName, byref(signalValue), operationMode), signalValue.value 861 | 862 | def simxGetStringSignal(clientID, signalName, operationMode): 863 | ''' 864 | Please have a look at the function description/documentation in the V-REP user manual 865 | ''' 866 | 867 | signalLength = c_int(); 868 | signalValue = pointer(c_ubyte()) 869 | ret = c_GetStringSignal(clientID, signalName, byref(signalValue), byref(signalLength), operationMode) 870 | 871 | a = bytearray() 872 | if ret == 0: 873 | for i in range(signalLength.value): 874 | a.append(signalValue[i]) 875 | 876 | return ret, str(a) 877 | 878 | def simxGetAndClearStringSignal(clientID, signalName, operationMode): 879 | ''' 880 | Please have a look at the function description/documentation in the V-REP user manual 881 | ''' 882 | 883 | signalLength = c_int(); 884 | signalValue = pointer(c_ubyte()) 885 | ret = c_GetAndClearStringSignal(clientID, signalName, byref(signalValue), byref(signalLength), operationMode) 886 | 887 | a = bytearray() 888 | if ret == 0: 889 | for i in range(signalLength.value): 890 | a.append(signalValue[i]) 891 | 892 | return ret, str(a) 893 | 894 | def simxReadStringStream(clientID, signalName, operationMode): 895 | ''' 896 | Please have a look at the function description/documentation in the V-REP user manual 897 | ''' 898 | 899 | signalLength = c_int(); 900 | signalValue = pointer(c_ubyte()) 901 | ret = c_ReadStringStream(clientID, signalName, byref(signalValue), byref(signalLength), operationMode) 902 | 903 | a = bytearray() 904 | if ret == 0: 905 | for i in range(signalLength.value): 906 | a.append(signalValue[i]) 907 | 908 | return ret, str(a) 909 | 910 | def simxSetFloatSignal(clientID, signalName, signalValue, operationMode): 911 | ''' 912 | Please have a look at the function description/documentation in the V-REP user manual 913 | ''' 914 | 915 | return c_SetFloatSignal(clientID, signalName, signalValue, operationMode) 916 | 917 | def simxSetIntegerSignal(clientID, signalName, signalValue, operationMode): 918 | ''' 919 | Please have a look at the function description/documentation in the V-REP user manual 920 | ''' 921 | 922 | return c_SetIntegerSignal(clientID, signalName, signalValue, operationMode) 923 | 924 | def simxSetStringSignal(clientID, signalName, signalValue, operationMode): 925 | ''' 926 | Please have a look at the function description/documentation in the V-REP user manual 927 | ''' 928 | 929 | return c_SetStringSignal(clientID, signalName, signalValue, len(signalValue), operationMode) 930 | 931 | def simxAppendStringSignal(clientID, signalName, signalValue, operationMode): 932 | ''' 933 | Please have a look at the function description/documentation in the V-REP user manual 934 | ''' 935 | 936 | return c_AppendStringSignal(clientID, signalName, signalValue, len(signalValue), operationMode) 937 | 938 | def simxWriteStringStream(clientID, signalName, signalValue, operationMode): 939 | ''' 940 | Please have a look at the function description/documentation in the V-REP user manual 941 | ''' 942 | 943 | return c_WriteStringStream(clientID, signalName, signalValue, len(signalValue), operationMode) 944 | 945 | def simxGetObjectFloatParameter(clientID, objectHandle, parameterID, operationMode): 946 | ''' 947 | Please have a look at the function description/documentation in the V-REP user manual 948 | ''' 949 | 950 | parameterValue = c_float() 951 | return c_GetObjectFloatParameter(clientID, objectHandle, parameterID, byref(parameterValue), operationMode), parameterValue.value 952 | 953 | def simxSetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): 954 | ''' 955 | Please have a look at the function description/documentation in the V-REP user manual 956 | ''' 957 | 958 | return c_SetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) 959 | 960 | def simxGetObjectIntParameter(clientID, objectHandle, parameterID, operationMode): 961 | ''' 962 | Please have a look at the function description/documentation in the V-REP user manual 963 | ''' 964 | 965 | parameterValue = c_int() 966 | return c_GetObjectIntParameter(clientID, objectHandle, parameterID, byref(parameterValue), operationMode), parameterValue.value 967 | 968 | def simxSetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): 969 | ''' 970 | Please have a look at the function description/documentation in the V-REP user manual 971 | ''' 972 | 973 | return c_SetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) 974 | 975 | def simxGetModelProperty(clientID, objectHandle, operationMode): 976 | ''' 977 | Please have a look at the function description/documentation in the V-REP user manual 978 | ''' 979 | prop = c_int() 980 | return c_GetModelProperty(clientID, objectHandle, byref(prop), operationMode), prop.value 981 | 982 | def simxSetModelProperty(clientID, objectHandle, prop, operationMode): 983 | ''' 984 | Please have a look at the function description/documentation in the V-REP user manual 985 | ''' 986 | 987 | return c_SetModelProperty(clientID, objectHandle, prop, operationMode) 988 | 989 | def simxStart(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs): 990 | ''' 991 | Please have a look at the function description/documentation in the V-REP user manual 992 | ''' 993 | 994 | return c_Start(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs) 995 | 996 | def simxFinish(clientID): 997 | ''' 998 | Please have a look at the function description/documentation in the V-REP user manual 999 | ''' 1000 | 1001 | return c_Finish(clientID) 1002 | 1003 | def simxGetPingTime(clientID): 1004 | ''' 1005 | Please have a look at the function description/documentation in the V-REP user manual 1006 | ''' 1007 | pingTime = c_int() 1008 | return c_GetPingTime(clientID, byref(pingTime)), pingTime.value 1009 | 1010 | def simxGetLastCmdTime(clientID): 1011 | ''' 1012 | Please have a look at the function description/documentation in the V-REP user manual 1013 | ''' 1014 | 1015 | return c_GetLastCmdTime(clientID) 1016 | 1017 | def simxSynchronousTrigger(clientID): 1018 | ''' 1019 | Please have a look at the function description/documentation in the V-REP user manual 1020 | ''' 1021 | 1022 | return c_SynchronousTrigger(clientID) 1023 | 1024 | def simxSynchronous(clientID, enable): 1025 | ''' 1026 | Please have a look at the function description/documentation in the V-REP user manual 1027 | ''' 1028 | 1029 | return c_Synchronous(clientID, enable) 1030 | 1031 | def simxPauseCommunication(clientID, enable): 1032 | ''' 1033 | Please have a look at the function description/documentation in the V-REP user manual 1034 | ''' 1035 | 1036 | return c_PauseCommunication(clientID, enable) 1037 | 1038 | def simxGetInMessageInfo(clientID, infoType): 1039 | ''' 1040 | Please have a look at the function description/documentation in the V-REP user manual 1041 | ''' 1042 | info = c_int() 1043 | return c_GetInMessageInfo(clientID, infoType, byref(info)), info.value 1044 | 1045 | def simxGetOutMessageInfo(clientID, infoType): 1046 | ''' 1047 | Please have a look at the function description/documentation in the V-REP user manual 1048 | ''' 1049 | info = c_int() 1050 | return c_GetOutMessageInfo(clientID, infoType, byref(info)), info.value 1051 | 1052 | def simxGetConnectionId(clientID): 1053 | ''' 1054 | Please have a look at the function description/documentation in the V-REP user manual 1055 | ''' 1056 | 1057 | return c_GetConnectionId(clientID) 1058 | 1059 | def simxCreateBuffer(bufferSize): 1060 | ''' 1061 | Please have a look at the function description/documentation in the V-REP user manual 1062 | ''' 1063 | 1064 | return c_CreateBuffer(bufferSize) 1065 | 1066 | def simxReleaseBuffer(buffer): 1067 | ''' 1068 | Please have a look at the function description/documentation in the V-REP user manual 1069 | ''' 1070 | 1071 | return c_ReleaseBuffer(buffer) 1072 | 1073 | def simxTransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode): 1074 | ''' 1075 | Please have a look at the function description/documentation in the V-REP user manual 1076 | ''' 1077 | 1078 | return c_TransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode) 1079 | 1080 | def simxEraseFile(clientID, fileName_serverSide, operationMode): 1081 | ''' 1082 | Please have a look at the function description/documentation in the V-REP user manual 1083 | ''' 1084 | 1085 | return c_EraseFile(clientID, fileName_serverSide, operationMode) 1086 | 1087 | def simxCreateDummy(clientID, size, color, operationMode): 1088 | ''' 1089 | Please have a look at the function description/documentation in the V-REP user manual 1090 | ''' 1091 | 1092 | handle = c_int() 1093 | if color != None: 1094 | c_color = (c_ubyte*12)(*color) 1095 | else: 1096 | c_color = None 1097 | return c_CreateDummy(clientID, size, c_color, byref(handle), operationMode), handle.value 1098 | 1099 | def simxQuery(clientID, signalName, signalValue, retSignalName, timeOutInMs): 1100 | ''' 1101 | Please have a look at the function description/documentation in the V-REP user manual 1102 | ''' 1103 | 1104 | retSignalLength = c_int(); 1105 | retSignalValue = pointer(c_ubyte()) 1106 | 1107 | ret = c_Query(clientID, signalName, signalValue, len(signalValue), retSignalName, byref(retSignalValue), byref(retSignalLength), timeOutInMs) 1108 | 1109 | a = bytearray() 1110 | if ret == 0: 1111 | for i in range(retSignalLength.value): 1112 | a.append(retSignalValue[i]) 1113 | 1114 | return ret, str(a) 1115 | 1116 | def simxGetObjectGroupData(clientID, objectType, dataType, operationMode): 1117 | ''' 1118 | Please have a look at the function description/documentation in the V-REP user manual 1119 | ''' 1120 | 1121 | handles =[] 1122 | intData =[] 1123 | floatData =[] 1124 | stringData =[] 1125 | handlesC = c_int() 1126 | handlesP = pointer(c_int()) 1127 | intDataC = c_int() 1128 | intDataP = pointer(c_int()) 1129 | floatDataC = c_int() 1130 | floatDataP = pointer(c_float()) 1131 | stringDataC = c_int() 1132 | stringDataP = pointer(c_char()) 1133 | ret = c_GetObjectGroupData(clientID, objectType, dataType, byref(handlesC), byref(handlesP), byref(intDataC), byref(intDataP), byref(floatDataC), byref(floatDataP), byref(stringDataC), byref(stringDataP), operationMode) 1134 | 1135 | if ret == 0: 1136 | for i in range(handlesC.value): 1137 | handles.append(handlesP[i]) 1138 | for i in range(intDataC.value): 1139 | intData.append(intDataP[i]) 1140 | for i in range(floatDataC.value): 1141 | floatData.append(floatDataP[i]) 1142 | s = 0 1143 | for i in range(stringDataC.value): 1144 | a = bytearray() 1145 | while stringDataP[s] != '\0': 1146 | a.append(stringDataP[s]) 1147 | s += 1 1148 | s += 1 #skip null 1149 | stringData.append(str(a)) 1150 | 1151 | return ret, handles, intData, floatData, stringData 1152 | 1153 | def simxGetObjectVelocity(clientID, objectHandle, operationMode): 1154 | ''' 1155 | Please have a look at the function description/documentation in the V-REP user manual 1156 | ''' 1157 | linearVel = (c_float*3)() 1158 | angularVel = (c_float*3)() 1159 | ret = c_GetObjectVelocity(clientID, objectHandle, linearVel, angularVel, operationMode) 1160 | arr1 = [] 1161 | for i in range(3): 1162 | arr1.append(linearVel[i]) 1163 | arr2 = [] 1164 | for i in range(3): 1165 | arr2.append(angularVel[i]) 1166 | return ret, arr1, arr2 1167 | 1168 | def simxPackInts(intList): 1169 | ''' 1170 | Please have a look at the function description/documentation in the V-REP user manual 1171 | ''' 1172 | s='' 1173 | for i in range(len(intList)): 1174 | s+=struct.pack('. 25 | # ------------------------------------------------------------------- 26 | # 27 | # This file was automatically created for V-REP release V3.1.3 on Sept. 30th 2014 28 | 29 | #constants 30 | #Scene object types. Values are serialized 31 | sim_object_shape_type =0 32 | sim_object_joint_type =1 33 | sim_object_graph_type =2 34 | sim_object_camera_type =3 35 | sim_object_dummy_type =4 36 | sim_object_proximitysensor_type =5 37 | sim_object_reserved1 =6 38 | sim_object_reserved2 =7 39 | sim_object_path_type =8 40 | sim_object_visionsensor_type =9 41 | sim_object_volume_type =10 42 | sim_object_mill_type =11 43 | sim_object_forcesensor_type =12 44 | sim_object_light_type =13 45 | sim_object_mirror_type =14 46 | 47 | #General object types. Values are serialized 48 | sim_appobj_object_type =109 49 | sim_appobj_collision_type =110 50 | sim_appobj_distance_type =111 51 | sim_appobj_simulation_type =112 52 | sim_appobj_ik_type =113 53 | sim_appobj_constraintsolver_type=114 54 | sim_appobj_collection_type =115 55 | sim_appobj_ui_type =116 56 | sim_appobj_script_type =117 57 | sim_appobj_pathplanning_type =118 58 | sim_appobj_RESERVED_type =119 59 | sim_appobj_texture_type =120 60 | 61 | # Ik calculation methods. Values are serialized 62 | sim_ik_pseudo_inverse_method =0 63 | sim_ik_damped_least_squares_method =1 64 | sim_ik_jacobian_transpose_method =2 65 | 66 | # Ik constraints. Values are serialized 67 | sim_ik_x_constraint =1 68 | sim_ik_y_constraint =2 69 | sim_ik_z_constraint =4 70 | sim_ik_alpha_beta_constraint=8 71 | sim_ik_gamma_constraint =16 72 | sim_ik_avoidance_constraint =64 73 | 74 | # Ik calculation results 75 | sim_ikresult_not_performed =0 76 | sim_ikresult_success =1 77 | sim_ikresult_fail =2 78 | 79 | # Scene object sub-types. Values are serialized 80 | # Light sub-types 81 | sim_light_omnidirectional_subtype =1 82 | sim_light_spot_subtype =2 83 | sim_light_directional_subtype =3 84 | # Joint sub-types 85 | sim_joint_revolute_subtype =10 86 | sim_joint_prismatic_subtype =11 87 | sim_joint_spherical_subtype =12 88 | # Shape sub-types 89 | sim_shape_simpleshape_subtype =20 90 | sim_shape_multishape_subtype =21 91 | # Proximity sensor sub-types 92 | sim_proximitysensor_pyramid_subtype =30 93 | sim_proximitysensor_cylinder_subtype=31 94 | sim_proximitysensor_disc_subtype =32 95 | sim_proximitysensor_cone_subtype =33 96 | sim_proximitysensor_ray_subtype =34 97 | # Mill sub-types 98 | sim_mill_pyramid_subtype =40 99 | sim_mill_cylinder_subtype =41 100 | sim_mill_disc_subtype =42 101 | sim_mill_cone_subtype =42 102 | # No sub-type 103 | sim_object_no_subtype =200 104 | 105 | 106 | #Scene object main properties (serialized) 107 | sim_objectspecialproperty_collidable =0x0001 108 | sim_objectspecialproperty_measurable =0x0002 109 | #reserved =0x0004 110 | #reserved =0x0008 111 | sim_objectspecialproperty_detectable_ultrasonic =0x0010 112 | sim_objectspecialproperty_detectable_infrared =0x0020 113 | sim_objectspecialproperty_detectable_laser =0x0040 114 | sim_objectspecialproperty_detectable_inductive =0x0080 115 | sim_objectspecialproperty_detectable_capacitive =0x0100 116 | sim_objectspecialproperty_renderable =0x0200 117 | sim_objectspecialproperty_detectable_all =sim_objectspecialproperty_detectable_ultrasonic|sim_objectspecialproperty_detectable_infrared|sim_objectspecialproperty_detectable_laser|sim_objectspecialproperty_detectable_inductive|sim_objectspecialproperty_detectable_capacitive 118 | sim_objectspecialproperty_cuttable =0x0400 119 | sim_objectspecialproperty_pathplanning_ignored =0x0800 120 | 121 | # Model properties (serialized) 122 | sim_modelproperty_not_collidable =0x0001 123 | sim_modelproperty_not_measurable =0x0002 124 | sim_modelproperty_not_renderable =0x0004 125 | sim_modelproperty_not_detectable =0x0008 126 | sim_modelproperty_not_cuttable =0x0010 127 | sim_modelproperty_not_dynamic =0x0020 128 | sim_modelproperty_not_respondable =0x0040 # cannot be selected if sim_modelproperty_not_dynamic is not selected 129 | sim_modelproperty_not_reset =0x0080 # Model is not reset at simulation end. This flag is cleared at simulation end 130 | sim_modelproperty_not_visible =0x0100 # Whole model is invisible independent of local visibility settings 131 | sim_modelproperty_not_model =0xf000 # object is not a model 132 | 133 | 134 | # Check the documentation instead of comments below!! 135 | # Following messages are dispatched to the Lua-message container 136 | sim_message_ui_button_state_change =0 # a UI button slider etc. changed (due to a user's action). aux[0]=UI handle aux[1]=button handle aux[2]=button attributes aux[3]=slider position (if slider) 137 | sim_message_reserved9 =1 # Do not use 138 | sim_message_object_selection_changed=2 139 | sim_message_reserved10 =3 # do not use 140 | sim_message_model_loaded =4 141 | sim_message_reserved11 =5 # do not use 142 | sim_message_keypress =6 # a key was pressed while the focus was on a page (aux[0]=key aux[1]=ctrl and shift key state) 143 | sim_message_bannerclicked =7 # a banner was clicked (aux[0]=banner ID) 144 | 145 | 146 | # Following messages are dispatched only to the C-API (not available from Lua) 147 | sim_message_for_c_api_only_start =0x100 # Do not use 148 | sim_message_reserved1 =0x101 # Do not use 149 | sim_message_reserved2 =0x102 # Do not use 150 | sim_message_reserved3 =0x103 # Do not use 151 | sim_message_eventcallback_scenesave =0x104 # about to save a scene 152 | sim_message_eventcallback_modelsave =0x105 # about to save a model (current selection will be saved) 153 | sim_message_eventcallback_moduleopen =0x106 # called when simOpenModule in Lua is called 154 | sim_message_eventcallback_modulehandle =0x107 # called when simHandleModule in Lua is called with argument false 155 | sim_message_eventcallback_moduleclose =0x108 # called when simCloseModule in Lua is called 156 | sim_message_reserved4 =0x109 # Do not use 157 | sim_message_reserved5 =0x10a # Do not use 158 | sim_message_reserved6 =0x10b # Do not use 159 | sim_message_reserved7 =0x10c # Do not use 160 | sim_message_eventcallback_instancepass =0x10d # Called once every main application loop pass. auxiliaryData[0] contains event flags of events that happened since last time 161 | sim_message_eventcallback_broadcast =0x10e 162 | sim_message_eventcallback_imagefilter_enumreset =0x10f 163 | sim_message_eventcallback_imagefilter_enumerate =0x110 164 | sim_message_eventcallback_imagefilter_adjustparams =0x111 165 | sim_message_eventcallback_imagefilter_reserved =0x112 166 | sim_message_eventcallback_imagefilter_process =0x113 167 | sim_message_eventcallback_reserved1 =0x114 # do not use 168 | sim_message_eventcallback_reserved2 =0x115 # do not use 169 | sim_message_eventcallback_reserved3 =0x116 # do not use 170 | sim_message_eventcallback_reserved4 =0x117 # do not use 171 | sim_message_eventcallback_abouttoundo =0x118 # the undo button was hit and a previous state is about to be restored 172 | sim_message_eventcallback_undoperformed =0x119 # the undo button was hit and a previous state restored 173 | sim_message_eventcallback_abouttoredo =0x11a # the redo button was hit and a future state is about to be restored 174 | sim_message_eventcallback_redoperformed =0x11b # the redo button was hit and a future state restored 175 | sim_message_eventcallback_scripticondblclick =0x11c # scipt icon was double clicked. (aux[0]=object handle associated with script set replyData[0] to 1 if script should not be opened) 176 | sim_message_eventcallback_simulationabouttostart =0x11d 177 | sim_message_eventcallback_simulationended =0x11e 178 | sim_message_eventcallback_reserved5 =0x11f # do not use 179 | sim_message_eventcallback_keypress =0x120 # a key was pressed while the focus was on a page (aux[0]=key aux[1]=ctrl and shift key state) 180 | sim_message_eventcallback_modulehandleinsensingpart =0x121 # called when simHandleModule in Lua is called with argument true 181 | sim_message_eventcallback_renderingpass =0x122 # called just before the scene is rendered 182 | sim_message_eventcallback_bannerclicked =0x123 # called when a banner was clicked (aux[0]=banner ID) 183 | sim_message_eventcallback_menuitemselected =0x124 # auxiliaryData[0] indicates the handle of the item auxiliaryData[1] indicates the state of the item 184 | sim_message_eventcallback_refreshdialogs =0x125 # aux[0]=refresh degree (0=light 1=medium 2=full) 185 | sim_message_eventcallback_sceneloaded =0x126 186 | sim_message_eventcallback_modelloaded =0x127 187 | sim_message_eventcallback_instanceswitch =0x128 188 | sim_message_eventcallback_guipass =0x129 189 | sim_message_eventcallback_mainscriptabouttobecalled =0x12a 190 | sim_message_eventcallback_rmlposition =0x12b #the command simRMLPosition was called. The appropriate plugin should handle the call 191 | sim_message_eventcallback_rmlvelocity =0x12c # the command simRMLVelocity was called. The appropriate plugin should handle the call 192 | sim_message_simulation_start_resume_request =0x1000 193 | sim_message_simulation_pause_request =0x1001 194 | sim_message_simulation_stop_request =0x1002 195 | 196 | # Scene object properties. Combine with the | operator 197 | sim_objectproperty_reserved1 =0x0000 198 | sim_objectproperty_reserved2 =0x0001 199 | sim_objectproperty_reserved3 =0x0002 200 | sim_objectproperty_reserved4 =0x0003 201 | sim_objectproperty_reserved5 =0x0004 # formely sim_objectproperty_visible 202 | sim_objectproperty_reserved6 =0x0008 # formely sim_objectproperty_wireframe 203 | sim_objectproperty_collapsed =0x0010 204 | sim_objectproperty_selectable =0x0020 205 | sim_objectproperty_reserved7 =0x0040 206 | sim_objectproperty_selectmodelbaseinstead =0x0080 207 | sim_objectproperty_dontshowasinsidemodel =0x0100 208 | # reserved =0x0200 209 | sim_objectproperty_canupdatedna =0x0400 210 | sim_objectproperty_selectinvisible =0x0800 211 | sim_objectproperty_depthinvisible =0x1000 212 | 213 | 214 | # type of arguments (input and output) for custom lua commands 215 | sim_lua_arg_nil =0 216 | sim_lua_arg_bool =1 217 | sim_lua_arg_int =2 218 | sim_lua_arg_float =3 219 | sim_lua_arg_string =4 220 | sim_lua_arg_invalid =5 221 | sim_lua_arg_table =8 222 | 223 | # custom user interface properties. Values are serialized. 224 | sim_ui_property_visible =0x0001 225 | sim_ui_property_visibleduringsimulationonly =0x0002 226 | sim_ui_property_moveable =0x0004 227 | sim_ui_property_relativetoleftborder =0x0008 228 | sim_ui_property_relativetotopborder =0x0010 229 | sim_ui_property_fixedwidthfont =0x0020 230 | sim_ui_property_systemblock =0x0040 231 | sim_ui_property_settocenter =0x0080 232 | sim_ui_property_rolledup =0x0100 233 | sim_ui_property_selectassociatedobject =0x0200 234 | sim_ui_property_visiblewhenobjectselected =0x0400 235 | 236 | 237 | # button properties. Values are serialized. 238 | sim_buttonproperty_button =0x0000 239 | sim_buttonproperty_label =0x0001 240 | sim_buttonproperty_slider =0x0002 241 | sim_buttonproperty_editbox =0x0003 242 | sim_buttonproperty_staydown =0x0008 243 | sim_buttonproperty_enabled =0x0010 244 | sim_buttonproperty_borderless =0x0020 245 | sim_buttonproperty_horizontallycentered =0x0040 246 | sim_buttonproperty_ignoremouse =0x0080 247 | sim_buttonproperty_isdown =0x0100 248 | sim_buttonproperty_transparent =0x0200 249 | sim_buttonproperty_nobackgroundcolor =0x0400 250 | sim_buttonproperty_rollupaction =0x0800 251 | sim_buttonproperty_closeaction =0x1000 252 | sim_buttonproperty_verticallycentered =0x2000 253 | sim_buttonproperty_downupevent =0x4000 254 | 255 | 256 | # Simulation status 257 | sim_simulation_stopped =0x00 # Simulation is stopped 258 | sim_simulation_paused =0x08 # Simulation is paused 259 | sim_simulation_advancing =0x10 # Simulation is advancing 260 | sim_simulation_advancing_firstafterstop =sim_simulation_advancing|0x00 # First simulation pass (1x) 261 | sim_simulation_advancing_running =sim_simulation_advancing|0x01 # Normal simulation pass (>=1x) 262 | # reserved =sim_simulation_advancing|0x02 263 | sim_simulation_advancing_lastbeforepause =sim_simulation_advancing|0x03 # Last simulation pass before pause (1x) 264 | sim_simulation_advancing_firstafterpause =sim_simulation_advancing|0x04 # First simulation pass after pause (1x) 265 | sim_simulation_advancing_abouttostop =sim_simulation_advancing|0x05 # "Trying to stop" simulation pass (>=1x) 266 | sim_simulation_advancing_lastbeforestop =sim_simulation_advancing|0x06 # Last simulation pass (1x) 267 | 268 | 269 | # Script execution result (first return value) 270 | sim_script_no_error =0 271 | sim_script_main_script_nonexistent =1 272 | sim_script_main_script_not_called =2 273 | sim_script_reentrance_error =4 274 | sim_script_lua_error =8 275 | sim_script_call_error =16 276 | 277 | 278 | # Script types (serialized!) 279 | sim_scripttype_mainscript =0 280 | sim_scripttype_childscript =1 281 | sim_scripttype_pluginscript =2 282 | sim_scripttype_threaded =0x00f0 # Combine with one of above's type values 283 | 284 | # API call error messages 285 | sim_api_errormessage_ignore =0 # does not memorize nor output errors 286 | sim_api_errormessage_report =1 # memorizes errors (default for C-API calls) 287 | sim_api_errormessage_output =2 # memorizes and outputs errors (default for Lua-API calls) 288 | 289 | 290 | # special argument of some functions 291 | sim_handle_all =-2 292 | sim_handle_all_except_explicit =-3 293 | sim_handle_self =-4 294 | sim_handle_main_script =-5 295 | sim_handle_tree =-6 296 | sim_handle_chain =-7 297 | sim_handle_single =-8 298 | sim_handle_default =-9 299 | sim_handle_all_except_self =-10 300 | sim_handle_parent =-11 301 | 302 | 303 | # special handle flags 304 | sim_handleflag_assembly =0x400000 305 | sim_handleflag_model =0x800000 306 | 307 | 308 | # distance calculation methods (serialized) 309 | sim_distcalcmethod_dl =0 310 | sim_distcalcmethod_dac =1 311 | sim_distcalcmethod_max_dl_dac =2 312 | sim_distcalcmethod_dl_and_dac =3 313 | sim_distcalcmethod_sqrt_dl2_and_dac2=4 314 | sim_distcalcmethod_dl_if_nonzero =5 315 | sim_distcalcmethod_dac_if_nonzero =6 316 | 317 | 318 | # Generic dialog styles 319 | sim_dlgstyle_message =0 320 | sim_dlgstyle_input =1 321 | sim_dlgstyle_ok =2 322 | sim_dlgstyle_ok_cancel =3 323 | sim_dlgstyle_yes_no =4 324 | sim_dlgstyle_dont_center =32# can be combined with one of above values. Only with this flag can the position of the related UI be set just after dialog creation 325 | 326 | # Generic dialog return values 327 | sim_dlgret_still_open =0 328 | sim_dlgret_ok =1 329 | sim_dlgret_cancel =2 330 | sim_dlgret_yes =3 331 | sim_dlgret_no =4 332 | 333 | 334 | # Path properties 335 | sim_pathproperty_show_line =0x0001 336 | sim_pathproperty_show_orientation =0x0002 337 | sim_pathproperty_closed_path =0x0004 338 | sim_pathproperty_automatic_orientation =0x0008 339 | sim_pathproperty_invert_velocity =0x0010 340 | sim_pathproperty_infinite_acceleration =0x0020 341 | sim_pathproperty_flat_path =0x0040 342 | sim_pathproperty_show_position =0x0080 343 | sim_pathproperty_auto_velocity_profile_translation =0x0100 344 | sim_pathproperty_auto_velocity_profile_rotation =0x0200 345 | sim_pathproperty_endpoints_at_zero =0x0400 346 | sim_pathproperty_keep_x_up =0x0800 347 | 348 | 349 | # drawing objects 350 | # following are mutually exclusive 351 | sim_drawing_points =0 # 3 values per point (point size in pixels) 352 | sim_drawing_lines =1 # 6 values per line (line size in pixels) 353 | sim_drawing_triangles =2 # 9 values per triangle 354 | sim_drawing_trianglepoints =3 # 6 values per point (3 for triangle position 3 for triangle normal vector) (triangle size in meters) 355 | sim_drawing_quadpoints =4 # 6 values per point (3 for quad position 3 for quad normal vector) (quad size in meters) 356 | sim_drawing_discpoints =5 # 6 values per point (3 for disc position 3 for disc normal vector) (disc size in meters) 357 | sim_drawing_cubepoints =6 # 6 values per point (3 for cube position 3 for cube normal vector) (cube size in meters) 358 | sim_drawing_spherepoints =7 # 3 values per point (sphere size in meters) 359 | 360 | # following can be or-combined 361 | sim_drawing_itemcolors =0x00020 # +3 values per item (each item has its own ambient color (rgb values)). 362 | # Mutually exclusive with sim_drawing_vertexcolors 363 | sim_drawing_vertexcolors =0x00040 # +3 values per vertex (each vertex has its own ambient color (rgb values). Only for sim_drawing_lines (+6) and for sim_drawing_triangles(+9)). Mutually exclusive with sim_drawing_itemcolors 364 | sim_drawing_itemsizes =0x00080 # +1 value per item (each item has its own size). Not for sim_drawing_triangles 365 | sim_drawing_backfaceculling =0x00100 # back faces are not displayed for all items 366 | sim_drawing_wireframe =0x00200 # all items displayed in wireframe 367 | sim_drawing_painttag =0x00400 # all items are tagged as paint (for additinal processing at a later stage) 368 | sim_drawing_followparentvisibility =0x00800 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible 369 | sim_drawing_cyclic =0x01000 # if the max item count was reached then the first items are overwritten. 370 | sim_drawing_50percenttransparency =0x02000 # the drawing object will be 50% transparent 371 | sim_drawing_25percenttransparency =0x04000 # the drawing object will be 25% transparent 372 | sim_drawing_12percenttransparency =0x08000 # the drawing object will be 12.5% transparent 373 | sim_drawing_emissioncolor =0x10000 # When used in combination with sim_drawing_itemcolors or sim_drawing_vertexcolors then the specified colors will be for the emissive component 374 | sim_drawing_facingcamera =0x20000 # Only for trianglepoints quadpoints discpoints and cubepoints. If specified the normal verctor is calculated to face the camera (each item data requires 3 values less) 375 | sim_drawing_overlay =0x40000 # When specified objects are always drawn on top of "regular objects" 376 | sim_drawing_itemtransparency =0x80000 # +1 value per item (each item has its own transparency value (0-1)). Not compatible with sim_drawing_vertexcolors 377 | 378 | # banner values 379 | # following can be or-combined 380 | sim_banner_left =0x00001 # Banners display on the left of the specified point 381 | sim_banner_right =0x00002 # Banners display on the right of the specified point 382 | sim_banner_nobackground =0x00004 # Banners have no background rectangle 383 | sim_banner_overlay =0x00008 # When specified banners are always drawn on top of "regular objects" 384 | sim_banner_followparentvisibility =0x00010 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible 385 | sim_banner_clickselectsparent =0x00020 # if the object is associated with a scene object then clicking the banner will select the scene object 386 | sim_banner_clicktriggersevent =0x00040 # if the banner is clicked an event is triggered (sim_message_eventcallback_bannerclicked and sim_message_bannerclicked are generated) 387 | sim_banner_facingcamera =0x00080 # If specified the banner will always face the camera by rotating around the banner's vertical axis (y-axis) 388 | sim_banner_fullyfacingcamera =0x00100 # If specified the banner will always fully face the camera (the banner's orientation is same as the camera looking at it) 389 | sim_banner_backfaceculling =0x00200 # If specified the banner will only be visible from one side 390 | sim_banner_keepsamesize =0x00400 # If specified the banner will always appear in the same size. In that case size represents the character height in pixels 391 | sim_banner_bitmapfont =0x00800 # If specified a fixed-size bitmap font is used. The text will also always fully face the camera and be right 392 | # to the specified position. Bitmap fonts are not clickable 393 | 394 | 395 | # particle objects following are mutually exclusive 396 | sim_particle_points1 =0 # 6 values per point (pt1 and pt2. Pt1 is start position pt2-pt1 is the initial velocity vector). i 397 | #Point is 1 pixel big. Only appearance is a point internally handled as a perfect sphere 398 | sim_particle_points2 =1 # 6 values per point. Point is 2 pixel big. Only appearance is a point internally handled as a perfect sphere 399 | sim_particle_points4 =2 # 6 values per point. Point is 4 pixel big. Only appearance is a point internally handled as a perfect sphere 400 | sim_particle_roughspheres =3 # 6 values per sphere. Only appearance is rough. Internally a perfect sphere 401 | sim_particle_spheres =4 # 6 values per sphere. Internally a perfect sphere 402 | 403 | 404 | 405 | 406 | # following can be or-combined 407 | sim_particle_respondable1to4 =0x0020 # the particles are respondable against shapes (against all objects that have at least one bit 1-4 activated in the global respondable mask) 408 | sim_particle_respondable5to8 =0x0040 # the particles are respondable against shapes (against all objects that have at least one bit 5-8 activated in the global respondable mask) 409 | sim_particle_particlerespondable =0x0080 # the particles are respondable against each other 410 | sim_particle_ignoresgravity =0x0100 # the particles ignore the effect of gravity. Not compatible with sim_particle_water 411 | sim_particle_invisible =0x0200 # the particles are invisible 412 | sim_particle_itemsizes =0x0400 # +1 value per particle (each particle can have a different size) 413 | sim_particle_itemdensities =0x0800 # +1 value per particle (each particle can have a different density) 414 | sim_particle_itemcolors =0x1000 # +3 values per particle (each particle can have a different color) 415 | sim_particle_cyclic =0x2000 # if the max item count was reached then the first items are overwritten. 416 | sim_particle_emissioncolor =0x4000 # When used in combination with sim_particle_itemcolors then the specified colors will be for the emissive component 417 | sim_particle_water =0x8000 # the particles are water particles (no weight in the water (i.e. when z<0)). Not compatible with sim_particle_ignoresgravity 418 | sim_particle_painttag =0x10000 # The particles can be seen by vision sensors (sim_particle_invisible must not be set) 419 | 420 | 421 | 422 | 423 | # custom user interface menu attributes 424 | sim_ui_menu_title =1 425 | sim_ui_menu_minimize =2 426 | sim_ui_menu_close =4 427 | sim_ui_menu_systemblock =8 428 | 429 | 430 | 431 | # Boolean parameters 432 | sim_boolparam_hierarchy_visible =0 433 | sim_boolparam_console_visible =1 434 | sim_boolparam_collision_handling_enabled =2 435 | sim_boolparam_distance_handling_enabled =3 436 | sim_boolparam_ik_handling_enabled =4 437 | sim_boolparam_gcs_handling_enabled =5 438 | sim_boolparam_dynamics_handling_enabled =6 439 | sim_boolparam_joint_motion_handling_enabled =7 440 | sim_boolparam_path_motion_handling_enabled =8 441 | sim_boolparam_proximity_sensor_handling_enabled =9 442 | sim_boolparam_vision_sensor_handling_enabled =10 443 | sim_boolparam_mill_handling_enabled =11 444 | sim_boolparam_browser_visible =12 445 | sim_boolparam_scene_and_model_load_messages =13 446 | sim_reserved0 =14 447 | sim_boolparam_shape_textures_are_visible =15 448 | sim_boolparam_display_enabled =16 449 | sim_boolparam_infotext_visible =17 450 | sim_boolparam_statustext_open =18 451 | sim_boolparam_fog_enabled =19 452 | sim_boolparam_rml2_available =20 453 | sim_boolparam_rml4_available =21 454 | sim_boolparam_mirrors_enabled =22 455 | sim_boolparam_aux_clip_planes_enabled =23 456 | sim_boolparam_full_model_copy_from_api =24 457 | sim_boolparam_realtime_simulation =25 458 | sim_boolparam_video_recording_triggered =29 459 | 460 | 461 | # Integer parameters 462 | sim_intparam_error_report_mode =0 # Check sim_api_errormessage_... constants above for valid values 463 | sim_intparam_program_version =1 # e.g Version 2.1.4 --> 20104. Can only be read 464 | sim_intparam_instance_count =2 # do not use anymore (always returns 1 since V-REP 2.5.11) 465 | sim_intparam_custom_cmd_start_id =3 # can only be read 466 | sim_intparam_compilation_version =4 # 0=evaluation version 1=full version 2=player version. Can only be read 467 | sim_intparam_current_page =5 468 | sim_intparam_flymode_camera_handle =6 # can only be read 469 | sim_intparam_dynamic_step_divider =7 # can only be read 470 | sim_intparam_dynamic_engine =8 # 0=Bullet 1=ODE. 2=Vortex. 471 | sim_intparam_server_port_start =9 # can only be read 472 | sim_intparam_server_port_range =10 # can only be read 473 | sim_intparam_visible_layers =11 474 | sim_intparam_infotext_style =12 475 | sim_intparam_settings =13 476 | sim_intparam_edit_mode_type =14 # can only be read 477 | sim_intparam_server_port_next =15 # is initialized at sim_intparam_server_port_start 478 | sim_intparam_qt_version =16 # version of the used Qt framework 479 | sim_intparam_event_flags_read =17 # can only be read 480 | sim_intparam_event_flags_read_clear =18 # can only be read 481 | sim_intparam_platform =19 # can only be read 482 | sim_intparam_scene_unique_id =20 # can only be read 483 | 484 | # Float parameters 485 | sim_floatparam_rand=0 # random value (0.0-1.0) 486 | sim_floatparam_simulation_time_step =1 487 | 488 | # String parameters 489 | sim_stringparam_application_path=0 # path of V-REP's executable 490 | sim_stringparam_video_filename=1 491 | 492 | # Array parameters 493 | sim_arrayparam_gravity =0 494 | sim_arrayparam_fog =1 495 | sim_arrayparam_fog_color =2 496 | sim_arrayparam_background_color1=3 497 | sim_arrayparam_background_color2=4 498 | sim_arrayparam_ambient_light =5 499 | 500 | 501 | # User interface elements 502 | sim_gui_menubar =0x0001 503 | sim_gui_popups =0x0002 504 | sim_gui_toolbar1 =0x0004 505 | sim_gui_toolbar2 =0x0008 506 | sim_gui_hierarchy =0x0010 507 | sim_gui_infobar =0x0020 508 | sim_gui_statusbar =0x0040 509 | sim_gui_scripteditor =0x0080 510 | sim_gui_scriptsimulationparameters =0x0100 511 | sim_gui_dialogs =0x0200 512 | sim_gui_browser =0x0400 513 | sim_gui_all =0xffff 514 | 515 | 516 | # Joint modes 517 | sim_jointmode_passive =0 518 | sim_jointmode_motion =1 519 | sim_jointmode_ik =2 520 | sim_jointmode_ikdependent =3 521 | sim_jointmode_dependent =4 522 | sim_jointmode_force =5 523 | 524 | 525 | # Navigation and selection modes with the mouse. Lower byte values are mutually exclusive upper byte bits can be combined 526 | sim_navigation_passive =0x0000 527 | sim_navigation_camerashift =0x0001 528 | sim_navigation_camerarotate =0x0002 529 | sim_navigation_camerazoom =0x0003 530 | sim_navigation_cameratilt =0x0004 531 | sim_navigation_cameraangle =0x0005 532 | sim_navigation_camerafly =0x0006 533 | sim_navigation_objectshift =0x0007 534 | sim_navigation_objectrotate =0x0008 535 | sim_navigation_reserved2 =0x0009 536 | sim_navigation_reserved3 =0x000A 537 | sim_navigation_jointpathtest =0x000B 538 | sim_navigation_ikmanip =0x000C 539 | sim_navigation_objectmultipleselection =0x000D 540 | # Bit-combine following values and add them to one of above's values for a valid navigation mode 541 | sim_navigation_reserved4 =0x0100 542 | sim_navigation_clickselection =0x0200 543 | sim_navigation_ctrlselection =0x0400 544 | sim_navigation_shiftselection =0x0800 545 | sim_navigation_camerazoomwheel =0x1000 546 | sim_navigation_camerarotaterightbutton =0x2000 547 | 548 | 549 | 550 | #Remote API constants 551 | SIMX_VERSION =0 552 | # Remote API message header structure 553 | SIMX_HEADER_SIZE =18 554 | simx_headeroffset_crc =0 # 1 simxUShort. Generated by the client or server. The CRC for the message 555 | simx_headeroffset_version =2 # 1 byte. Generated by the client or server. The version of the remote API software 556 | simx_headeroffset_message_id =3 # 1 simxInt. Generated by the client (and used in a reply by the server) 557 | simx_headeroffset_client_time =7 # 1 simxInt. Client time stamp generated by the client (and sent back by the server) 558 | simx_headeroffset_server_time =11 # 1 simxInt. Generated by the server when a reply is generated. The server timestamp 559 | simx_headeroffset_scene_id =15 # 1 simxUShort. Generated by the server. A unique ID identifying the scene currently displayed 560 | 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) 561 | 562 | # Remote API command header 563 | SIMX_SUBHEADER_SIZE =26 564 | simx_cmdheaderoffset_mem_size =0 # 1 simxInt. Generated by the client or server. The buffer size of the command. 565 | 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). 566 | simx_cmdheaderoffset_pdata_offset0 =8 # 1 simxUShort. Generated by the client or server. The amount of data that is part of the command identification. 567 | 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). 568 | 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. 569 | 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). 570 | 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) 571 | 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 572 | simx_cmdheaderoffset_reserved =25 # 1 byte. Not yet used 573 | 574 | 575 | 576 | 577 | 578 | # Regular operation modes 579 | simx_opmode_oneshot =0x000000 # sends command as one chunk. Reply will also come as one chunk. Doesn't wait for the reply. 580 | 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). 581 | simx_opmode_continuous =0x020000 582 | simx_opmode_streaming =0x020000 # sends command as one chunk. Command will be stored on the server and always executed 583 | #(every x ms (as far as possible) where x can be 0-65535. just add x to opmode_continuous). 584 | # A reply will be sent continuously each time as one chunk. Doesn't wait for the reply. 585 | 586 | # Operation modes for heavy data 587 | 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. 588 | simx_opmode_continuous_split =0x040000 589 | 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. 590 | 591 | # Special operation modes 592 | simx_opmode_discontinue =0x050000 # removes and cancels all commands stored on the client or server side (also continuous commands) 593 | 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) 594 | 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) 595 | 596 | 597 | # Command return codes 598 | simx_return_ok =0x000000 599 | simx_return_novalue_flag =0x000001 # input buffer doesn't contain the specified command 600 | simx_return_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode 601 | simx_return_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode 602 | simx_return_remote_error_flag =0x000008 # command caused an error on the server side 603 | simx_return_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) 604 | simx_return_local_error_flag =0x000020 # command caused an error on the client side 605 | simx_return_initialize_error_flag =0x000040 # simxStart was not yet called 606 | 607 | # Following for backward compatibility (same as above) 608 | simx_error_noerror =0x000000 609 | simx_error_novalue_flag =0x000001 # input buffer doesn't contain the specified command 610 | simx_error_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode 611 | simx_error_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode 612 | simx_error_remote_error_flag =0x000008 # command caused an error on the server side 613 | simx_error_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) 614 | simx_error_local_error_flag =0x000020 # command caused an error on the client side 615 | simx_error_initialize_error_flag =0x000040 # simxStart was not yet called 616 | 617 | 618 | --------------------------------------------------------------------------------