def teardown(self):
print 'Stopping data streaming for all collisions.'
for collision in COLLISION_OBJECTS:
collision_handle = self._scene_handles[collision]
vrep.simxReadCollision(self._client_id, collision_handle,
vrep.simx_opmode_discontinue)
print 'Stopping data streaming for all proximity sensors.'
for sensor in PROXIMITY_SENSORS:
sensor_handle = self._scene_handles[sensor]
vrep.simxReadProximitySensor(self._client_id, sensor_handle,
vrep.simx_opmode_discontinue)
print 'Stopping data streaming for robot tip.'
code = vrep.simxGetObjectPosition(self._client_id,
self._scene_handles[TIP_OBJECT], -1,
vrep.simx_opmode_discontinue)
# pray that graceful shutdown actually happened
print 'Stopping simulation.'
vrep.simxStopSimulation(self._client_id, vrep.simx_opmode_blocking)
time.sleep(0.1)
return True
def startDataStreaming():
if (not clientID):
print('connect first')
elif (clientID[0] == -1):
print('connect first')
else:
#Position
for i in Objects_handle:
vrep.simxGetObjectPosition(clientID[0], i, UR3_handle[0],
vrep.simx_opmode_streaming)
#Dummy position
vrep.simxGetObjectPosition(clientID[0], end_effector[0], UR3_handle[0],
vrep.simx_opmode_streaming)
#vision
for i in vision_sensor:
vrep.simxGetVisionSensorImage(clientID[0], i, 0,
vrep.simx_opmode_streaming)
#collision
for i in collision_handle:
vrep.simxReadCollision(clientID[0], i, vrep.simx_opmode_streaming)
#UR3 Joint
for i in UR3_joint_handle:
vrep.simxGetJointPosition(clientID[0], i,
vrep.simx_opmode_streaming)
def setup_devices():
""" Assign the devices from the simulator to specific IDs """
global robotID, left_motorID, right_motorID, kinect_depthID, collisionID
# res: result (1(OK), -1(error), 0(not called))
# robot
res, robotID = vrep.simxGetObjectHandle(clientID, 'robot#', WAIT)
# people
# motors
res, left_motorID = vrep.simxGetObjectHandle(clientID, 'leftMotor#', WAIT)
res, right_motorID = vrep.simxGetObjectHandle(clientID, 'rightMotor#',
WAIT)
# kinetic
res, kinect_depthID = vrep.simxGetObjectHandle(clientID, 'kinect_depth#',
WAIT)
# if res == vrep.simx_return_ok: # [debug]
# print("vrep.simxGetDistanceHandle executed fine")
# collision object
res, collisionID = vrep.simxGetCollisionHandle(clientID, "Collision#",
BLOCKING)
# start up devices
# wheels
vrep.simxSetJointTargetVelocity(clientID, left_motorID, 0, STREAMING)
vrep.simxSetJointTargetVelocity(clientID, right_motorID, 0, STREAMING)
# pose
vrep.simxGetObjectPosition(clientID, robotID, -1, MODE_INI)
vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE_INI)
# collision
vrep.simxReadCollision(clientID, collisionID, STREAMING)
# kinect
vrep.simxGetVisionSensorDepthBuffer(clientID, kinect_depthID, STREAMING)
return
def getCollisonStates(self):
_, collision1 = vrep.simxReadCollision(self.clientID,
self.robot1_collisionHandle,
vrep.simx_opmode_blocking)
_, collision2 = vrep.simxReadCollision(self.clientID,
self.robot2_collisionHandle,
vrep.simx_opmode_blocking)
return collision1, collision2
def getCollisionState(self, initialize=False):
if initialize:
returnCode, state = vrep.simxReadCollision(
self.clientID, self.collision_handle,
vrep.simx_opmode_streaming)
returnCode, state = vrep.simxReadCollision(self.clientID,
self.collision_handle,
vrep.simx_opmode_buffer)
return state
def if_collision():
""" judge if collision happens"""
res, csl = vrep.simxReadCollision(clientID, left_collisionID, BUFFER)
res, csr = vrep.simxReadCollision(clientID, right_collisionID, BUFFER)
collision = 0
if csl == 1:
print("Collision with left wall!")
collision = 1
if csr == 1:
print("Collision with right wall!")
collision = 1
return collision
def setup_devices():
""" Assign the devices from the simulator to specific IDs """
global robotID, left_motorID, right_motorID, ultraID, rewardRefID, goalID, wall0_collisionID, wall1_collisionID, wall2_collisionID, wall3_collisionID, target_collisionID
# res: result (1(OK), -1(error), 0(not called))
# robot
res, robotID = vrep.simxGetObjectHandle(clientID, 'robot#', WAIT)
# motors
res, left_motorID = vrep.simxGetObjectHandle(clientID, 'leftMotor#', WAIT)
res, right_motorID = vrep.simxGetObjectHandle(clientID, 'rightMotor#',
WAIT)
# ultrasonic sensors
for idx, item in enumerate(config.ultra_distribution):
res, ultraID[idx] = vrep.simxGetObjectHandle(clientID, item, WAIT)
# reward reference distance object
res, rewardRefID = vrep.simxGetDistanceHandle(clientID, 'Distance#', WAIT)
# if res == vrep.simx_return_ok: # [debug]
# print("vrep.simxGetDistanceHandle executed fine")
# goal reference object
res, goalID = vrep.simxGetObjectHandle(clientID, 'Dummy#', WAIT)
# collision object
res, target_collisionID = vrep.simxGetCollisionHandle(
clientID, "targetCollision#", BLOCKING)
res, wall0_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall0#", BLOCKING)
res, wall1_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall1#", BLOCKING)
res, wall2_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall2#", BLOCKING)
res, wall3_collisionID = vrep.simxGetCollisionHandle(
clientID, "wall3#", BLOCKING)
# start up devices
# wheels
vrep.simxSetJointTargetVelocity(clientID, left_motorID, 0, STREAMING)
vrep.simxSetJointTargetVelocity(clientID, right_motorID, 0, STREAMING)
# pose
vrep.simxGetObjectPosition(clientID, robotID, -1, MODE_INI)
vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE_INI)
# reading-related function initialization according to the recommended operationMode
for i in ultraID:
vrep.simxReadProximitySensor(clientID, i, STREAMING)
vrep.simxReadDistance(clientID, rewardRefID, STREAMING)
vrep.simxReadCollision(clientID, wall0_collisionID, STREAMING)
vrep.simxReadCollision(clientID, wall1_collisionID, STREAMING)
vrep.simxReadCollision(clientID, wall2_collisionID, STREAMING)
vrep.simxReadCollision(clientID, wall3_collisionID, STREAMING)
vrep.simxReadCollision(clientID, target_collisionID, STREAMING)
def _start_streaming(self):
print 'Beginning to stream all collisions.'
for collision in COLLISION_OBJECTS:
collision_handle = self._scene_handles[collision]
code = vrep.simxReadCollision(self._client_id, collision_handle,
vrep.simx_opmode_streaming)[0]
if code != vrep.simx_return_novalue_flag:
raise SimulatorException(
'Failed to start streaming for collision object {}'.format(
collision))
print 'Beginning to stream data for all proximity sensors.'
for sensor in PROXIMITY_SENSORS:
sensor_handle = self._scene_handles[sensor]
code = vrep.simxReadProximitySensor(self._client_id, sensor_handle,
vrep.simx_opmode_streaming)[0]
if code != vrep.simx_return_novalue_flag:
raise SimulatorException(
'Failed to start streaming for proximity sensor {}'.format(
sensor))
print 'Beginning to stream data for robot tip.'
code = vrep.simxGetObjectPosition(self._client_id,
self._scene_handles[TIP_OBJECT], -1,
vrep.simx_opmode_streaming)[0]
if code != vrep.simx_return_novalue_flag:
raise SimulatorException('Failed to start streaming for robot tip')
def collision_read(self):
assert self.clientID != -1, 'Failed to connect to the V-rep server'
for i in range(124):
err, collision = vrep.simxReadCollision(self.clientID,
self.handle_collision[i],
vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
#print('collision information imported')
if (collision == True and i < 92):
#print('Tray collision or Object collision detected')
return 1 # tray collision or object collision detected => break
elif (collision == True and i > 91):
#print('Self collision detected')
return 2 # self collision detected => negative reward
else:
break
elif err == vrep.simx_return_novalue_flag:
print("no collision information")
else:
print('Error')
print(err)
#print('No Collision')
return 0 # no collision
def collsion_Read():
if (not clientID):
print('connect first')
elif (clientID[0] == -1):
print('connect first')
else:
for i in collision_handle:
check = []
while (vrep.simxGetConnectionId(clientID[0]) != -1):
err, collision = vrep.simxReadCollision(
clientID[0], i, vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
print('collision information imported')
del check[:]
check.append(collision)
break
elif err == vrep.simx_return_novalue_flag:
print("no collision information")
else:
print('Error')
print(err)
if (check[0] == 1):
print('Collision detected')
return 1
print('No Collision')
return 0
def check_for_collisions(self):
"""Checks for collision among all the collision objects."""
for collision_handler in self.collision_handlers:
res, tmp = vrep.simxReadCollision(self.client_id, collision_handler, vrep.simx_opmode_oneshot_wait)
if tmp == True:
return True
return False
def read_collision(self, collision_handle, mode):
"""
Reads a collision of a collision object
The collision interaction has to be created manually in the VREP scene editor
Returns a boolean value of whether collision is present
"""
collision_status = vrep.simxReadCollision(self.client_id, collision_handle, self.modes[mode])[1]
return collision_status
def __init__(self,clientID):
motors =['motor1','motor2','motor3','motor4','motor5','motor6','motor7']
self.motor_handles = [vrep.simxGetObjectHandle(clientID,name, vrep.simx_opmode_blocking)[1] for name in motors]
sensors = ['head_sensor','sensor1r','sensor1l','sensor2r','sensor2l','sensor3r','sensor3l','sensor4r','sensor4l', 'sensor5r','sensor5l','sensor6r','sensor6l','sensor7r','sensor7l','sensor8r','sensor8l']
self.sensor_handles = [vrep.simxGetObjectHandle(clientID, sensor, vrep.simx_opmode_blocking)[1] for sensor in sensors]
obstacles = ['Obj0','Obj1','Obj2','Obj3','Obj4','Obj5','Obj6','Obj7','Obj8','Obj9','Obj10']
self.obst_handles = [vrep.simxGetObjectHandle(clientID, obj, vrep.simx_opmode_blocking)[1] for obj in obstacles]
body_modules = ['snake_v2', 'module2', 'module3', 'module4', 'module5', 'module6', 'module7', 'module8']
self.body_module_handles = [vrep.simxGetObjectHandle(clientID, obj, vrep.simx_opmode_blocking)[1] for obj in body_modules]
self.snake_body = vrep.simxGetObjectHandle(clientID, 'snake_v2', vrep.simx_opmode_blocking)[1]
print ("\nSet motor torques...\n")
for motor_handle in self.motor_handles:
_=vrep.simxSetJointForce(clientID,motor_handle,5,vrep.simx_opmode_blocking)
if _ !=0: raise Exception()
self.setMotor(clientID, 0, 1)
print ("\nAssign sensor values\n")
for sensor_handle in self.sensor_handles:
_,_,_,_,_ = vrep.simxReadProximitySensor(clientID, sensor_handle, vrep.simx_opmode_streaming)
print ("\nSetting Object postions...\n")
self.obst_count = r.randint(5,8)
for i in range (self.obst_count):
obst = r.choice(self.obst_handles)
_, [obst_copy] = vrep.simxCopyPasteObjects(clientID, [obst], vrep.simx_opmode_blocking)
x = round(r.uniform(-5,8),3)
y = round(r.uniform(-7.5,8),3)
_ = vrep.simxSetObjectPosition(clientID, obst_copy, -1, (x, y, 0.25),vrep.simx_opmode_oneshot)
_ = vrep.simxSetObjectOrientation(clientID, obst_copy, obst_copy, (r.randint(0,180),0,0), vrep.simx_opmode_oneshot)
#_ = vrep.simxSetObjectPosition(clientID, self.snake_body, -1, (-9.5, 10, 0.085), vrep.simx_opmode_oneshot)
#---------------------------------------------Initialising the motor position buffer
self.modulepos = list()
for motor_handle in self.motor_handles:
_,moduleCord = vrep.simxGetObjectPosition(clientID,motor_handle,-1,vrep.simx_opmode_streaming)
self.modulepos.append(moduleCord)
if _ !=1 :
print (_)
raise Exception()
#---------------------------------------------Initialising body_modules collision buffer
for module in self.body_module_handles:
_,collision_state = vrep.simxReadCollision(clientID, module, vrep.simx_opmode_streaming)
print _
def is_collided_with_wall():
""" judge if collision happens"""
global clientID, wall0_collisionID, wall1_collisionID, wall2_collisionID, wall3_collisionID
for id in [
wall0_collisionID, wall1_collisionID, wall2_collisionID,
wall3_collisionID
]:
res, status = vrep.simxReadCollision(clientID, id, BUFFER)
if status:
print("Collision has been detected!")
return True
return False
def _data_streaming(self, option=True):
assert self.clientID != -1, 'Failed to connect to the V-rep server'
if option:
op = vrep.simx_opmode_streaming
else:
op = vrep.simx_opmode_discontinue
#Object postion
for i in self.handle_obj:
vrep.simxGetObjectPosition(self.clientID, i, self.handle_robot, op)
#Dummy position
vrep.simxGetObjectPosition(self.clientID, self.handle_end,
self.handle_robot, op)
#vision
for i in self.handle_cam:
vrep.simxGetVisionSensorImage(self.clientID, i, 0, op)
#collision
for i in self.handle_collision:
vrep.simxReadCollision(self.clientID, i, op)
#UR3 Joint
for i in self.handle_joint:
vrep.simxGetJointPosition(self.clientID, i, op)
def _check_for_collisions(self):
self._is_colliding = False
for collision_object_name in COLLISION_OBJECTS:
collision_handle = self._scene_handles[collision_object_name]
code, state = vrep.simxReadCollision(self._client_id,
collision_handle,
vrep.simx_opmode_buffer)
if code == vrep.simx_return_ok and state:
print 'COLLISION DETECTED WITH {}'.format(
collision_object_name)
self._is_colliding = True
return
def is_collided_with_target():
global clientID, target_collisionID
res, flag = vrep.simxReadCollision(clientID, target_collisionID, BUFFER)
return flag
def learning():
frames = 100000
observation = int(frames/100)
instances = 5
epsilon = 1
batchSize = 60
buffer = 500
replay = []
gamma = 0.9
bufferCount = 0
sensorInformation = np.array([])
#capturing the current state
returnCode,collisionState=vrep.simxReadCollision(clientID,cH,vrep.simx_opmode_streaming)
for x in range(1,16+1):
errorCodeProximity3,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_h[x-1],vrep.simx_opmode_streaming)
for x in range(1,16+1):
errorCodeProximity3,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_h[x-1],vrep.simx_opmode_buffer)
sensorInformation=np.append(sensorInformation,np.linalg.norm(detectedPoint)) #get list of values
#sensorInformation=np.append(sensorInformation,detectedPoint[2])
print(detectedPoint)
state=sensorInformation
t = time.time()
# no iter
for i in range(frames):
noOfCollisions=0
#sensorInformation = np.array([])
#check for randomness or for intially observation before training begins
if (random.random() < epsilon) or (i < observation):
action = random.randint(0,4)
else:
qVal = model.predict(state)
action = np.argmax(qVal)
new_state = makeMove(state,action)
reward = rewardFunction(state,action)
replay.append((state,action,reward,new_state))
if i > observation:
if len(replay) > buffer:
replay.pop(0)
#!!!!!check for batchSize, it may not be correct
minibatch = random.sample(replay,batchSize)
Xtrain , ytrain = mini_batch_processing(minibatch,model)
#==============================================================================
# for j in range(batchSize):
# print("j is ",j)
#
#==============================================================================
for i in range(batchSize):
model.fit(Xtrain[i], ytrain[i], batch_size = 1, epochs = 1 , verbose = 1 )
state = new_state
if epsilon > 0.1 and i > observation:
epsilon -= (1/frames)
bufferCount+=1
print("\n\n\n\n\nGame no: " ,bufferCount)
print(state)
k=""
with open('stateFile.txt', 'a') as the_file:
for i in range(len(state)):
k=k+","+str(state[i])
the_file.write(k+"\n")
#write to file
time.sleep(0.02)
returnCode,collisionState=vrep.simxReadCollision(clientID,cH,vrep.simx_opmode_buffer)
print("Collison State: " , collisionState)
#if np.amin(state) >= 0.7 and bufferCount>10:
if collisionState==True:
sys.exit("HERE: ")
vrep.simxRemoveModel(clientID,modelHandle,vrep.simx_opmode_oneshot)
vrep.simxLoadModel(clientID,modelHandle,0,vrep.simx_opmode_blocking)
def eval_robot(robot, chromosome):
# Enable the synchronous mode
vrep.simxSynchronous(settings.CLIENT_ID, True)
if (vrep.simxStartSimulation(settings.CLIENT_ID, settings.OP_MODE) == -1):
print('Failed to start the simulation\n')
print('Program ended\n')
return
robot.chromosome = chromosome
individual = robot
start_position = None
fitness_agg = np.array([])
# collistion detection initialization
errorCode, collision_handle = vrep.simxGetCollisionHandle(
settings.CLIENT_ID, 'robot_collision', vrep.simx_opmode_oneshot_wait)
collision = False
now = datetime.now()
id = uuid.uuid1()
if start_position is None:
start_position = individual.position
distance_acc = 0.0
previous = np.array(start_position)
collisionDetected, collision = vrep.simxReadCollision(
settings.CLIENT_ID, collision_handle, vrep.simx_opmode_streaming)
if settings.DEBUG: individual.logger.info('Chromosome {}'.format(individual.chromosome))
while not collision and datetime.now() - now < timedelta(seconds=settings.RUNTIME):
# The first simulation step waits for a trigger before being executed
# start_time = time.time()
vrep.simxSynchronousTrigger(settings.CLIENT_ID)
collisionDetected, collision = vrep.simxReadCollision(
settings.CLIENT_ID, collision_handle, vrep.simx_opmode_buffer)
individual.loop()
# # Traveled distance calculation
# current = np.array(individual.position)
# distance = math.sqrt(((current[0] - previous[0])**2) + ((current[1] - previous[1])**2))
# distance_acc += distance
# previous = current
# After this call, the first simulation step is finished
vrep.simxGetPingTime(settings.CLIENT_ID)
# Fitness function; each feature;
# V - wheel center
V = f_wheel_center(individual.norm_wheel_speeds[0], individual.norm_wheel_speeds[1])
if settings.DEBUG: individual.logger.info('f_wheel_center {}'.format(V))
# pleasure - straight movements
pleasure = f_straight_movements(individual.norm_wheel_speeds[0], individual.norm_wheel_speeds[1])
if settings.DEBUG: individual.logger.info('f_straight_movements {}'.format(pleasure))
# pain - closer to an obstacle more pain
pain = f_pain(individual.sensor_activation)
if settings.DEBUG: individual.logger.info('f_pain {}'.format(pain))
# fitness_t at time stamp
fitness_t = V * pleasure * pain
fitness_agg = np.append(fitness_agg, fitness_t)
# elapsed_time = time.time() - start_time
# dump individuals data
if settings.DEBUG:
with open(settings.PATH_EA + str(id) + '_fitness.txt', 'a') as f:
f.write('{0!s},{1},{2},{3},{4},{5},{6},{7},{8}, {9}\n'.format(id, individual.wheel_speeds[0],
individual.wheel_speeds[1], individual.norm_wheel_speeds[0], individual.norm_wheel_speeds[1], V, pleasure, pain, fitness_t, distance_acc))
# aggregate fitness function
# fitness_aff = [distance_acc]
# behavarioral fitness function
fitness_bff = [np.sum(fitness_agg)]
# tailored fitness function
fitness = fitness_bff[0] # * fitness_aff[0]
# Now send some data to V-REP in a non-blocking fashion:
vrep.simxAddStatusbarMessage(settings.CLIENT_ID, 'fitness: {}'.format(fitness), vrep.simx_opmode_oneshot)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(settings.CLIENT_ID)
print('%s fitness: %f | fitness_bff %f | fitness_aff %f' % (str(id), fitness, fitness_bff[0], 0.0)) # , fitness_aff[0]))
# save individual as object
if settings.DEBUG:
if fitness > 30:
individual.save_robot(settings.PATH_EA + str(id) + '_robot')
if (vrep.simxStopSimulation(settings.CLIENT_ID, settings.OP_MODE) == -1):
print('Failed to stop the simulation\n')
print('Program ended\n')
return
time.sleep(1)
return [fitness]
#errorCode,linearVelocity,angularVelocity=vrep.simxGetObjectVelocity(clientID,cam_handle,vrep.simx_opmode_buffer)
#print(linearVelocity)
weightReadings = [1, 1, 1]
reward = np.dot(train,weightReadings)
reward = reward.astype(int)
state = np.array([train])
state = state.astype(int)
loss_log = []
replay = []
data_collect = []
maxroute = 0
driveCount = 0
trainCount = 0
errorCode, collisionState1=vrep.simxReadCollision(clientID,collision_handle1,vrep.simx_opmode_streaming)
while trainCount < train_frames:
trainCount += 1
print(trainCount)
#Collision handler
errorCode, collisionState1=vrep.simxReadCollision(clientID,collision_handle1,vrep.simx_opmode_buffer)
print(collisionState1)
print(maxroute)
if collisionState1 == 1 or min(state[0]) <=1:
if maxroute < driveCount and trainCount > observe:
maxroute = driveCount
# Log the car's distance at this T.
data_collect.append([trainCount, maxroute])
jointConfig1 = np.zeros((jointNum, ))
jointConfig2 = np.zeros((jointNum, ))
# for i in range(jointNum):
# _, jpos = vrep.simxGetJointPosition(clientID, robot1_jointHandle[i],vrep.simx_opmode_blocking)
# jointConfig1[i] = jpos
# print(jointConfig1*180./np.pi)
# for i in range(jointNum):
# _, jpos = vrep.simxGetJointPosition(clientID, robot2_jointHandle[i],vrep.simx_opmode_blocking)
# jointConfig2[i] = jpos
_, pos = vrep.simxGetObjectPosition(clientID, ballHandle, base1Handle,
vrep.simx_opmode_blocking)
print(pos)
_, pos1 = vrep.simxGetObjectPosition(clientID, ball0Handle, -1,
vrep.simx_opmode_blocking)
_, collision = vrep.simxReadCollision(clientID, collisionHandle,
vrep.simx_opmode_blocking)
# for i in range(0):
# pos[0]+=0.01
# pos1[0]-=0.01
# _ = vrep.simxSetObjectPosition(clientID,ballHandle,-1,pos,vrep.simx_opmode_oneshot)
# _ = vrep.simxSetObjectPosition(clientID, ball0Handle, -1, pos1, vrep.simx_opmode_oneshot)
# for i in range(jointNum):
# _, jpos = vrep.simxGetJointPosition(clientID, robot1_jointHandle[i], vrep.simx_opmode_blocking)
# jointConfig1[i] = jpos
# _, collision = vrep.simxReadCollision(clientID, collisionHandle, vrep.simx_opmode_blocking)
# print(collision,jointConfig1*180./np.pi)
# #get simulation time
# while vrep.simxGetConnectionId(clientID)!=-1:
# currCmdTime = vrep.simxGetLastCmdTime(clientID)
if clientID != -1: #check if client connection successful
print('Connected to remote API server')
else:
print('Connection not successful')
sys.exit('Could not connect')
errorCode, left_motor_handle = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
errorCode, right_motor_handle = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
errorCode, robot = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx',
vrep.simx_opmode_oneshot_wait)
code, collision = vrep.simxReadCollision(clientID, robot,
vrep.simx_opmode_streaming)
# Preallocaation
sensor_h = [] #empty list for handles
sensor_val = np.array([]) #empty array for sensor measurements
#orientation of all the sensors:
sensor_loc = np.array([
-PI / 2, -50 / 180.0 * PI, -30 / 180.0 * PI, -10 / 180.0 * PI,
10 / 180.0 * PI, 30 / 180.0 * PI, 50 / 180.0 * PI, PI / 2, PI / 2,
130 / 180.0 * PI, 150 / 180.0 * PI, 170 / 180.0 * PI, -170 / 180.0 * PI,
-150 / 180.0 * PI, -130 / 180.0 * PI, -PI / 2
])
errorCode = vrep.simxSetJointTargetVelocity(clientID, left_motor_handle, 0.2,
vrep.simx_opmode_streaming)
def __init__(self):
#建立通信
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
5000, 5)
if self.clientID != -1:
break
else:
time.sleep(0.1)
print("Failed connecting to remote API server!")
print("Connection success!")
# # 设置机械臂仿真步长，为了保持API端与V-rep端相同步长
vrep.simxSetFloatingParameter(self.clientID, vrep.sim_floatparam_simulation_time_step, self.dt,\
vrep.simx_opmode_oneshot)
# # 然后打开同步模式
vrep.simxSynchronous(self.clientID, False)
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot)
# vrep.simxSynchronousTrigger(self.clientID)
#获取 joint 句柄
self.robot1_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
self.robot2_jointHandle = np.zeros((self.jointNum, ),
dtype=np.int) # joint 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_jointHandle[i] = returnHandle
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.jointName + str(i + 4),
vrep.simx_opmode_blocking)
self.robot2_jointHandle[i] = returnHandle
# 获取 Link 句柄
self.robot1_linkHandle = np.zeros((self.jointNum, ),
dtype=np.int) # link 句柄
self.robot2_linkHandle = np.zeros((self.jointNum, ),
dtype=np.int) # link 句柄
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 1),
vrep.simx_opmode_blocking)
self.robot1_linkHandle[i] = returnHandle
for i in range(self.jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
self.clientID, self.linkName + str(i + 4),
vrep.simx_opmode_blocking)
self.robot2_linkHandle[i] = returnHandle
# 获取碰撞句柄
_, self.robot1_collisionHandle = vrep.simxGetCollisionHandle(
self.clientID, 'Collision_robot1', vrep.simx_opmode_blocking)
_, self.robot2_collisionHandle = vrep.simxGetCollisionHandle(
self.clientID, 'Collision_robot2', vrep.simx_opmode_blocking)
# 获取距离句柄
# _,self.mindist_robot1_Handle = vrep.simxGetDistanceHandle(self.clientID,'dis_robot1',vrep.simx_opmode_blocking)
# _,self.mindist_robot2_Handle = vrep.simxGetDistanceHandle(self.clientID,'dis_robot2', vrep.simx_opmode_blocking)
# _,self.mindist_robots_Handle = vrep.simxGetDistanceHandle(self.clientID,'robots_dist', vrep.simx_opmode_blocking)
_, self.robot1_goal_Handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot1_goal', vrep.simx_opmode_blocking)
_, self.robot2_goal_Handle = vrep.simxGetDistanceHandle(
self.clientID, 'robot2_goal', vrep.simx_opmode_blocking)
# 获取末端句柄
_, self.end1_Handle = vrep.simxGetObjectHandle(
self.clientID, 'end', vrep.simx_opmode_blocking)
_, self.end2_Handle = vrep.simxGetObjectHandle(
self.clientID, 'end0', vrep.simx_opmode_blocking)
_, self.goal1_Handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_1', vrep.simx_opmode_blocking)
_, self.goal2_Handle = vrep.simxGetObjectHandle(
self.clientID, 'goal_2', vrep.simx_opmode_blocking)
print('Handles available!')
_, self.goal_1 = vrep.simxGetObjectPosition(self.clientID,
self.goal1_Handle, -1,
vrep.simx_opmode_blocking)
_, self.goal_2 = vrep.simxGetObjectPosition(self.clientID,
self.goal2_Handle, -1,
vrep.simx_opmode_blocking)
del self.goal_1[2]
del self.goal_2[2]
self.goal_1 = np.array(self.goal_1)
self.goal_2 = np.array(self.goal_2)
self.jointConfig1 = np.zeros((self.jointNum, ))
self.jointConfig2 = np.zeros((self.jointNum, ))
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot1_jointHandle[i],
vrep.simx_opmode_blocking)
self.jointConfig1[i] = jpos
for i in range(self.jointNum):
_, jpos = vrep.simxGetJointPosition(self.clientID,
self.robot2_jointHandle[i],
vrep.simx_opmode_blocking)
self.jointConfig2[i] = jpos
_, collision1 = vrep.simxReadCollision(self.clientID,
self.robot1_collisionHandle,
vrep.simx_opmode_blocking)
_, collision2 = vrep.simxReadCollision(self.clientID,
self.robot2_collisionHandle,
vrep.simx_opmode_blocking)
_, pos1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle,
-1, vrep.simx_opmode_blocking)
_, pos2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle,
-1, vrep.simx_opmode_blocking)
_, d1 = vrep.simxReadDistance(self.clientID, self.robot1_goal_Handle,
vrep.simx_opmode_blocking)
_, d2 = vrep.simxReadDistance(self.clientID, self.robot2_goal_Handle,
vrep.simx_opmode_blocking)
for i in range(self.jointNum):
_, returnpos = vrep.simxGetObjectPosition(
self.clientID, self.robot1_linkHandle[i], -1,
vrep.simx_opmode_blocking)
for i in range(self.jointNum):
_, returnpos = vrep.simxGetObjectPosition(
self.clientID, self.robot2_linkHandle[i], -1,
vrep.simx_opmode_blocking)
print('programing start!')
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
def detectar_colision():
errorCode, value = vrep.simxReadCollision(clientID, collision,
vrep.simx_opmode_oneshot_wait)
#print("--------------->Colision", value)
if value == True: return 1
else: return 0
def evaluation_and_stat(weights, conf):
'''
Assume time is fixed : t = 5s
sampling_rate = how many samples per second is sent to simulator
redundant_simulation_step = how many additional simulation steps are necessary for reaching stable system state.
'''
spillage_distance_threshold = 0.11
redundant_simulation_step = round(1.25/conf['dt']) # 1.25s
linear_mode = False
worst_cost_function_value = 180 * 1.2 + 2 * float(len(conf['particle_handles'])) * (80/20)# as collision is one time such cost
collision = False
add_noise = True
if linear_mode == True:
total_time = 1.
sampling_rate = 1./conf['dt']
def sample_points_linear(sampling_rate, current_time_step, weights):
"""
weights = [a1,...a7]
"""
current_joint_angles = np.zeros(len(weights))
for i in range(len(weights)):
if i == 0:
current_joint_angles[i] = weights[i] * current_time_step / sampling_rate + ms.pi/2
elif i == 1:
current_joint_angles[i] = weights[i] * current_time_step / sampling_rate - ms.pi/2
else:
current_joint_angles[i] = weights[i] * current_time_step / sampling_rate
return current_joint_angles
for t in range(round(total_time * sampling_rate)):
theta = sample_points_linear(sampling_rate, t, weights)
for i in range(len(conf['joint_handles'])):
returnCode = vrep.simxSetJointTargetPosition(conf['clientID'], conf['joint_handles'][i], theta[i], vrep.simx_opmode_streaming) #Original: vrep.simx_opmode_blocking
vrep.simxSynchronousTrigger(conf['clientID'])
if returnCode != 0:
print('Can not set joint target positions!')
constraint_satisfied = True
else:# -----------------------------------------------------------------DMP mode-------------------------------------------------------------
joint_angle_traj, joint_vel_traj, joint_acc_traj, angle_to_vertical_axis, step_spillage = [] , [] , [] , [] , []
# end_effector_position, end_effector_euler_angles = [] , []
n_dmps = 1
rollout_timesteps = conf['dmp_trajectory_timesteps']# Originally, 5 seconds, now even different among different joint angles.
for i in range(len(conf['joint_handles'])):
##Allowing alpha and beta learning
if conf['enable_beta'] == True:
alpha_rescaled, beta_rescaled = alpha_beta_rescaling(weights[i][-2], weights[i][-1], conf)
else:
alpha_rescaled, beta_rescaled = alpha_beta_rescaling(weights[i][-1], weights[i][-2], conf)
# Goal position allowed
dmp = DMPs_discrete(dt=conf['dt_dmp'], n_dmps=n_dmps, n_bfs=conf['n_bfs'], w=np.expand_dims(weights[i][:-2], axis=0), goal=weights[i][-2]+conf['initial_joint_angles'][i]*180 /ms.pi, ay=alpha_rescaled*np.ones(n_dmps), by=beta_rescaled*np.ones(n_dmps), y0=conf['initial_joint_angles'][i]*180 /ms.pi) # weight as parameters, goal as known
# Goal position fixed
# dmp = DMPs_discrete(dt=conf['dt_dmp'], n_dmps=n_dmps, n_bfs=conf['n_bfs'], w=np.exp(np.expand_dims(weights[i][:-1], axis=0)), goal=conf['initial_joint_angles'][i]*180 /ms.pi, ay=alpha_rescaled*np.ones(n_dmps), by=beta_rescaled*np.ones(n_dmps), y0=conf['initial_joint_angles'][i]*180 /ms.pi) # weight as parameters, goal as known
# # rescale the runtime
# if conf['enable_beta'] == False:
# dmp.cs.run_time = runtime_rescaling(weights[i][-1], conf)
y_track, dy_track, ddy_track = dmp.rollout(timesteps = rollout_timesteps, tau = 1.0) # Makes sure that the trajectory lengths are the same. #dmp.rollout(timesteps = total_time_for_dyms)
# Post-process the y_track and dy_track
y_track = y_track[0::round(conf['dt']/conf['dt_dmp'])]
dy_track = np.mean(dy_track.reshape(-1, round(conf['dt']/conf['dt_dmp'])), axis=1)
ddy_track = np.mean(ddy_track.reshape(-1, round(conf['dt']/conf['dt_dmp'])), axis=1)
if add_noise == True:
y_track += 0.2 * np.random.randn(*y_track.shape)
# dy_track += 0.2 * np.random.randn(*dy_track.shape)
joint_angle_traj.append(y_track)
joint_vel_traj.append(dy_track)
joint_acc_traj.append(ddy_track)
# ----------------------------------------Check boundaries is fulfilled or not---------------------------------------------
constraint_satisfied, constraint_punishment = DMP_trajectory_constraint_check(conf, joint_angle_traj, joint_vel_traj, joint_acc_traj)
# -----------------------------------------------------------Start simulation------------------------------------------------------
if constraint_satisfied == True:
for t in range(len(y_track)):
vrep.simxPauseCommunication(conf['clientID'],True)
for i in range(len(conf['joint_handles'])):
returnCode = vrep.simxSetJointTargetPosition(conf['clientID'], conf['joint_handles'][i], joint_angle_traj[i][t]/180.*ms.pi, vrep.simx_opmode_streaming) #Original: vrep.simx_opmode_blocking
if returnCode != 0 :
print('Can not set joint target positions!' + str(returnCode))
vrep.simxPauseCommunication(conf['clientID'],False)
vrep.simxSynchronousTrigger(conf['clientID'])
if t == 0:
vrep.simxGetObjectOrientation(conf['clientID'], conf['cup_handle'], -1, vrep.simx_opmode_streaming)
startTime=time.time()
while time.time()-startTime < 5:
returnCode, cup_eulerAngles = vrep.simxGetObjectOrientation(conf['clientID'], conf['cup_handle'], -1, vrep.simx_opmode_buffer)
if returnCode==vrep.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code
break
time.sleep(0.01)
else:
returnCode, cup_eulerAngles = vrep.simxGetObjectOrientation(conf['clientID'], conf['cup_handle'], -1, vrep.simx_opmode_buffer)
if returnCode != 0:
print('Can not get cup orientation!' + str(returnCode))
R = vrepEulerRotation(cup_eulerAngles)
cup_directional_vector = np.matmul(R, np.array([0,0,1])) # cup_directional_vector w.r.t. global reference frame
# Compute angle between 2 vectors
# https://stackoverflow.com/questions/2827393/angles-between-two-n-dimensional-vectors-in-python/13849249#13849249
c = np.dot(cup_directional_vector,np.array([0, 0, -1]))/np.linalg.norm(cup_directional_vector)
angle_to_vertical_axis.append(np.arccos(np.clip(c, -1, 1)) * 180 / ms.pi )
#-------------------------------------------------------Collision Check -------------------------------------
for i in range(len(conf['collision_handle'])):
if t == 0:
vrep.simxReadCollision(conf['clientID'], conf['collision_handle'][i], vrep.simx_opmode_streaming)
startTime=time.time()
while time.time()-startTime < 5:
returnCode,collision_temp=vrep.simxReadCollision(conf['clientID'], conf['collision_handle'][i], vrep.simx_opmode_buffer)
if returnCode==vrep.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code
break
time.sleep(0.01)
else:
returnCode,collision_temp=vrep.simxReadCollision(conf['clientID'], conf['collision_handle'][i], vrep.simx_opmode_buffer)
# returnCode, collision_temp = vrep.simxReadCollision(conf['clientID'], conf['collision_handle'][i], vrep.simx_opmode_buffer)
collision = collision or collision_temp
if returnCode != vrep.simx_return_ok:
print('Can not read collsion state!' + str(returnCode))
step_spillage_cumulative, end_effector_pose, end_effector_euler_angle = step_check_routine(conf['clientID'], conf['cup_handle'], conf['end_effector_handle'], conf['particle_handles'], conf['distance_threshold'])
step_spillage.append(step_spillage_cumulative)
# if collision == True:
# break
if (constraint_satisfied == True): #and (min(angle_to_vertical_axis) < 90):
returnCode,Target_Position = vrep.simxGetObjectPosition(conf['clientID'],conf['cup_handle'],-1,vrep.simx_opmode_buffer)
if returnCode != 0:
print('Can not get Target position!')
if collision == False:
for j in range(redundant_simulation_step):
for i in range(len(conf['joint_handles'])):
returnCode = vrep.simxSetJointTargetPosition(conf['clientID'], conf['joint_handles'][i], joint_angle_traj[i][-1]/180.*ms.pi, vrep.simx_opmode_streaming) #Original: vrep.simx_opmode_blocking
vrep.simxSynchronousTrigger(conf['clientID'])
total_spilled_amount = episodic_spillage_detection(conf['clientID'], conf['cup_handle'], conf['particle_handles'], spillage_distance_threshold)
# e = np.linalg.norm((conf['p_d'] - Target_Position), ord=2) # no longer needed for turnover
e = total_spilled_amount * (80/20) + min(angle_to_vertical_axis) + 0.2 * (180 - angle_to_vertical_axis[-1])
if collision == True:
print('colliding')
e = float(collision) * (80 * 1.2 + float(len(conf['particle_handles'])) * 80 /20) + total_spilled_amount * (80/20)
else:
e = worst_cost_function_value + constraint_punishment
record_step_spillage = constraint_satisfied and (not collision) # If one of the criterion is true, then don't record step spillage
record_episodic_spillage = constraint_satisfied
if record_step_spillage == True:
# Process the cumulative information to increamental information
step_spillage_tmp = step_spillage.copy()
step_spillage.append(step_spillage_cumulative) #
step_spillage_tmp.insert(0, step_spillage[0])
step_spillage_tmp = np.array(step_spillage_tmp)
step_spillage = np.array(step_spillage)
step_spillage = step_spillage - step_spillage_tmp
else:
step_spillage = 25 * np.ones(rollout_timesteps)
if record_episodic_spillage == True:
episodic_spillage = total_spilled_amount
max_cup_rotation_angle = 180 - min(angle_to_vertical_axis)
final_cup_resting_angle = 180 - angle_to_vertical_axis[-1]
else:
episodic_spillage = 25
max_cup_rotation_angle = 200
final_cup_resting_angle = -25
return e, record_episodic_spillage, episodic_spillage, record_step_spillage, step_spillage, max_cup_rotation_angle, final_cup_resting_angle
def eval_genomes(robot, genomes, config):
for genome_id, genome in genomes:
# Enable the synchronous mode
vrep.simxSynchronous(settings.CLIENT_ID, True)
if (vrep.simxStartSimulation(settings.CLIENT_ID,
vrep.simx_opmode_oneshot) == -1):
print('Failed to start the simulation\n')
print('Program ended\n')
return
robot.chromosome = genome
robot.wheel_speeds = np.array([])
robot.sensor_activation = np.array([])
robot.norm_wheel_speeds = np.array([])
individual = robot
start_position = None
# collistion detection initialization
errorCode, collision_handle = vrep.simxGetCollisionHandle(
settings.CLIENT_ID, 'robot_collision', vrep.simx_opmode_blocking)
collision = False
first_collision_check = True
now = datetime.now()
fitness_agg = np.array([])
scaled_output = np.array([])
net = neat.nn.FeedForwardNetwork.create(genome, config)
id = uuid.uuid1()
if start_position is None:
start_position = individual.position
distance_acc = 0.0
previous = np.array(start_position)
collisionDetected, collision = vrep.simxReadCollision(
settings.CLIENT_ID, collision_handle, vrep.simx_opmode_streaming)
while not collision and datetime.now() - now < timedelta(
seconds=settings.RUNTIME):
# The first simulation step waits for a trigger before being executed
vrep.simxSynchronousTrigger(settings.CLIENT_ID)
collisionDetected, collision = vrep.simxReadCollision(
settings.CLIENT_ID, collision_handle, vrep.simx_opmode_buffer)
individual.neuro_loop()
# # Traveled distance calculation
# current = np.array(individual.position)
# distance = math.sqrt(((current[0] - previous[0])**2) + ((current[1] - previous[1])**2))
# distance_acc += distance
# previous = current
output = net.activate(individual.sensor_activation)
# normalize motor wheel wheel_speeds [0.0, 2.0] - robot
scaled_output = np.array([scale(xi, 0.0, 2.0) for xi in output])
if settings.DEBUG:
individual.logger.info('Wheels {}'.format(scaled_output))
individual.set_motors(*list(scaled_output))
# After this call, the first simulation step is finished
vrep.simxGetPingTime(settings.CLIENT_ID)
# Fitness function; each feature;
# V - wheel center
V = f_wheel_center(output[0], output[1])
if settings.DEBUG:
individual.logger.info('f_wheel_center {}'.format(V))
# pleasure - straight movements
pleasure = f_straight_movements(output[0], output[1])
if settings.DEBUG:
individual.logger.info(
'f_straight_movements {}'.format(pleasure))
# pain - closer to an obstacle more pain
pain = f_pain(individual.sensor_activation)
if settings.DEBUG: individual.logger.info('f_pain {}'.format(pain))
# fitness_t at time stamp
fitness_t = V * pleasure * pain
fitness_agg = np.append(fitness_agg, fitness_t)
# dump individuals data
if settings.SAVE_DATA:
with open(settings.PATH_NE + str(id) + '_fitness.txt',
'a') as f:
f.write(
'{0!s},{1},{2},{3},{4},{5},{6},{7},{8}, {9}\n'.format(
id, scaled_output[0], scaled_output[1], output[0],
output[1], V, pleasure, pain, fitness_t,
distance_acc))
# errorCode, distance = vrep.simxGetFloatSignal(CLIENT_ID, 'distance', vrep.simx_opmode_blocking)
# aggregate fitness function - traveled distance
# fitness_aff = [distance_acc]
# behavarioral fitness function
fitness_bff = [np.sum(fitness_agg)]
# tailored fitness function
fitness = fitness_bff[0] # * fitness_aff[0]
# Now send some data to V-REP in a non-blocking fashion:
vrep.simxAddStatusbarMessage(settings.CLIENT_ID,
'fitness: {}'.format(fitness),
vrep.simx_opmode_oneshot)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(settings.CLIENT_ID)
print('%s fitness: %f | fitness_bff %f | fitness_aff %f' %
(str(genome_id), fitness, fitness_bff[0],
0.0)) # , fitness_aff[0]))
if (vrep.simxStopSimulation(settings.CLIENT_ID,
settings.OP_MODE) == -1):
print('Failed to stop the simulation\n')
print('Program ended\n')
return
time.sleep(1)
genome.fitness = fitness
def if_collision():
""" judge if collision happens"""
res, collision = vrep.simxReadCollision(clientID, collisionID, BUFFER)
if collision == 1:
print("Collision!")
return collision
opmode=vrep.simx_opmode_blocking
res, cuboid0Handle=vrep.simxGetObjectHandle(clientID,"Cuboid0",vrep.simx_opmode_oneshot_wait)
res, cuboidHash0Handle=vrep.simxGetObjectHandle(clientID,"Cuboid0#0",opmode)
errorCode,cuboid0=vrep.simxGetObjectHandle(clientID,'cuboid0',vrep.simx_opmode_blocking)
returnCode_1,cuboid_position=vrep.simxGetObjectPosition(clientID,cuboid0Handle,-1,vrep.simx_opmode_streaming)
print(res)
print(cuboid_position)
while True:
joint_force1(0.0)
joint_force2(1.6)
joint_force3(0.0)
joint_force4(1.75)
#returnCode=vrep.simxSetJointTargetVelocity(clientID,phantom_gripper_close_joint,0.00,vrep.simx_opmode_oneshot)
#returnCode = vrep.simxSetJointForce(clientID, phantom_gripper_close_joint,5.0, vrep.simx_opmode_oneshot)
#print(force)
operate_gripper(0)
#print(position)
res, cuboid0Handle = vrep.simxGetObjectHandle(clientID, "Cuboid0", vrep.simx_opmode_oneshot_wait)
returnCode_1, cuboid_position = vrep.simxGetObjectPosition(clientID, cuboid0Handle, -1, vrep.simx_opmode_streaming)
#print(cuboid_position)
res, landing_zone = vrep.simxGetObjectHandle(clientID,"landing_zone",vrep.simx_opmode_oneshot_wait)
returnCode_2, landing_zone_position = vrep.simxGetObjectPosition(clientID, landing_zone, -1, vrep.simx_opmode_streaming)
#print("Landing Zone:", landing_zone_position)
returnCode,handle_1=vrep.simxGetCollisionHandle(clientID,"Collision0",vrep.simx_opmode_blocking)
returnCode, collisionState=vrep.simxReadCollision(clientID,handle_1,vrep.simx_opmode_streaming)
print(collisionState)
def perform_collisiondetect(clientID, collision):
res, in_collision = vrep.simxReadCollision(clientID, collision,
vrep.simx_opmode_streaming)
print "Collision Status: " + str(in_collision)
return in_collision
codErro, roboObstDist = vrep.simxGetDistanceHandle(
clientID, 'roboObstDistDetect', operationMode=vrep.simx_opmode_blocking)
codErro, roboTargetDist = vrep.simxGetDistanceHandle(
clientID, 'roboTargetDistDetect', operationMode=vrep.simx_opmode_blocking)
codErro, obstaculos = vrep.simxGetCollectionHandle(
clientID, 'obstaculos#', operationMode=vrep.simx_opmode_oneshot)
codErro, robo = vrep.simxGetObjectHandle(
clientID, 'Pionee_p3dx', operationMode=vrep.simx_opmode_oneshot)
codErro, motorE = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_blocking)
codErro, motorD = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_blocking)
codErro, colisao = vrep.simxReadCollision(
clientID, pioneerColDetect, operationMode=vrep.simx_opmode_streaming)
posXR = vrep.simxGetFloatSignal(clientID, 'posXR', vrep.simx_opmode_streaming)
posYR = vrep.simxGetFloatSignal(clientID, 'posYR', vrep.simx_opmode_streaming)
posX1 = vrep.simxGetFloatSignal(clientID, 'posX1', vrep.simx_opmode_streaming)
posY1 = vrep.simxGetFloatSignal(clientID, 'posY1', vrep.simx_opmode_streaming)
posX2 = vrep.simxGetFloatSignal(clientID, 'posX2', vrep.simx_opmode_streaming)
posY2 = vrep.simxGetFloatSignal(clientID, 'posY2', vrep.simx_opmode_streaming)
posX3 = vrep.simxGetFloatSignal(clientID, 'posX3', vrep.simx_opmode_streaming)
posY3 = vrep.simxGetFloatSignal(clientID, 'posY3', vrep.simx_opmode_streaming)
posX4 = vrep.simxGetFloatSignal(clientID, 'posX4', vrep.simx_opmode_streaming)
posY4 = vrep.simxGetFloatSignal(clientID, 'posY4', vrep.simx_opmode_streaming)
