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