def get_handles(self):
# retrieve motor handles
_, self.left_motor_handle = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_blocking)
_, self.right_motor_handle = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_blocking)
# empty list for handles
self.proxy_sensors = []
# for loop to retrieve proxy sensor arrays and initiate sensors
for i in range(16):
_, sensor_handle = vrep.simxGetObjectHandle(
self.client_id, 'ultrasonic_sensor#' + str(i),
vrep.simx_opmode_blocking)
# Append to the list of sensors
self.proxy_sensors.append(sensor_handle)
# empty list for handles
self.light_sensors = []
# for loop to retrieve light sensor arrays and initiate sensors
for i in range(8):
_, sensor_handle = vrep.simxGetObjectHandle(
self.client_id, 'light_sensor#' + str(i),
vrep.simx_opmode_blocking)
# Append to the list of sensors
self.light_sensors.append(sensor_handle)
def time(self):
"""Query current time. Simulation starts at time 0.
Returns:
Elapsed simulated seconds.
"""
# query a fake handle just to get correct time afterwards
vrep.simxGetObjectHandle(self._clientID, "dummy", vrep.simx_opmode_oneshot_wait)
t = vrep.simxGetLastCmdTime(self._clientID)
return t / 1000.0
def time(self):
"""Query current time. Simulation starts at time 0.
Returns:
Elapsed simulated seconds.
"""
# query a fake handle just to get correct time afterwards
vrep.simxGetObjectHandle(self._clientID, "dummy",
vrep.simx_opmode_oneshot_wait)
t = vrep.simxGetLastCmdTime(self._clientID)
return t / 1000.0
def ini_engins(self):
error_code_left, self.left_motor_handle = vrep.simxGetObjectHandle(
self.clientId, 'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_oneshot_wait)
error_code_right, self.right_motor_handle = vrep.simxGetObjectHandle(
self.clientId, 'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_oneshot_wait)
if error_code_left == -1:
print('Can' 't find left or right motor')
if error_code_right == -1:
print('Can' 't find left or right motor')
def sleep(self, wait_in_sec):
"""Wait for the specified number of seconds (simulation time).
The simulation will continue making progress.
Args:
wait_in_sec (float): time (in seconds) to continue simulation.
"""
# query a fake handle just to get correct time afterwards
vrep.simxGetObjectHandle(self._clientID, "dummy", vrep.simx_opmode_oneshot_wait)
start = vrep.simxGetLastCmdTime(self._clientID)
while True:
vrep.simxSynchronousTrigger(self._clientID)
time = vrep.simxGetLastCmdTime(self._clientID)
if start + wait_in_sec * 1000 <= time:
break
def get_handle(self, name):
(check,
handle) = vrep.simxGetObjectHandle(self.ID, name,
const_v.simx_opmode_blocking)
if check != 0:
print("Couldn't find %s" % name)
return handle
def ini_sensors(self):
self.sensor_list = {}
for i in range(0, self.COUNT_SENSOR):
name_of_sensor = self.SENSOR_NAME + str(i)
error_code_left, sensor = vrep.simxGetObjectHandle(
self.clientId, name_of_sensor, vrep.simx_opmode_blocking)
self.sensor_list[name_of_sensor] = sensor
def get_object_handle(self, obj_name):
ret, handle = vrep.simxGetObjectHandle(self.client_id, obj_name,
vrep.simx_opmode_blocking)
if ret == vrep.simx_return_ok:
return handle
else:
raise Exception("Failed to get object handle. Object name: " +
obj_name)
def get_handle(self, name):
# print(name)
(check,
handle) = vrep.simxGetObjectHandle(self.ID, name,
const_v.simx_opmode_oneshot_wait)
if check != 0:
print("Couldn't find %s" % name)
return handle
def sleep(self, wait_in_sec):
"""Wait for the specified number of seconds (simulation time).
The simulation will continue making progress.
Args:
wait_in_sec (float): time (in seconds) to continue simulation.
"""
# query a fake handle just to get correct time afterwards
vrep.simxGetObjectHandle(self._clientID, "dummy",
vrep.simx_opmode_oneshot_wait)
start = vrep.simxGetLastCmdTime(self._clientID)
while True:
vrep.simxSynchronousTrigger(self._clientID)
time = vrep.simxGetLastCmdTime(self._clientID)
if start + wait_in_sec * 1000 <= time:
break
def __init__(self, client_id):
"""Constructor.
Args:
client_id (integer): V-REP client id.
"""
self._clientID = client_id
# query objects
rc, self._sensor = vrep.simxGetObjectHandle(self._clientID, "Proximity_sensor", vrep.simx_opmode_oneshot_wait)
def __init__(self, client_id):
self._clientID = client_id
# query joints
self._joints = []
for i in range(1, 8):
rc, joint = vrep.simxGetObjectHandle(
self._clientID, "LBR4p_joint{}".format(i),
vrep.simx_opmode_oneshot_wait)
self._joints.append(joint)
def __init__(self, client_id):
"""Constructor.
Args:
client_id (integer): V-REP client id.
"""
self._clientID = client_id
# query objects
rc, self._sensor = vrep.simxGetObjectHandle(
self._clientID, "Proximity_sensor", vrep.simx_opmode_oneshot_wait)
def getRobot(self, name):
if name not in ['ePuck']:
raise Exception('VrepWorld : no robot named ' + name)
_, robot_handle = vrep.simxGetObjectHandle(self.clientID, name,
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception("VrepRobot could not get robot")
return VrepRobot(name, self.clientID, robot_handle)
def setObstacle(self, dx, dy):
_, handle = vrep.simxGetObjectHandle(self.clientID, 'ObstacleTest',
vrep.simx_opmode_blocking)
_, newHandle = vrep.simxCopyPasteObjects(self.clientID, [handle],
vrep.simx_opmode_blocking)
self.obstacles.append(newHandle[0])
obs = VrepObstacle("Obstacle" + str(len(self.obstacles) - 1),
self.clientID, self.obstacles[-1])
obs.moveTo(dx, dy)
return obs
def __init__(self, client_id):
"""Constructor.
Args:
client_id (integer): V-REP client id.
"""
self._clientID = client_id
# query objects
rc, self._obj = vrep.simxGetObjectHandle(self._clientID, "create_estimate", vrep.simx_opmode_oneshot_wait)
# Use custom GUI
_, self._uiHandle = vrep.simxGetUIHandle(self._clientID, "UI", vrep.simx_opmode_oneshot_wait)
vrep.simxGetUIEventButton(self._clientID, self._uiHandle, vrep.simx_opmode_streaming)
def __init__(self, client_id):
self._clientID = client_id
# query wheel joints
rc, self._rightWheelJoint = vrep.simxGetObjectHandle(self._clientID, "right_wheel_joint",
vrep.simx_opmode_oneshot_wait)
rc, self._leftWheelJoint = vrep.simxGetObjectHandle(self._clientID, "left_wheel_joint",
vrep.simx_opmode_oneshot_wait)
# query create
rc, self._create = vrep.simxGetObjectHandle(self._clientID, "create",
vrep.simx_opmode_oneshot_wait)
# start the simulation:
vrep.simxStartSimulation(self._clientID, vrep.simx_opmode_oneshot_wait)
self._mode = Mode.Off
self._leftEncoderCount = 0
self._rightEncoderCount = 0
self._lastPosRight = None
self._lastPosLeft = None
self._sensorIDs = None
def __init__(self, name, clientId, handle):
self.name = name
self.clientID = clientId
self.handle = handle
#get handles for the wheel motors and stop them
_, self.leftJointHandle = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_blocking)
_, self.rightJointHandle = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.clientID, self.leftJointHandle, 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(self.clientID, self.rightJointHandle,
0, vrep.simx_opmode_oneshot)
# get handles for the proximity sensors
self.proximitySensorsHandles = [None] * 8 #empty table of size 8
for i in range(8):
_, self.proximitySensorsHandles[i] = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_proxSensor' + str(i + 1),
vrep.simx_opmode_blocking)
def __init__(self, clientID, suffix=""):
# vrep.simxFinish(-1) # just in case, close all opened connections
# self._clientID = vrep.simxStart('127.0.0.1',19997, True, True, 5000, 5) # Connect to V-REP
self._clientID = clientID
self.suffix = suffix
_, self._left_joint = vrep.simxGetObjectHandle(self._clientID, 'ePuck_leftJoint' + suffix, vrep.simx_opmode_oneshot_wait)
_, self._right_joint = vrep.simxGetObjectHandle(self._clientID, 'ePuck_rightJoint' + suffix, vrep.simx_opmode_oneshot_wait)
_, self._light_sensor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_lightSensor' + suffix, vrep.simx_opmode_oneshot_wait)
_, self._camera = vrep.simxGetObjectHandle(self._clientID, 'ePuck_camera' + suffix, vrep.simx_opmode_oneshot_wait)
self._prox_handles = []
for i in range(1,9):
_, p = vrep.simxGetObjectHandle(self._clientID, 'ePuck_proxSensor' + str(i) + suffix, vrep.simx_opmode_oneshot_wait)
self._prox_handles.append(p)
# First calls with simx_opmode_streaming
for i in range(8):
vrep.simxReadProximitySensor(self._clientID, self._prox_handles[i], vrep.simx_opmode_streaming)
_, self.camera_resolution, _ = vrep.simxGetVisionSensorImage(self._clientID, self._camera, options=0, operationMode=vrep.simx_opmode_streaming)
_, self.light_sensor_resolution, _ = vrep.simxGetVisionSensorImage(self._clientID, self._light_sensor, options=0, operationMode=vrep.simx_opmode_streaming)
self._body = vrep.simxGetObjectHandle(self._clientID, "ePuck_bodyElements" + suffix, vrep.simx_opmode_oneshot_wait)
self.wheel_diameter = 4.25 * 10 ** -2
self.base_lenght = 7 * 10 ** -2
self._prox_aliases = {"all" : range(8),
"all-but-rear" : range(6),
"front" : [2, 3],
"rear" : [6, 7],
"front-left" : [0, 1, 2],
"front-right": [3, 4, 5]}
self.no_detection_value = 2000.
self._fwd_spd, self._rot_spd = 0., 0.
self._left_spd, self._right_spd = 0., 0.
self.fwd_spd, self.rot_spd = 0., 0.
vrep.simxGetFloatSignal(self._clientID, "CurrentTime", vrep.simx_opmode_streaming)
self.freq = 100
self._behaviors = {}
self._runnings = {}
self._to_detach = {}
self._sensations = {}
self._conditions = {}
self._registered_objects = {}
_, _, _ , _, _ = vrep.simxGetObjectGroupData(self._clientID, vrep.sim_object_shape_type, 0, vrep.simx_opmode_streaming)
sleep(0.5)
self.register_all_scene_objects()
def __init__(self, client_id):
"""Constructor.
Args:
client_id (integer): V-REP client id.
"""
self._clientID = client_id
# query objects
rc, self._obj = vrep.simxGetObjectHandle(self._clientID,
"create_estimate",
vrep.simx_opmode_oneshot_wait)
# Use custom GUI
_, self._uiHandle = vrep.simxGetUIHandle(self._clientID, "UI",
vrep.simx_opmode_oneshot_wait)
vrep.simxGetUIEventButton(self._clientID, self._uiHandle,
vrep.simx_opmode_streaming)
def __init__(self, hostname):
"""Constructor.
Args:
hostname (string): IP address for host running the simulation.
"""
vrep.simxFinish(-1)
self._clientID = vrep.simxStart(hostname, 19997, True, False, 5000, 5) # Connect to V-REP
print("clientId:", self._clientID)
# query objects
rc, self._obj = vrep.simxGetObjectHandle(self._clientID, "create_estimate", vrep.simx_opmode_oneshot_wait)
print("Return code 1:", rc)
# Use custom GUI
rc, self._uiHandle = vrep.simxGetUIHandle(self._clientID, "UI", vrep.simx_opmode_oneshot_wait)
print("Return code 2:", rc)
vrep.simxGetUIEventButton(self._clientID, self._uiHandle, vrep.simx_opmode_streaming)
print("RealCreate uiHandle:", self._uiHandle)
def get_handle(self, name):
status, handle = vrep.simxGetObjectHandle(self.id, name,
vrep.simx_opmode_blocking)
self.check_return_code(status)
return handle
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19990, True, True, 5000, 5)
if (clientID != -1):
print('Connected to remote API server')
else:
print('Connection not successful')
sys.exit('Could not connect')
collision_objects = [0] * 9
for i in range(9):
_, collision_objects[i] = vrep.simxGetCollisionHandle(
clientID, 'Collision{}'.format(i), vrep.simx_opmode_blocking)
ret2, target = vrep.simxGetObjectHandle(clientID, 'target',
vrep.simx_opmode_blocking)
ret3, dummy = vrep.simxGetObjectHandle(clientID, 'Dummy',
vrep.simx_opmode_blocking)
joint_handles = list()
for i in range(6):
handle = 'joint_{}'.format(i + 1)
_, joint_handle = vrep.simxGetObjectHandle(clientID, handle,
vrep.simx_opmode_blocking)
joint_handles.append(joint_handle)
while (True):
for i in range(6):
q = np.random.uniform(-2.61799387, 2.61799387, 1)
rc = vrep.simxSetJointPosition(clientID, joint_handles[i], q,
vrep.simx_opmode_oneshot)
if (rc != 0):
print('Return Code = {}'.format(rc))
def _get_object_handle(self, name):
code, handle = v.simxGetObjectHandle(self._id, name, self._def_op_mode)
if code == v.simx_return_ok:
return handle
else:
return None
def register_object(self, name):
res, handle = vrep.simxGetObjectHandle(self._clientID, name, vrep.simx_opmode_oneshot_wait)
if res == vrep.simx_return_ok:
self._registered_objects[handle] = name
else:
print 'Object "' + name + '" does not exist in the current VREP scene'
import time
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
else:
print('Failed connecting to remote API server')
print('Program ended')
#vrep.sim_object_shape_type
#vrep.simx_opmode_oneshot
#simxGetObjectHandle
errorCode, carro = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx',
vrep.simx_opmode_oneshot_wait)
errorCode, left_Motor = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_oneshot_wait)
errorCode, right_Motor = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
errorCode, sensor1 = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_ultrasonicSensor1', vrep.simx_opmode_oneshot_wait)
errorCode, cam1 = vrep.simxGetObjectHandle(clientID, 'cam2',
vrep.simx_opmode_oneshot_wait)
#
#vrep.simxReadProximitySensor
returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
clientID, sensor1, vrep.simx_opmode_streaming)
time.sleep(1)
returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(