def moveKJuniorReadProxSensors(self):
"slowly move forward and print normal vector readings"
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0]
for step in xrange(self.noSteps):
rightSpeed = 10
leftSpeed = rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0],
leftInput[3],
leftInput[4],
rightInput[0],
rightInput[3],
rightInput[4])
sleep(.2)
def initialize_proximity_sensor(self):
self.sensor_handle = self.get_handle('Proximity_sensor')
if self.sensor_handle is not None:
vrep.simxReadProximitySensor(self.client_id, self.sensor_handle,
streaming)
else:
print("Coudln't get the proximity sensor handle.")
def braiten1b(self):
"slowly move forward and print normal vector readings"
intens = 100
ambientIntensRatio = 0.2
attVect = [0,0,pi *4]
for step in xrange(self.noSteps):
rightInput = vrep.simxReadProximitySensor(self.simulID, self.rightEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.leftEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
angle = degrees(self.angleBetVecs([0,0,1], centerInput[2]))
lightReading = self.irradAtSensor(intens, ambientIntensRatio, centerInput[2], attVect)
print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0],
centerInput[3],
angle,
lightReading,
norm(centerInput[2]),
centerInput[2])
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, 1/lightReading, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, 1/lightReading, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(0)
def moveAndReadProxSensors(self):
"rotate in place and print sensor distance and normal vector readings"
for step in xrange(self.noSteps):
if step>self.noSteps / 2:
rightSpeed = -1
leftSpeed = -rightSpeed
else:
rightSpeed = 1
leftSpeed = -rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
rightInput = vrep.simxReadProximitySensor(self.simulID, self.rightEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.leftEye, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
print "Left-->err:%s - Detct'd: %s\t%s\t\tRight--> err:%s - Detct'd: %s\t\t\t%s" % (leftInput[0],
leftInput[3],
leftInput[2],
rightInput[0],
rightInput[3],
rightInput[2])
sleep(.1)
self.stopRobot(self.simulID,[self.rightMotor,self.leftMotor])
vrep.simxSynchronousTrigger(self.simulID)
def stop_function(self, port):
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', port, True, True, 5000, 5)
if clientID != -1:
_, ps = vrep.simxGetObjectHandle(
clientID, self.bot_config['proximity_sensor_one'],
vrep.simx_opmode_oneshot_wait)
_, detectionState, detectionPoint, detectionObjectHandle, _ = vrep.simxReadProximitySensor(
clientID, ps, vrep.simx_opmode_oneshot_wait)
_, ps1 = vrep.simxGetObjectHandle(
clientID, self.bot_config['proximity_sensor_two'],
vrep.simx_opmode_oneshot_wait)
_, detectionState1, detectionPoint1, detectionObjectHandle1, _ = vrep.simxReadProximitySensor(
clientID, ps1, vrep.simx_opmode_oneshot_wait)
_, ps2 = vrep.simxGetObjectHandle(
clientID, self.bot_config['proximity_sensor_three'],
vrep.simx_opmode_oneshot_wait)
_, detectionState2, detectionPoint2, detectionObjectHandle2, _ = vrep.simxReadProximitySensor(
clientID, ps2, vrep.simx_opmode_oneshot_wait)
dis = math.sqrt(detectionPoint[0]**2 + detectionPoint[1]**2 +
detectionPoint[2]**2)
dis1 = math.sqrt(detectionPoint1[0]**2 + detectionPoint1[1]**2 +
detectionPoint1[2]**2)
dis2 = math.sqrt(detectionPoint2[0]**2 + detectionPoint2[1]**2 +
detectionPoint2[2]**2)
if (detectionObjectHandle
== 40) or (detectionObjectHandle1
== 40) or (detectionObjectHandle2 == 40):
return [dis, dis1, dis2]
else:
return [100, 100, 100]
def simulate(self):
plt.xlabel('time')
plt.ylabel('ditance, meters')
while vrep.simxGetConnectionId(self.clientID) != -1:
# получаем данные с сенсоров
(errorCode, sensorState, sensorDetection, detectedObjectHandle,
detectedSurfaceNormalVectorUp) = vrep.simxReadProximitySensor(
self.clientID, self.sonic_sensor, vrep.simx_opmode_streaming)
(errorCode, frontState, frontDetection, detectedObjectHandle,
detectedSurfaceNormalVectorFr) = vrep.simxReadProximitySensor(
self.clientID, self.front_sonic_sensor,
vrep.simx_opmode_streaming)
if (frontState and sensorState):
# есть данные с двух сенсоров
self.calulate(sensorState,
min(sensorDetection[2], frontDetection[2]))
self.dists.append(min(sensorDetection[2], frontDetection[2]))
elif (frontState):
# данные только с переднего сенсора
self.calulate(frontState, frontDetection[2])
self.dists.append(frontDetection[2])
elif (sensorState):
# данные только с бокового сенсора
self.calulate(sensorState, sensorDetection[2])
self.dists.append(sensorDetection[2])
else:
# нет данных
self.calulate(sensorState, self.reqDist + 0.1)
self.dists.append(self.reqDist + 0.1)
self.timeLine.append(time.clock())
# добавляем точку на график в реальном времени
plt.scatter(self.timeLine[-1], self.dists[-1])
plt.pause(0.05)
time.sleep(0.12)
plt.show()
def getSensor(self):
#####################
## return distance detected by proximity sensor
#####################
retries = 0
while True:
returnCode1, sonarHandle1 = vrep.simxGetObjectHandle(self.vrepclientID,'Proximity_sensor1',vrep.simx_opmode_oneshot_wait)
returnCode2, sonarHandle2 = vrep.simxGetObjectHandle(self.vrepclientID,'Proximity_sensor2',vrep.simx_opmode_oneshot_wait)
retries += 1
if (returnCode1 == 0 and returnCode2 == 0) or retries > 3:
break
retries = 0
dist1 = -1
dist2 = -1
if returnCode1 == 0 and returnCode2 == 0:
while True:
if self.firstOrBuffered:
returnCode1,_,detectedPoint1,_,_=vrep.simxReadProximitySensor(self.vrepclientID,sonarHandle1,vrep.simx_opmode_streaming)
returnCode2,_,detectedPoint2,_,_=vrep.simxReadProximitySensor(self.vrepclientID,sonarHandle2,vrep.simx_opmode_streaming)
#do not use initializing value from sensor
dist1 = 1
dist2 = 1
else:
returnCode1,_,detectedPoint1,_,_=vrep.simxReadProximitySensor(self.vrepclientID,sonarHandle1,vrep.simx_opmode_buffer)
returnCode2,_,detectedPoint2,_,_=vrep.simxReadProximitySensor(self.vrepclientID,sonarHandle2,vrep.simx_opmode_buffer)
dist1 = np.linalg.norm(np.array(detectedPoint1))
dist2 = np.linalg.norm(np.array(detectedPoint2))
retries += 1
if (returnCode1 == 0 and returnCode2 == 0) or retries > 3:
break
dist = min(dist1, dist2)
return dist, dist1, dist2
def check_collision(sensor):
maxd=0.5 ###clearance from the obstacle
mind=0.2
detect=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
braitenbergL=[-0.2,-0.4,-0.6,-0.8,-1,-1.2,-1.4,-1.6, 1.6,1.4,1.2,1,0.8,0.6,0.4,0.2]
braitenbergR=[-1.6,-1.4,-1.2,-1,-0.8,-0.6,-0.4,-0.2, 0.2,0.4,0.6,0.8,1,1.2,1.4,1.6]
# braitenbergL=[-0.2,-0.4,-0.6,-0.8,-1,-1.2,-1.4,-1.6, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
# braitenbergR=[-1.6,-1.4,-1.2,-1,-0.8,-0.6,-0.4,-0.2, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
v = 0.5 ###motor speed
for i in range(16):
Code,State,Point,ObjectHandle,SurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor[i],vrep.simx_opmode_streaming)
Code,State,Point,ObjectHandle,SurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor[i],vrep.simx_opmode_buffer)
# time.sleep(0.1)
# detect[i] = 0
# print("point = ",Point)
# dist = math.sqrt(Point[0]**2 + Point[1]**2 + Point[2]**2)
dist = math.sqrt(Point[0]**2 + Point[2]**2)
# if (dist < maxd and State != 1):
if ((State != 0) and (dist < maxd)):
# print("yes %d",i)
# print("dist x ,z ",Point[0], Point[2])
if(dist < mind):
dist = mind
detect[i] = 1 - ((dist - mind)/(maxd - mind))
else:
detect[i] = 0
v_left = v
v_right = v
for i in range(16):
v_left = v_left + braitenbergL[i] * detect[i]
v_right = v_right + braitenbergR[i] * detect[i]
return v_left, v_right
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 create_proximity_sensors(self):
err_code, self.proximity_sensor_1 = vrep.simxGetObjectHandle(
self.map.clientID, "s1", vrep.simx_opmode_blocking)
err_code, self.proximity_sensor_2 = vrep.simxGetObjectHandle(
self.map.clientID, "s2", vrep.simx_opmode_blocking)
err_code, self.proximity_sensor_3 = vrep.simxGetObjectHandle(
self.map.clientID, "s3", vrep.simx_opmode_blocking)
err_code, self.proximity_sensor_4 = vrep.simxGetObjectHandle(
self.map.clientID, "s4", vrep.simx_opmode_blocking)
err_code, self.proximity_sensor_5 = vrep.simxGetObjectHandle(
self.map.clientID, "s5", vrep.simx_opmode_blocking)
vrep.simxReadProximitySensor(self.map.clientID,
self.proximity_sensor_1,
vrep.simx_opmode_streaming)
vrep.simxReadProximitySensor(self.map.clientID,
self.proximity_sensor_2,
vrep.simx_opmode_streaming)
vrep.simxReadProximitySensor(self.map.clientID,
self.proximity_sensor_3,
vrep.simx_opmode_streaming)
vrep.simxReadProximitySensor(self.map.clientID,
self.proximity_sensor_4,
vrep.simx_opmode_streaming)
vrep.simxReadProximitySensor(self.map.clientID,
self.proximity_sensor_5,
vrep.simx_opmode_streaming)
def readProximitySensor(self, object):
operation = self.__controlCommandsOnServer('proximity_sensor', object)
if operation is vrep.simx_opmode_streaming:
vrep.simxReadProximitySensor(self.clientID, object, operation)
_, sensor_reading, detected_object, detected_object_handle, detected_surface = vrep.simxReadProximitySensor(
self.clientID, object, vrep.simx_opmode_streaming)
return sensor_reading, detected_object, detected_object_handle, detected_surface
def _init_sensor_handles(self):
self.sensor_handles = [] # empty list for handles
for x in range(1, 16 + 1):
error_code, sensor_handle = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_ultrasonicSensor' + str(x), vrep.simx_opmode_oneshot_wait)
self.sensor_handles.append(sensor_handle)
vrep.simxReadProximitySensor(self.client_id, sensor_handle, vrep.simx_opmode_streaming)
def pick_b(self,port):
dis1_flag = False
dis2_flag = False
dis3_flag = False
dis4_flag = False
dis5_flag = False
_,ps1=vrep.simxGetObjectHandle(self.clientID,self.bot_config['proximity_sensor_one'],vrep.simx_opmode_oneshot_wait)
_,detectionState1,detectionPoint1,detectionObjectHandle1,_=vrep.simxReadProximitySensor(self.clientID,ps1,vrep.simx_opmode_oneshot_wait)
_,ps2=vrep.simxGetObjectHandle(self.clientID,self.bot_config['proximity_sensor_two'],vrep.simx_opmode_oneshot_wait)
_,detectionState2,detectionPoint2,detectionObjectHandle2,_=vrep.simxReadProximitySensor(self.clientID,ps2,vrep.simx_opmode_oneshot_wait)
_,ps3=vrep.simxGetObjectHandle(self.clientID,self.bot_config['proximity_sensor_three'],vrep.simx_opmode_oneshot_wait)
_,detectionState3,detectionPoint3,detectionObjectHandle3,_=vrep.simxReadProximitySensor(self.clientID,ps3,vrep.simx_opmode_oneshot_wait)
_,ps4=vrep.simxGetObjectHandle(self.clientID,self.bot_config['proximity_sensor_four'],vrep.simx_opmode_oneshot_wait)
_,detectionState4,detectionPoint4,detectionObjectHandle4,_=vrep.simxReadProximitySensor(self.clientID,ps4,vrep.simx_opmode_oneshot_wait)
_,ps5=vrep.simxGetObjectHandle(self.clientID,self.bot_config['proximity_sensor_five'],vrep.simx_opmode_oneshot_wait)
_,detectionState5,detectionPoint5,detectionObjectHandle5,_=vrep.simxReadProximitySensor(self.clientID,ps5,vrep.simx_opmode_oneshot_wait)
dis = 10000
print(detectionState1,detectionState2,detectionState3, detectionState4, detectionState5)
if detectionState1:
dis = math.sqrt(detectionPoint1[0]**2+detectionPoint1[1]**2+detectionPoint1[2]**2)
dis1_flag = True
elif detectionState2:
dis = math.sqrt(detectionPoint2[0]**2+detectionPoint2[1]**2+detectionPoint2[2]**2)
dis2_flag = True
elif detectionState3:
dis = math.sqrt(detectionPoint3[0]**2+detectionPoint3[1]**2+detectionPoint3[2]**2)
dis3_flag = True
elif detectionState4:
dis = math.sqrt(detectionPoint4[0]**2+detectionPoint4[1]**2+detectionPoint4[2]**2)
dis4_flag = True
elif detectionState5:
dis = math.sqrt(detectionPoint5[0]**2+detectionPoint5[1]**2+detectionPoint5[2]**2)
dis5_flag = True
# print("inside pick:::: ", dis)
if (dis<0.3):
if (dis1_flag):
_=vrep.simxSetObjectPosition(self.clientID,detectionObjectHandle1,ps1,[0,0,12],vrep.simx_opmode_oneshot_wait)
return detectionObjectHandle1
elif dis2_flag:
_=vrep.simxSetObjectPosition(self.clientID,detectionObjectHandle2,ps2,[0,0,12],vrep.simx_opmode_oneshot_wait)
return detectionObjectHandle2
elif dis3_flag:
_=vrep.simxSetObjectPosition(self.clientID,detectionObjectHandle3,ps3,[0,0,12],vrep.simx_opmode_oneshot_wait)
return detectionObjectHandle3
elif (dis4_flag):
_=vrep.simxSetObjectPosition(self.clientID,detectionObjectHandle4,ps4,[0,0,12],vrep.simx_opmode_oneshot_wait)
return detectionObjectHandle4
elif (dis5_flag):
_=vrep.simxSetObjectPosition(self.clientID,detectionObjectHandle5,ps5,[0,0,12],vrep.simx_opmode_oneshot_wait)
return detectionObjectHandle5
else :
print("not able to pick any block")
return 0
def init_sensors(self):
self.__sensors = []
for i in range(1, 9):
error_code, sensor = vrep.simxGetObjectHandle(
self.__client_ID, "ePuck_proxSensor" + str(i) + self.__suffix,
vrep.simx_opmode_blocking)
assert error_code == 0, "Sensor handle could not be obtained."
self.__sensors.append(sensor)
vrep.simxReadProximitySensor(self.__client_ID,
self.__sensors[i - 1],
vrep.simx_opmode_streaming)
def _init_sensor_handles(self):
self.sensor_handles = [] # empty list for handles
for x in range(1, 16 + 1):
error_code, sensor_handle = vrep.simxGetObjectHandle(
self.client_id, 'Pioneer_p3dx_ultrasonicSensor' + str(x),
vrep.simx_opmode_oneshot_wait)
self.sensor_handles.append(sensor_handle)
vrep.simxReadProximitySensor(self.client_id, sensor_handle,
vrep.simx_opmode_streaming)
def get_distance_sensor(self, name):
"""Get the distance read by the sensor with name <name>."""
handle = self.get_handle(name)
if handle not in self._s:
vrep.simxReadProximitySensor(
self.id, handle, vrep.simx_opmode_streaming)
self._s.append(handle)
time.sleep(0.5)
r, s, p, h, _ = vrep.simxReadProximitySensor(
self.id, handle, vrep.simx_opmode_buffer)
if r != vrep.simx_return_ok:
raise NameError('Sensor unknown')
return p[2]
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 readProximitySensor():
valores_1 = vrep.simxReadProximitySensor(clientID, proximitySensors[0],
vrep.simx_opmode_oneshot_wait)
#print(valores_1)
valores_2 = vrep.simxReadProximitySensor(clientID, proximitySensors[1],
vrep.simx_opmode_oneshot_wait)
valores_3 = vrep.simxReadProximitySensor(clientID, proximitySensors[2],
vrep.simx_opmode_oneshot_wait)
valores_4 = vrep.simxReadProximitySensor(clientID, proximitySensors[3],
vrep.simx_opmode_oneshot_wait)
valores_5 = vrep.simxReadProximitySensor(clientID, proximitySensors[4],
vrep.simx_opmode_oneshot_wait)
valores_6 = vrep.simxReadProximitySensor(clientID, proximitySensors[5],
vrep.simx_opmode_oneshot_wait)
valores_7 = vrep.simxReadProximitySensor(clientID, proximitySensors[6],
vrep.simx_opmode_oneshot_wait)
valores_8 = vrep.simxReadProximitySensor(clientID, proximitySensors[7],
vrep.simx_opmode_oneshot_wait)
sensores_detectados = [
valores_1[1] * 1, valores_2[1] * 1, valores_3[1] * 1, valores_4[1] * 1,
valores_5[1] * 1, valores_6[1] * 1, valores_7[1] * 1, valores_8[1] * 1
]
#print(sensores_detectados)
return sensores_detectados
def read_proximity_sensor(self):
rc, detectionState, detectedPoint, detectedObjectHandle, DSNV = vrep.simxReadProximitySensor(
self.client_id, self.sensor_handle, buffering)
distance = round(detectedPoint[2], 3)
if detectionState:
print("Detected Object: {}\t|\tDistance: {}\tMeters".format(
self.dict_handle_to_name[detectedObjectHandle], distance))
def get_proximity_data(self):
(e, detectionState, detectedPoint, detectedObjectHandle,
detectedSurfaceNormalVector) = vrep.simxReadProximitySensor(
self.client_id, self.proximity_handle, vrep.simx_opmode_buffer)
self.check_error_code(e, "simxReadProximitySensor error")
return detectionState, np.linalg.norm(
detectedPoint), detectedSurfaceNormalVector
def drive(self):
Left = self.v0
Right = self.v0
for i in range(0,len(self.ultra_sensors)-1):
res, detectionState, \
detectedPoint, detectedObjectHandle, \
detectedSurfaceNormalVector = \
vrep.simxReadProximitySensor(self.clientID,
self.ultra_sensors[i],
vrep.simx_opmode_buffer)
dist = vec_length(detectedPoint)
if (res==0) and (dist<self.noDetectionDist):
if (dist<self.maxDetectionDist):
dist=self.maxDetectionDist
self.detect[i]=1-((dist-self.maxDetectionDist)/(self.noDetectionDist-self.maxDetectionDist))
else:
self.detect[i]=0
Left=Left+self.braitenbergL[i]*self.detect[i]
Right=Right+self.braitenbergR[i]*self.detect[i]
vrep.simxSetJointTargetVelocity(self.clientID,
self.left_motor_handle,
Left,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID,
self.right_motor_handle,
Right,
vrep.simx_opmode_streaming)
def __init__(self, clientID, name):
self.clientID = clientID
self.name = name
self.noDetectionDist = 0.5
self.maxDetectionDist = 0.2
self.detect = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
self.braitenbergL = [-0.2,-0.4,-0.6,-0.8,-1,-1.2,-1.4,-1.6, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
self.braitenbergR = [-1.6,-1.4,-1.2,-1,-0.8,-0.6,-0.4,-0.2, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
self.v0 = 2
errorCode, self.left_motor_handle = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_leftMotor'+name,
vrep.simx_opmode_oneshot_wait)
errorCode, self.right_motor_handle = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_rightMotor'+name,
vrep.simx_opmode_oneshot_wait)
self.ultra_sensors = []
for i in range(1,16):
errorCode, sensor_handle = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_ultrasonicSensor'+str(i)+name,
vrep.simx_opmode_oneshot_wait)
if not errorCode:
self.ultra_sensors.append(sensor_handle)
returnCode, detectionState, \
detectedPoint, detectedObjectHandle, \
detectedSurfaceNormalVector = \
vrep.simxReadProximitySensor(clientID,
self.ultra_sensors[i-1],
vrep.simx_opmode_streaming)
else:
print 'Failed to connect to ultrasonic sensor '+str(i)
def get_distance(self, fast=True, prec=None):
"""Retrieve distance to the detected point."""
if self._handle < 0:
if self._handle == MISSING_HANDLE:
raise RuntimeError("Could not retrieve data from {}: missing "
"name or handle.".format(self._name))
if self._handle == REMOVED_OBJ_HANDLE:
raise RuntimeError("Could not retrieve data from {}: object "
"removed.".format(self._name))
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not retrieve data from {}: not connected to V-REP "
"remote API server.".format(self._name))
res, detect, point, _, _ = vrep.simxReadProximitySensor(
client_id, self._handle, vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
if detect:
if fast:
distance = point[2]
else:
distance = math.sqrt(sum(coord * coord for coord in point))
return distance if prec is None else round(distance, prec)
else:
return None
elif res == vrep.simx_return_novalue_flag:
return None
else:
raise ServerError("Could not retrieve data from {}.".format(
self._name))
def get_distance_obstacle():
""" return an array of distances measured by lasers (m) """
for i in range(0, N_LASERS):
rc, ds, detected_point, doh, dsn = vrep.simxReadProximitySensor(
clientID, laserID[i], MODE)
distance[i] = detected_point[2]
return distance
def getObstacleDist(sensorHandler_):
dist2Obstacle_LR = [0.0, 0.0]
# Get raw sensor readings using API
rawSR = vrep.simxReadProximitySensor(clientID, sensorHandler_, vrep.simx_opmode_oneshot_wait)
# Calculate Euclidean distance
distance_ = math.sqrt(rawSR[2][0]*rawSR[2][0] + rawSR[2][1]*rawSR[2][1] + rawSR[2][2]*rawSR[2][2])
return distance_
def __init__(self, clientID, L, r):
self.clientID = clientID
# Get the necessary handles
_, self.pioneer = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx',
vrep.simx_opmode_blocking)
_, self.motorL = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_leftMotor',
vrep.simx_opmode_blocking)
_, self.motorR = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_blocking)
self.usensor = []
for i in range(1, 17):
err, s = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(i),
vrep.simx_opmode_blocking)
self.usensor.append(s)
# Init sensors
for i in range(16):
_, state, point, detectedObj, detectedSurfNormVec = vrep.simxReadProximitySensor(
clientID, self.usensor[i], vrep.simx_opmode_streaming)
self.L = L
self.r = r
self.sState = []
self.sPoints = []
def prox_dist(cid, handle, default=0):
err, state, point, handle, normal = vrep.simxReadProximitySensor(
cid, handle, vrep_mode)
if state:
return [point[2]]
else:
return [default]
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 drive(self):
Left = self.v0
Right = self.v0
for i in range(0, len(self.ultra_sensors) - 1):
res, detectionState, \
detectedPoint, detectedObjectHandle, \
detectedSurfaceNormalVector = \
vrep.simxReadProximitySensor(self.clientID,
self.ultra_sensors[i],
vrep.simx_opmode_buffer)
dist = vec_length(detectedPoint)
if (res == 0) and (dist < self.noDetectionDist):
if (dist < self.maxDetectionDist):
dist = self.maxDetectionDist
self.detect[i] = 1 - (
(dist - self.maxDetectionDist) /
(self.noDetectionDist - self.maxDetectionDist))
else:
self.detect[i] = 0
Left = Left + self.braitenbergL[i] * self.detect[i]
Right = Right + self.braitenbergR[i] * self.detect[i]
vrep.simxSetJointTargetVelocity(self.clientID, self.left_motor_handle,
Left, vrep.simx_opmode_streaming)
vrep.simxSetJointTargetVelocity(self.clientID, self.right_motor_handle,
Right, vrep.simx_opmode_streaming)
def prox_inv_dist(cid, handle, max_val=1):
# Returns a smaller value the further the detected object is away
err, state, point, handle, normal = vrep.simxReadProximitySensor(cid, handle, vrep_mode)
if state:
return [max_val - point[2]]
else:
return [0]
def loop(self):
operationMode=vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop=False
else:
operationMode=vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__orientation=orientation[2]
else:
self.__orientation = None
# print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
self.__mind.setInput("orientation", self.__orientation)
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__position=[0.0,0.0]
self.__position[0]=position[0] #X
self.__position[1]=position[1] #Y
else:
self.__position=None
# print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode)
if returnCode==vrep.simx_return_ok:
try:
# self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation)
self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2)
self.__mind.setInput("velocity", self.__velocity)
except TypeError:
pass
# if self.__name=="BubbleRob#1":
# print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation)
else:
self.__velocity=None
# print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
if returnCode==vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID))
self.__mind.setInput("sensorTrigger", sensorTrigger)
self.__mind.setInput("mostSalientItem", self.__mind.selectMostSalient("I"))
# print self.__name, self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName"))
self.__mind.setInput("attendedItem", self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName")))
self.__mind.applyRules()
self.__mind.setStates()
if self.__mind.getOutput("steering")!=None:
self.setSteering(self.__mind.getOutput("steering"))
if self.__mind.getOutput("thrust")!=None:
self.setThrust(self.__mind.getOutput("thrust"))
if self.__mind.getOutput("reward")!=None:
if self.__mind.getOutput("reward")>0.5:
self.setEmotionalExpression(1)
elif self.__mind.getOutput("reward")<-0.5:
self.setEmotionalExpression(-1)
else:
self.setEmotionalExpression(0)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage!="":
print("Debug:"+dMessage)
self.__cnt=self.__cnt+1
def sensor_read(clientID, sensorHandler):
errorCode, state, point, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(
clientID, sensorHandler, vrep.simx_opmode_oneshot_wait)
dist = math.sqrt(point[0] * point[0] + point[1] * point[1])
zmax = 2
if dist > zmax:
dist = zmax
return state, dist
def prox_inv_dist(cid, handle, max_val=1):
# Returns a smaller value the further the detected object is away
err, state, point, handle, normal = vrep.simxReadProximitySensor(
cid, handle, vrep_mode)
if state:
return [max_val - point[2]]
else:
return [0]
def obstacle(self, port):
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', port, True, True, 5000, 5)
if clientID != -1:
_, ps1 = vrep.simxGetObjectHandle(
clientID, self.bot_config['proximity_sensor_one'],
vrep.simx_opmode_oneshot_wait)
_, detectionState1, detectionPoint1, detectionObjectHandle1, _ = vrep.simxReadProximitySensor(
clientID, ps1, vrep.simx_opmode_oneshot_wait)
_, ps2 = vrep.simxGetObjectHandle(
clientID, self.bot_config['proximity_sensor_two'],
vrep.simx_opmode_oneshot_wait)
_, detectionState2, detectionPoint2, detectionObjectHandle2, _ = vrep.simxReadProximitySensor(
clientID, ps2, vrep.simx_opmode_oneshot_wait)
_, ps3 = vrep.simxGetObjectHandle(
clientID, self.bot_config['proximity_sensor_three'],
vrep.simx_opmode_oneshot_wait)
_, detectionState3, detectionPoint3, detectionObjectHandle3, _ = vrep.simxReadProximitySensor(
clientID, ps3, vrep.simx_opmode_oneshot_wait)
_, ps4 = vrep.simxGetObjectHandle(
clientID, self.bot_config['proximity_sensor_four'],
vrep.simx_opmode_oneshot_wait)
_, detectionState4, detectionPoint4, detectionObjectHandle4, _ = vrep.simxReadProximitySensor(
clientID, ps4, vrep.simx_opmode_oneshot_wait)
_, ps5 = vrep.simxGetObjectHandle(
clientID, self.bot_config['proximity_sensor_five'],
vrep.simx_opmode_oneshot_wait)
_, detectionState5, detectionPoint5, detectionObjectHandle5, _ = vrep.simxReadProximitySensor(
clientID, ps5, vrep.simx_opmode_oneshot_wait)
dis1 = math.sqrt(detectionPoint1[0]**2 + detectionPoint1[1]**2 +
detectionPoint1[2]**2)
dis2 = math.sqrt(detectionPoint2[0]**2 + detectionPoint2[1]**2 +
detectionPoint2[2]**2)
dis3 = math.sqrt(detectionPoint3[0]**2 + detectionPoint3[1]**2 +
detectionPoint3[2]**2)
dis4 = math.sqrt(detectionPoint4[0]**2 + detectionPoint4[1]**2 +
detectionPoint4[2]**2)
dis5 = math.sqrt(detectionPoint5[0]**2 + detectionPoint5[1]**2 +
detectionPoint5[2]**2)
list1 = [dis1, dis2, dis3, dis4, dis5]
list1.sort()
print(list1[1])
def load_proximity_sensor(self, sensor):
"""First call to initiate given proximity sensor.
Args:
sensor(int): the index of the sensor to load.
"""
return vrep.simxReadProximitySensor(self.client_id,
self.proximity_sensors[sensor],
vrep.simx_opmode_streaming)
def get_proximity_sensor(self, sensor):
"""Get value of given proximity sensor.
Args:
sensor(int): the index of the sensor to get value of.
"""
return vrep.simxReadProximitySensor(self.client_id,
self.proximity_sensors[sensor],
vrep.simx_opmode_buffer)
def read(self) -> (bool, Vec3):
"""
Reads the state of a proximity sensor.
@return detection state and detected point
@rtype (bool, Vec3)
"""
code, state, point, handle, snv = v.simxReadProximitySensor(
self._id, self._handle, self._def_op_mode)
return state, Vec3(point[0], point[1], point[2])
def get_distance_obstacle():
for i in range(0, N_Ultrasonic):
rc, ds, detected_point, doh, dsn = vrep.simxReadProximitySensor(
clientID, ultrasonicID[i], MODE)
if ds == 1:
distance[i] = detected_point[2]
else:
distance[i] = float('inf')
return distance
def readASensor(self, sensor):
retcode, activated, point, dummy1, dummy2 = vrep.simxReadProximitySensor(self.clientID, sensor, vrep.simx_opmode_streaming)
if retcode != vrep.simx_return_ok:
self.printMessage('Failed reading proximity sensor id= ' + str(sensor) + str(retcode))
else:
if activated:
return point[2]
else:
return 1.0
def getObstacleDist(sensorHandler_):
# Get raw sensor readings using API
rawSR = vrep.simxReadProximitySensor(robot.clientID, sensorHandler_, vrep.simx_opmode_oneshot_wait)
#print(rawSR)
# Calculate Euclidean distance
if rawSR[1]: # if true, obstacle is within detection range, return distance to obstacle
return math.sqrt(rawSR[2][0]*rawSR[2][0] + rawSR[2][1]*rawSR[2][1] + rawSR[2][2]*rawSR[2][2])
else: # if false, obstacle out of detection range, return inf.
return float('inf')
def getObstacleDist(sensorHandler_):
dist2Obstacle_LR = [0.0, 0.0]
# Get raw sensor readings using API
rawSR = vrep.simxReadProximitySensor(clientID, sensorHandler_, vrep.simx_opmode_oneshot_wait)
print(rawSR)
# Calculate Euclidean distance
if rawSR[1]: # if true, obstacle is within detection range, return distance to obstacle
return math.sqrt(rawSR[2][0]*rawSR[2][0] + rawSR[2][1]*rawSR[2][1] + rawSR[2][2]*rawSR[2][2])
else: # if false, obstacle out of detection range, return inf.
return float('inf')
def prox_sens_initialize(clientID, botName):
""" Initialize proximity sensor. Maybe helpful later """
proxSens=[]
for i in range(4):
handleName = '%s_proxSensor%d' % (botName, i+1)
_, oneProxSens = vrep.simxGetObjectHandle(clientID, handleName, vrep.simx_opmode_oneshot_wait)
# call the prox sensor once to start it
_ = vrep.simxReadProximitySensor(clientID, oneProxSens, vrep.simx_opmode_streaming)
proxSens.append(oneProxSens)
return proxSens
def prox_sens_read(clientID, proxSens):
""" Read the proximity sensor
clientID: vrep clientID
proxSens: array of proxSensor handles
"""
outputs = []
keys = ('returnCode','detectionState','detectedPoint','detectedObjectHandle','detectedSurfaceNormalVector')
# NOTE: take norm of deteected point to get the distance
for i in range(8):
proxOut=vrep.simxReadProximitySensor(clientID, proxSens[i], vrep.simx_opmode_streaming)
outputs.append(dict(zip(keys, proxOut)))
return outputs
def read_proximity( self ):
err, state, point, handle, normal = vrep.simxReadProximitySensor(self.cid, self.vert_prox, vrep_mode)
if state:
self.vert_prox_dist = point[2]
else:
self.vert_prox_dist = self.max_vert_dist
err, state, point, handle, normal =\
vrep.simxReadProximitySensor(self.cid, self.left_prox, vrep_mode)
if state:
self.left_prox_dist = point[2]
else:
self.left_prox_dist = self.max_left_dist
err, state, point, handle, normal =\
vrep.simxReadProximitySensor(self.cid, self.right_prox, vrep_mode)
if state:
self.right_prox_dist = point[2]
else:
self.right_prox_dist = self.max_right_dist
def loop(self):
operationMode=vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop=False
else:
operationMode=vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__orientation=orientation[2] #Z
else:
self.__orientation = None
print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__position=[0.0,0.0]
self.__position[0]=position[0] #X
self.__position[1]=position[1] #Y
else:
print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
self.__position=None
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
if returnCode==vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
simulationTime=vrep.simxGetLastCmdTime(self.__clientID)
thrust=0.0
steering=0.0
if simulationTime-self.__driveBackStartTime<3000:
# driving backwards while slightly turning:
thrust=-1.0
steering=-1.0
else:
# going forward:
thrust=1.0
steering=0.0
if sensorTrigger:
self.__driveBackStartTime=simulationTime # We detected something, and start the backward mode
self.setSteering(steering)
self.setThrust(thrust)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage!="":
print("Debug:"+dMessage)
def observe(self):
operationMode=vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop=False
else:
operationMode=vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__orientation=orientation[2]
else:
self.__orientation = None
# print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
self.__mind.setInput("orientation", self.__orientation)
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__position=[0.0,0.0]
self.__position[0]=position[0] #X
self.__position[1]=position[1] #Y
else:
self.__position=None
print >> sys.stderr, "Error in VRepBubbleRob.getPosition()", self.__clientID, self.__bodyHandle
returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode)
if returnCode==vrep.simx_return_ok:
try:
# self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation)
self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2)
self.__mind.setInput("velocity", self.__velocity)
self.__linearVelocity=linearVelocity
except TypeError:
pass
# if self.__name=="BubbleRob#1":
# print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation)
else:
self.__velocity=None
# print >> sys.stderr, "Error in VRepBubbleRob.getVelocity()"
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
if returnCode==vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID))
self.__mind.setInput("sensorTrigger", sensorTrigger)
self.blocked() # judge if blocked
def moveKJuniorReadProxSensors(self):
"slowly move forward and print normal vector readings"
intens = 100
ambientIntens = 0
attVect = [0,0,pi *4]
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0]
for step in xrange(self.noSteps):
rightSpeed = 10
leftSpeed = rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
#===================================================================
# print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0],
# leftInput[3],
# leftInput[4],
# rightInput[0],
# rightInput[3],
# rightInput[4])
#===================================================================
angle = degrees(self.angleBetVecs([0,0,1], centerInput[2]))
lightReading = self.irradAtSensor(intens, ambientIntens, centerInput[2], attVect)
print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0],
centerInput[3],
angle,
lightReading,
norm(centerInput[2]),
centerInput[2])
sleep(0)
def braiten2a(self):
"Seek light source"
intens = 50
ambientIntensRatio = .2
attVect = [0,0,1]
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0]
for step in xrange(self.noSteps):
rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
#===================================================================
# print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0],
# leftInput[3],
# leftInput[4],
# rightInput[0],
# rightInput[3],
# rightInput[4])
#===================================================================
angle = degrees(self.angleBetVecs([0,0,1], centerInput[2]))
lightReading = self.irradAtSensor(intens, ambientIntens, centerInput[2], attVect)
print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0],
centerInput[3],
angle,
lightReading,
norm(centerInput[2]),
centerInput[2])
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, lightReading, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, lightReading, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
sleep(0)
def raw_readProximitySensor(self, sensorHandle, operationMode = vrep.simx_opmode_oneshot_wait):
return vrep.simxReadProximitySensor(self.clientID, sensorHandle, operationMode)
def ReadProximitySensor(clientID, sensor_handles):
# Get all the handles at once
shape = len(sensor_handles)
sensor_val=np.array([]) #empty array for sensor measurements
sensor_state=np.array([]) #empty array for sensor measurements
errorcode =[]
for x in range(1,shape+1):
get_errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_handles[x-1],vrep.simx_opmode_buffer)
errorcode.append(get_errorCode)
sensor_val=np.append(sensor_val,np.linalg.norm(detectedPoint)) #get list of values
sensor_state=np.append(sensor_state,detectionState) #get list of values
return errorcode, sensor_val, sensor_state
def read_from_sensors(self):
for i in range(0, 16):
error_code, detection_state, detected_point, detected_object_handle, detected_surface_normal_vector = vrep.simxReadProximitySensor(self.client_id, self.sensor_handles[i], vrep.simx_opmode_streaming)
dist = math.sqrt(detected_point[0] ** 2 + detected_point[1] ** 2 + detected_point[2] ** 2)
if dist < self.MIN_DETECTION_DIST:
self.detect[i] = self.MIN_DETECTION_DIST
elif dist > self.MAX_DETECTION_DIST or detection_state is False:
self.detect[i] = self.MAX_DETECTION_DIST
else:
self.detect[i] = self.MAX_DETECTION_DIST - ((dist - self.MAX_DETECTION_DIST) / (self.MIN_DETECTION_DIST - self.MAX_DETECTION_DIST))
def mapgen(scene_name,x,y,z,clientID):
#part which loads
if os.path.isfile('./Mapdata/'+ scene_name +'.npy'):
arr=np.load('./Mapdata/'+ scene_name +'.npy')
#part which creates
else:
#initialize the 2 sensors
errorCode,sensor1=vrep.simxGetObjectHandle(clientID,'Sensor_1',vrep.simx_opmode_oneshot_wait)
errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor1,vrep.simx_opmode_streaming)
errorCode,sensor2=vrep.simxGetObjectHandle(clientID,'Sensor_2',vrep.simx_opmode_oneshot_wait)
errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor2,vrep.simx_opmode_streaming)
#init some values
time.sleep(3)
xmax=x/0.4
ymax=y/0.4
zmax=z/0.4
xmax=int(math.ceil(xmax))
ymax=int(math.ceil(ymax))
zmax=int(math.ceil(zmax))
arr=np.ndarray(shape=(xmax,ymax,zmax),dtype=int)
#move the sensors through the map and create the data
for index in range(xmax):
for index2 in range(ymax):
for index3 in range(zmax):
#move sensor
x=0.4+0.4*index
y=0.2+0.4*index2
z=0.3+0.4*index3
vrep.simxSetObjectPosition (clientID,sensor1,-1,(x,y,z),vrep.simx_opmode_oneshot)
vrep.simxSetObjectPosition (clientID,sensor2,-1,(x-0.4,y,z),vrep.simx_opmode_oneshot)
time.sleep(0.2)
#check for collision
errorCode,detectionState1,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor1,vrep.simx_opmode_buffer)
errorCode,detectionState2,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor2,vrep.simx_opmode_buffer)
#save the collision result in an array
if (detectionState1 or detectionState2):
arr[index,index2,index3]=1
else:
arr[index,index2,index3]=0
#save the final array as file
np.save('./Mapdata/'+scene_name, np.ndarray(arr))
return arr
def main():
_clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
# get all sensors from robot dr_20
if _clientID != -1:
print "Verbindung zu Server aufgebaut"
print "ClientID: %d" % _clientID
else:
print "Keine Vebindung zum Server moeglich"
exit(0)
(err_h_body, err_h_leftJoint, err_h_rightJoint, err_h_sonicJoint,
err_h_rightWheel, err_h_leftWheel, _bodyHandle, _leftJoint,
_rightJoint, _sonicJoint, _rightWheel,
_leftWheel) = _dr20_login_vrep_func.getJointHandles_dr20_(_clientID)
(err_h_infra6, err_h_infra2, err_h_infra5, err_h_infra1, err_h_sonic,
infra6, infra2, infra5, infra1, sonic) = _dr20_login_vrep_func.getSensorHandles_dr20_(_clientID)
Variables.clientID = _clientID
print Variables.clientID
# pruefe, ob Objekte einen Fehler zurueckgeben
_dr20_login_vrep_func.areJointHandlesValid(err_h_body, err_h_leftJoint,
err_h_rightJoint, err_h_sonicJoint,
err_h_rightWheel, err_h_leftWheel)
_dr20_login_vrep_func.areSensorHandlesValid_dr20_(err_h_infra6, err_h_infra2, err_h_infra5, err_h_infra1, err_h_sonic)
Variables.bodyHandle = _bodyHandle
Variables.leftJoint = _leftJoint
Variables.rightJoint = _rightJoint
Variables.sonicJoint = _sonicJoint
'''
errShape, rightWheel_high_min = vrep.simxGetObjectFloatParameter(_clientID, _rightWheel,
23, vrep.simx_opmode_oneshot_wait)
errShape2, rightWheel_high_max = vrep.simxGetObjectFloatParameter(_clientID, _rightWheel,
26, vrep.simx_opmode_oneshot_wait)
print rightWheel_high_min
print rightWheel_high_max
'''
(err_r_infra6,det_state_infra6,detpoint_infra6,det_handle_infra6,
det_surf_norm_vec_infra6) = vrep.simxReadProximitySensor(_clientID,infra6,
vrep.simx_opmode_streaming)
(err_r_infra2,det_state_infra2,detpoint_infra2,det_handle_infra2,
det_surf_norm_vec_infra2) = vrep.simxReadProximitySensor(_clientID,infra2,
vrep.simx_opmode_streaming)
(err_r_infra5,det_state_infra5,detpoint_infra5,det_handle_infra5,
det_surf_norm_vec_infra5) = vrep.simxReadProximitySensor(_clientID,infra5,
vrep.simx_opmode_streaming)
(err_r_infra1,det_state_infra1,detpoint_infra1,det_handle_infra1,
det_surf_norm_vec_infra1) = vrep.simxReadProximitySensor(_clientID,infra1,
vrep.simx_opmode_streaming)
(err_r_sonic,det_state_sonic,detpoint_sonic,det_handle_sonic,
det_surf_norm_vec_sonic) = vrep.simxReadProximitySensor(_clientID,sonic,
vrep.simx_opmode_streaming)
while(True):
# _dr20_login_vrep_func.moveRobotRaw_dr20_(_leftJoint, _rightJoint, _clientID)
print _clientID
print Variables.clientID
print Variables.leftJoint
_dr20_login_vrep_func.moveLeftJoint(2)
_dr20_login_vrep_func.moveRightJoint(2)
_dr20_login_vrep_func.odometryData()
(err_r_infra6,det_state_infra6,detpoint_infra6,det_handle_infra6,
det_surf_norm_vec_infra6) = vrep.simxReadProximitySensor(_clientID,infra6,
vrep.simx_opmode_buffer)
Variables.det_state_infra6 = det_state_infra6
(err_r_infra2,det_state_infra2,detpoint_infra2,det_handle_infra2,
det_surf_norm_vec_infra2) = vrep.simxReadProximitySensor(_clientID,infra2,
vrep.simx_opmode_buffer)
Variables.det_state_infra2 = det_state_infra2
(err_r_infra5,det_state_infra5,detpoint_infra5,det_handle_infra5,
det_surf_norm_vec_infra5) = vrep.simxReadProximitySensor(_clientID,infra5,
vrep.simx_opmode_buffer)
Variables.det_state_infra5 = det_state_infra5
(err_r_infra1,det_state_infra1,detpoint_infra1,det_handle_infra1,
det_surf_norm_vec_infra1) = vrep.simxReadProximitySensor(_clientID,infra1,
vrep.simx_opmode_buffer)
Variables.det_state_infra1 = det_state_infra1
(err_r_sonic,det_state_sonic,detpoint_sonic,det_handle_sonic,
det_surf_norm_vec_sonic) = vrep.simxReadProximitySensor(_clientID,sonic,
vrep.simx_opmode_buffer)
Variables.det_state_sonic = det_state_sonic
_dr20_login_vrep_func.areSensorReadsValid_dr20_(err_r_infra6, err_r_infra2, err_r_infra5, err_r_infra1, err_r_sonic)
_dr20_login_vrep_func.getDistanceSensors(detpoint_infra6, detpoint_infra2,
detpoint_infra5, detpoint_infra1,
detpoint_sonic)
print Variables.distance
print Variables.robot_pos_old
print Variables.robot_pos_new
vrep.simxFinish(_clientID)
errorCode,usensor13=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor13',vrep.simx_opmode_oneshot_wait) errorCode,usensor14=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor14',vrep.simx_opmode_oneshot_wait) errorCode,usensor15=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor15',vrep.simx_opmode_oneshot_wait) errorCode,usensor16=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor16',vrep.simx_opmode_oneshot_wait) #for i in range(1,100): # # vrep.simxSetJointTargetVelocity(clientID,left_motor,2,vrep.simx_opmode_oneshot_wait) # vrep.simxSetJointTargetVelocity(clientID,right_motor,2,vrep.simx_opmode_oneshot_wait) for i in range(1,10000): vrep.simxSetJointTargetVelocity(clientID,left_motor,0,vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetVelocity(clientID,right_motor,0,vrep.simx_opmode_oneshot_wait) #Sensor4 true (Lurus) error_code, det_state, det_point, det_handle, det_vec = vrep.simxReadProximitySensor(clientID, usensor4,vrep.simx_opmode_oneshot_wait) print 'Sensor : 4 det state: ', det_state,' det point : ', det_point[2] if det_state == True: if det_point[2]<0.3: #Sensor8 #Sensor4 true error_code, det_state, det_point, det_handle, det_vec = vrep.simxReadProximitySensor(clientID, usensor8,vrep.simx_opmode_oneshot_wait) print 'Sensor : 8 det state: ', det_state,' det point : ', det_point[2] if det_state == True: if det_point[2]<0.3: vrep.simxSetJointTargetVelocity(clientID,left_motor,-2,vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetVelocity(clientID,right_motor,2,vrep.simx_opmode_oneshot_wait) time.sleep(0.5); else: vrep.simxSetJointTargetVelocity(clientID,left_motor,-2,vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetVelocity(clientID,right_motor,2,vrep.simx_opmode_oneshot_wait) time.sleep(0.5);
def mapgen_fast(scene_name,x,y,z,clientID):
#load mapdata if file exits
if os.path.isfile('./Mapdata/'+scene_name+'.txt'):
fo = open('./Mapdata/'+scene_name+'.txt', "r")
data_string = fo.read();
print "Read String is : ", str
# Close opend file
fo.close()
arr = np.loads(data_string)
return arr
#otherwise generate mapdata
else:
x0=x
y0=y
#generate the 100 sensors and put them to theirstarting position
errorCode,sensor1=vrep.simxGetObjectHandle(clientID,'Sensor_1',vrep.simx_opmode_oneshot_wait)
objectHandles=np.array([sensor1])
sensor_data=np.zeros(shape=(10,10,1),dtype=int)
for x in range(10):
for y in range(10):
returnCode,new_sensor_handle=vrep.simxCopyPasteObjects(clientID,objectHandles,vrep.simx_opmode_oneshot_wait)
vrep.simxReadProximitySensor(clientID,new_sensor_handle[0],vrep.simx_opmode_streaming)
sensor_data[x,y]=new_sensor_handle[0]
x1=0.4+x*0.4
y1=0.2+y*0.4
z1=0.21
vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot)
#init some values
xmax=x0/4
ymax=y0/4
zmax=z/0.4
xmax=int(math.ceil(xmax))
ymax=int(math.ceil(ymax))
zmax=int(math.ceil(zmax))
arr=np.zeros(shape=(xmax*10,ymax*10,zmax),dtype=int)
#loop to detect the whole area
for index in range(xmax):
for index2 in range(ymax):
for index3 in range(zmax):
for x in range(10):
for y in range(10):
errorCode,detectionState1,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor (clientID,sensor_data[x,y],vrep.simx_opmode_buffer)
#save result
if (detectionState1):
arr[index*10+x,index2*10+y,index3]=1
else:
arr[index*10+x,index2*10+y,index3]=0
for x in range(10):
for y in range(10):
x1=0.4+4*(index)+x*0.4
y1=0.2+4*(index2)+y*0.4
z1=0.4*index3+0.21
vrep.simxSetObjectPosition (clientID,sensor_data[x,y],-1,(x1,y1,z1),vrep.simx_opmode_oneshot)
time.sleep(1)
#save mapdata to file
arr=save(arr)
data_string=np.ndarray.dumps(arr)
print data_string
fo = open('./Mapdata/'+scene_name+'.txt', "w")
fo.write(data_string);
fo.close()
return arr
'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
if erro <> 0:
print 'Handle do motor direito nao encontrado!'
else:
print 'Conectado ao motor direito!'
#inicialização dos sensores (remoteApi)
for i in range(16):
erro, sensorHandle[i] = vrep.simxGetObjectHandle(clientID,
"Pioneer_p3dx_ultrasonicSensor%d" % (i+1),vrep.simx_opmode_oneshot_wait)
if erro <> 0:
print "Handle do sensor Pioneer_p3dx_ultrasonicSensor%d nao encontrado!" % (i+1)
else:
print "Conectado ao sensor Pioneer_p3dx_ultrasonicSensor%d!" % (i+1)
erro, state, coord, detectedObjectHandle,
detectedSurfaceNormalVector = vrep.simxReadProximitySensor(clientID,
sensorHandle[i],vrep.simx_opmode_streaming)
#desvio e velocidade do robo
while vrep.simxGetConnectionId(clientID) != -1:
for i in range(16):
erro, state, coord, detectedObjectHandle,
detectedSurfaceNormalVector = vrep.simxReadProximitySensor(clientID,
sensorHandle[i],vrep.simx_opmode_buffer)
if erro == 0:
dist = coord[2]
if state > 0 and dist < noDetectionDist:
if dist < maxDetectionDist:
dist = maxDetectionDist
detect[i] = 1-((dist-maxDetectionDist) /
(noDetectionDist-maxDetectionDist))
def prox_bool(cid, handle):
err, state, point, handle, normal = vrep.simxReadProximitySensor(cid, handle, vrep_mode)
return [1] if state else [0]
#retrieve motor handles
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)
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])
#for loop to retrieve sensor arrays and initiate sensors
for x in range(1,16+1):
errorCode,sensor_handle=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor'+str(x),vrep.simx_opmode_oneshot_wait)
sensor_h.append(sensor_handle) #keep list of handles
errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_handle,vrep.simx_opmode_streaming)
sensor_val=np.append(sensor_val,np.linalg.norm(detectedPoint)) #get list of values
t = time.time()
while (time.time()-t)<60:
#Loop Execution
sensor_val=np.array([])
for x in range(1,16+1):
errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_h[x-1],vrep.simx_opmode_buffer)
sensor_val=np.append(sensor_val,np.linalg.norm(detectedPoint)) #get list of values
#controller specific
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 ('Failing connecting remote API')
sys.exit('Could not connect')
#Handle creation for using the V-REP simulator
errorCode,left_motor_handle=vrep.simxGetObjectHandle(clientID,'motor_rueda_der',vrep.simx_opmode_oneshot_wait)
errorCode,right_motor_handle=vrep.simxGetObjectHandle(clientID,'motor_rueda_izq',vrep.simx_opmode_oneshot_wait)
#Creacion de handles para los ultrazonidos y primera lectura del valor
errorCode,ult_handle1=vrep.simxGetObjectHandle(clientID,'ultrasonido1',vrep.simx_opmode_oneshot_wait)
errorCode,detectionState,detectedPoint1,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,ult_handle1,vrep.simx_opmode_streaming)
u1_val = np.linalg.norm(detectedPoint1)
errorCode,ult_handle2=vrep.simxGetObjectHandle(clientID,'ultrasonido2',vrep.simx_opmode_oneshot_wait)
errorCode,detectionState,detectedPoint2,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,ult_handle2,vrep.simx_opmode_streaming)
u2_val = np.linalg.norm(detectedPoint2)
errorCode,ult_handle3=vrep.simxGetObjectHandle(clientID,'ultrasonido3',vrep.simx_opmode_oneshot_wait)
errorCode,detectionState,detectedPoint3,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,ult_handle3,vrep.simx_opmode_streaming)
u3_val = np.linalg.norm(detectedPoint3)
#Avoid Obstacle algorithm simulation
#-------------------------------------------------------------------------#
#Initialize the robot class
movil = gtg.Robot()
#Initialize the sensor class
def getSensor(self, name, typeSensor):
"""
Get data of robot sensor
Different type of sensor:
-prox
-cam
"""
if typeSensor == "prox":
if name in self.handles.keys():
pass
else:
self.initHandle(name)
sensor_handle=self.handles[name]
errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(self.clientID,sensor_handle,vrep.simx_opmode_streaming)
#errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(self.clientID,sensor_handle,vrep.simx_opmode_buffer)
sensor_val = np.linalg.norm(detectedPoint)
#print(name)
#print("detectionState", detectionState)
#print("detectedPoint", detectedPoint)
#print("sensor val", sensor_val)
if detectionState==False:
sensor_val = 0
else:
sensor_val=0.0415-sensor_val
return sensor_val
elif typeSensor == "cam":
pass