def getBall(): dt = 0.001 camera.setCameraPosition(0, 0) while True: ts = vrep.simxGetLastCmdTime(clientID) camera.getCameraImage() sign, x, y, area = camera.trackObject('blue') cameraAreaControl.control(area) cameraPosControl.control(y) if (sign): U1 = cameraPosControl.U U2 = cameraAreaControl.U #print(y) else: U1 = 0.02 U2 = 0 car.move(-U2 - U1, -U2 + U1, 0, dt) #print(area) if (area > 6700 and y < 126 and y > 122): car.stop() gripper.giveBall() break camera.showStream() dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
def colibrate(): camera.setCameraPosition(30, 0.8) dt = 0.001 while True: ts = vrep.simxGetLastCmdTime(clientID) camera.getCameraImage() sign, x, y, area = camera.trackObject('red') cameraAreaControl.control(area) cameraPosControl.control(y) if (sign): U1 = cameraPosControl.U U2 = cameraAreaControl.U else: U1 = 0.02 U2 = 0 car.move(-2 * U2 - U1, -2 * U2 + U1, 0, dt) #print(area) if (y < 128 and y > 122): car.stop() camera.destroyAllStream() break camera.showStream() dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
def send_gear_commands(clientID, target_degree1, target_degree2, gearHandle1, gearHandle2): RAD = 180 / math.pi lastCmdTime = vrep.simxGetLastCmdTime(clientID) vrep.simxSynchronousTrigger(clientID) count = 0 while vrep.simxGetConnectionId(clientID) != -1: count += 1 if count > 20: break currCmdTime = vrep.simxGetLastCmdTime(clientID) dt = currCmdTime - lastCmdTime ret, gear_pos1 = vrep.simxGetJointPosition(clientID, gearHandle1, vrep.simx_opmode_buffer) ret, gear_pos2 = vrep.simxGetJointPosition(clientID, gearHandle2, vrep.simx_opmode_buffer) degree1 = round(gear_pos1 * RAD, 2) degree2 = round(gear_pos2 * RAD, 2) print(degree1, degree2) if abs(degree1 - target_degree1) < 0.5 and abs(degree2 - target_degree2) < 0.5: break vrep.simxPauseCommunication(clientID, True) vrep.simxSetJointTargetPosition(clientID, gearHandle1, target_degree1 / RAD, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, gearHandle2, target_degree2 / RAD, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) lastCmdTime = currCmdTime vrep.simxSynchronousTrigger(clientID)
def searchDir(color): camera.setCameraPosition(30, 0.8) dt = 0.001 while True: ts = vrep.simxGetLastCmdTime(clientID) camera.getCameraImage() sign, x, y, area = camera.trackObject(color) cameraAreaControl.control(area) cameraPosControl.control(y) if (sign): U1 = cameraPosControl.U U2 = cameraAreaControl.U else: U1 = 0.02 U2 = 0 car.move(-U2 - U1, -U2 + U1, 0, dt) #print(area) if (area > 1000 and y < 128 and y > 122): car.move(-velocity, -velocity, 0, dt) break camera.showStream() dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
def readPositionSensors(self): """ get the position of robot TODO """ # get position error, position_hexa_base = vrep.simxGetObjectPosition( clientID, self.positionSensors["body"], -1, vrep.simx_opmode_blocking) time = vrep.simxGetLastCmdTime(clientID) if error == 0: self.XYZdata('bodyPosition', position_hexa_base, time) else: print("Position sensor read error: %d", error) # get orientation error, orientation_hexa_base = vrep.simxGetObjectOrientation( clientID, self.positionSensors["body"], -1, vrep.simx_opmode_blocking) time = vrep.simxGetLastCmdTime(clientID) if error == 0: self.XYZdata('bodyOrientation', orientation_hexa_base, time) else: print("Orientation sensor read error: %d", error) ############################################################################ error, position_hexa = vrep.simxGetObjectPosition( clientID, self.positionSensors["gyro"], -1, vrep.simx_opmode_blocking) time = vrep.simxGetLastCmdTime(clientID) if error == 0: self.XYZdata('gyro', position_hexa, time) else: print("Gyro sensor read error: %d", error)
def moveDir(vel, pos): dt = 0 edgeS = False #машинка на склоне edgeS2 = False #машинка проехала склон position = 1 while (position < pos): ts = vrep.simxGetLastCmdTime(clientID) camera.getCameraImage() sign, x, y, area = camera.trackObject('red') cameraPosControl.control(y) if area > 120 and area < 140: position = 3 elif area > 180 and area < 200: position = 4 elif area > 260 and area < 300: position = 5 elif area > 450 and area < 550: position = 6 elif area > 1300 and area < 1800: position = 7 elif area > 2400 and area < 3000: position = 8 if (sign): U1 = cameraPosControl.U else: U1 = 0.02 car.move(-U1 - vel, +U1 - vel, 0, dt) print(area, position) #if(position>=pos and y<128 and y>122): # break #sens.updateAllSensors() #regulator.pid(sens.lsVal-0.4,dt) #imu.upgrateIMU() #imuFilter.filter(imu.accelData,imu.gyroData,dt) #if(imuFilter.roll>0.2 and not lineSensors.getGreenColorSignal()): # U=0 # if not edgeS2 and not edgeS: # edgeS=True #else: # #U=regulator.U # U=0 #if(imuFilter.roll<0.2 and edgeS): # edgeS2=True # car.position=0 # edgeS=False #if edgeS: # car.position=0 #car.move(-vel-U,-vel+U,0,dt) dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000 car.stop() car.position = 0
def main(): ur5 = UR5() ur5.connect() ur5.ankleinit() clientID = ur5.get_clientID() camera = Camera(clientID) lastCmdTime = vrep.simxGetLastCmdTime(clientID) vrep.simxSynchronousTrigger(clientID) count = 0 while vrep.simxGetConnectionId(clientID) != -1 and count < Simu_STEP: currCmdTime = vrep.simxGetLastCmdTime(clientID) dt = currCmdTime - lastCmdTime # Camera achieve data cur_depth, cur_color = camera.get_camera_data() cur_depth_path, cur_img_path = camera.save_image( cur_depth, cur_color, currCmdTime) # Call affordance map function affordance_cmd = 'th ' + Lua_PATH + ' -imgColorPath ' + cur_img_path + ' -imgDepthPath ' + cur_depth_path + ' -resultPath ' + Save_PATH_RES + str( currCmdTime) + '_results.h5' try: os.system(affordance_cmd) print('-- Successfully create affordance map!') except: print( '!!!!!!!!!!!!!!!!!!!!!!!! Error occurred during creating affordance map' ) hdf2affimg(Save_PATH_RES + str(currCmdTime) + '_results.h5', currCmdTime) # TODO: get the push u,v coordinate u = 256 v = 212 camera_coor = pixel2camera(u, v, cur_depth, camera.intri, camera.Dis_FAR) _, ur5_position = vrep.simxGetObjectPosition( clientID, ur5.get_handle(), -1, vrep.simx_opmode_oneshot_wait) location = camera2ur5(camera_coor, ur5_position, camera.cam_position) # Move ur5 # location = random.randint(100, 200, (1, 3)) # location = location[0] / 400 print("UR5 Move to %s" % (location)) ur5.ur5moveto(location[0], location[1], location[2] - 0.05) count += 1 lastCmdTime = currCmdTime vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) ur5.disconnect()
def loop(self): lastCmdTime = vrep.simxGetLastCmdTime(self.clientID) / 1000 # 记录当前时间 vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步 t = 0 while t < np.pi * 2: currCmdTime = vrep.simxGetLastCmdTime(self.clientID) / 1000 dt = currCmdTime - lastCmdTime t = t + dt vrep.simxPauseCommunication(self.clientID, True) vrep.simxSetJointTargetPosition(self.clientID, self.robot1_jointHandle[0], np.sin(t), vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(self.clientID, self.robot1_jointHandle[1], 0, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(self.clientID, self.robot1_jointHandle[2], 0, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(self.clientID, False) lastCmdTime = currCmdTime vrep.simxSynchronousTrigger(self.clientID) vrep.simxGetPingTime(self.clientID) time.sleep(2) t = 0 while t < np.pi * 2: currCmdTime = vrep.simxGetLastCmdTime(self.clientID) / 1000 dt = currCmdTime - lastCmdTime t = t + dt vrep.simxPauseCommunication(self.clientID, True) vrep.simxSetJointTargetPosition(self.clientID, self.robot1_jointHandle[0], 0, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(self.clientID, self.robot1_jointHandle[1], np.sin(t), vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(self.clientID, self.robot1_jointHandle[2], 0, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(self.clientID, False) lastCmdTime = currCmdTime vrep.simxSynchronousTrigger(self.clientID) vrep.simxGetPingTime(self.clientID)
def get_sim_time(self): """ Gets the simulation time. Returns zero if the simulation is stopped. :return: simulation time in milliseconds. """ self.wait_for_ping() return vrep.simxGetLastCmdTime(self._clientID)
def fetchKinect(clientID,depthSTR,rgbSTR): errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,rgbSTR,vrep.simx_opmode_blocking) #errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_blocking) #errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_blocking) # image_byte_array = array.array('b', image) # image_buffer = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "BGR", 0, 1) # image_buffer = image_buffer.transpose(Image.FLIP_LEFT_RIGHT) # image_buffer = image_buffer.transpose(Image.ROTATE_180) # img2 = np.asarray(image_buffer) img_time=vrep.simxGetLastCmdTime(clientID) # img,imgArr=Raftaar.ProcessImage(image,resolution) # rgbArr=np.array(imgArr) #print(len(image)) #print(image.shape) #errorHere,resol,depth=vrep.simxGetVisionSensorDepthBuffer(clientID,kinectDepth,vrep.simx_opmode_oneshot_wait) #depthArr=np.array(depth) #print(image) #return image,img_time return img_time
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 main(): ur5 = UR5() ur5.connect() clientID = ur5.get_clientID() camera = Camera(clientID) lastCmdTime = vrep.simxGetLastCmdTime(clientID) vrep.simxSynchronousTrigger(clientID) count = 0 while vrep.simxGetConnectionId(clientID) != -1 and count < Simu_STEP: currCmdTime = vrep.simxGetLastCmdTime(clientID) dt = currCmdTime - lastCmdTime # Camera achieve data cur_depth, cur_color = camera.get_camera_data() cur_depth_path, cur_img_path = camera.save_image( cur_depth, cur_color, currCmdTime) # Call affordance map function affordance_cmd = 'th ' + Lua_PATH + ' -imgColorPath ' + cur_img_path + ' -imgDepthPath ' + cur_depth_path + ' -resultPath ' + Save_PATH_RES + str( currCmdTime) + '_results.h5' try: os.system(affordance_cmd) print('-- Successfully create affordance map!') except: print( '!!!!!!!!!!!!!!!!!!!!!!!! Error occurred during creating affordance map' ) hdf2affimg(Save_PATH_RES + str(currCmdTime) + '_results.h5', currCmdTime) # Move ur5 # TODO: location = random.randint(100, 200, (1, 3)) location = location[0] / 400 print("UR5 Move to %s" % (location)) ur5.ur5moveto(location[0], location[1], location[2]) count += 1 lastCmdTime = currCmdTime vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) ur5.disconnect()
def moveWhile(color): dt = 0.001 lineSensors.updateLineSensors() lineSensors.getBlackColorSignal() lineSensors.getGreenColorSignal() signal = False while (not signal): ts = vrep.simxGetLastCmdTime(clientID) lineSensors.getBlackColorSignal() lineSensors.getGreenColorSignal() if color == 'green': signal = lineSensors.greenSignal elif color == 'black': signal = lineSensors.blackSignal #print(lineSensors.blackSignal ) car.move(-velocity, -velocity, 0, dt) dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000 time.sleep(2) car.stop()
def readForceSensors(self): for sensor in self.forceSensors.keys(): error, state, forceVector, torqueVector = vrep.simxReadForceSensor( self.clientID, self.forceSensors[sensor], vrep.simx_opmode_blocking) time = vrep.simxGetLastCmdTime(clientID) if error == 0: self.XYZdata(sensor + 'force', forceVector, time) self.XYZdata(sensor + 'torque', torqueVector, time) else: print("Force sensor read error: %d", error)
def reset(self): self.get_point = False self.grab_counter = 0 self.get_obstacles = False self.obstacles_counter = 0 self.currCmdTime = vrep.simxGetLastCmdTime(self.clientID) # dt = (self.currCmdTime - self.lastCmdTime) / 1000 self.moveto(self.config1, self.config2) vrep.simxPauseCommunication(self.clientID, False) self.lastCmdTime = self.currCmdTime vrep.simxSynchronousTrigger(self.clientID) vrep.simxGetPingTime(self.clientID) s = self.getState_v1() return s
def controller(self): self.LCycleFreq = self.BaseFreq self.RCycleFreq = self.BaseFreq error = vrep.simxSetFloatSignal(self.clientID, self.LSignalName, self.LCycleFreq, vrep.simx_opmode_oneshot) error = error or vrep.simxSetFloatSignal( self.clientID, self.RSignalName, self.RCycleFreq, vrep.simx_opmode_oneshot) if error != 0: print("Function error: ", error) time = vrep.simxGetLastCmdTime(clientID) self.telemetryData['cycleFrequencyTime'] = time self.telemetryData['leftCycleFrequency'] = self.LCycleFreq self.telemetryData['rightCycleFrequency'] = self.RCycleFreq
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 init(): global robot global blockHandleArray, connectionTime print('Program started') vrep.simxFinish(-1) # just in case, close all opened connections int_portNb = 19999 # define port_nr clientID = vrep.simxStart('127.0.0.1', int_portNb, True, True, 5000, 5) # connect to server if clientID != -1: print('Connected to remote API server') res,objs = vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshot_wait) # get all objects in the scene if res == vrep.simx_return_ok: # Remote function call succeeded print('Number of objects in the scene: ',len(objs))# print number of object in the scene ret_lm, leftMotorHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait) ret_rm, rightMotorHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait) ret_pr, pioneerRobotHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait) ret_sl, ultraSonicSensorLeft = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor4',vrep.simx_opmode_oneshot_wait) ret_sr, ultraSonicSensorRight = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor5',vrep.simx_opmode_oneshot_wait) blockHandleArray = [] for i_block in range(12): blockName = 'ConcretBlock#'+str(i_block) retCode, handle = vrep.simxGetObjectHandle(clientID, blockName, vrep.simx_opmode_oneshot_wait) assert retCode==0, retCode if i_block>6: rand_loc = [random.random()*6-1.5, random.random()*7-2.5, 0.0537] # x[-1.5,4.5] y[-2.5,-4.5] vrep.simxSetObjectPosition(clientID, handle, -1, rand_loc, vrep.simx_opmode_oneshot_wait) retCode,position = vrep.simxGetObjectPosition(clientID, handle, -1, vrep.simx_opmode_oneshot_wait) assert retCode==0, retCode blockHandleArray.append([handle,i_block,position]) robot = EasyDict(clientID=clientID, leftMotorHandle=leftMotorHandle, rightMotorHandle=rightMotorHandle, pioneerRobotHandle=pioneerRobotHandle, ultraSonicSensorLeft=ultraSonicSensorLeft, ultraSonicSensorRight=ultraSonicSensorRight, energySensor=None) connectionTime = vrep.simxGetLastCmdTime(robot.clientID) return robot else: print('Remote API function call returned with error code: ',res) vrep.simxFinish(clientID) # close all opened connections else: print('Failed connecting to remote API server') print('Program finished') return {}
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 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)
_, quadricopter_target_handle = vrep.simxGetObjectHandle( clientID, "Quadricopter_target", vrep.simx_opmode_blocking) _, quadricopter_handle = vrep.simxGetObjectHandle(clientID, "Quadricopter_base", vrep.simx_opmode_blocking) _, v1 = vrep.simxGetObjectHandle(clientID, "zed_vision0", vrep.simx_opmode_blocking) print('Handles available!') _, jpos = vrep.simxGetObjectPosition(clientID, quadricopter_target_handle, -1, vrep.simx_opmode_streaming) _, _, imageBuffer = vrep.simxGetVisionSensorImage(clientID, v1, -1, vrep.simx_opmode_streaming) lastCmdTime = vrep.simxGetLastCmdTime(clientID) # 記錄當前時間 vrep.simxSynchronousTrigger(clientID) # 讓模擬走一步 # 開始模擬 while vrep.simxGetConnectionId(clientID) != -1: currCmdTime = vrep.simxGetLastCmdTime(clientID) # 記錄當前時間 dt = currCmdTime - lastCmdTime # 記錄時間間隔,用於控制 #------------------------------------------- _, jpos = vrep.simxGetObjectPosition(clientID, quadricopter_target_handle, -1, vrep.simx_opmode_buffer) _, shape, imageBuffer = vrep.simxGetVisionSensorImage( clientID, v1, -1, vrep.simx_opmode_streaming) print(np.size(imageBuffer)) print(type(imageBuffer)) img = Image.fromarray(np.resize(np.array(imageBuffer), (720, 1280)), mode='L')
print("Handles avaliable!") # 然后首次读取关节的初始值,以streaming的形式 for i in range(0, jointNum - 1): _, jpos = vrep.simxGetJointPosition(clientID, vJointHandle[i], vrep.simx_opmode_streaming) vJointPositionSensor[i] = jpos for i in range(0, jointNum - 1): _, jpos = vrep.simxGetJointPosition(clientID, hJointHandle[i], vrep.simx_opmode_streaming) hJointPositionSensor[i] = jpos # Simulation lastCmdTime = vrep.simxGetLastCmdTime(clientID) # 记录当前时间 vrep.simxSynchronousTrigger(clientID) # 让仿真走一步 # 开始仿真 steps = 0 while vrep.simxGetConnectionId(clientID) != -1 and steps <= 2000: currCmdTime = vrep.simxGetLastCmdTime(clientID) # 记录当前时间 # strCmdTime = currCmdTime # 记录初始时间 dt = currCmdTime - lastCmdTime # 记录时间间隔,用于控制 # ---控制部分 # 读取当前的状态值,之后都用buffer形式读取 # 读取关节角(传感器返回值) for i in range(0, jointNum - 1): _, jpos = vrep.simxGetJointPosition(clientID, vJointHandle[i], vrep.simx_opmode_buffer)
def get_sim_time(self) -> float: """Returns the current simulation time in seconds. """ return vrep.simxGetLastCmdTime(self.vr.client_id) / 1000.0
Q = 0.001 Rgps = np.array([[sqrt(0.5)], [sqrt(0.5)], [sqrt(0.0001)]]) R = np.zeros((3, 3)) np.fill_diagonal(R, Rgps) car_EKF = dan_lib.EKF(vehicleWidth=width, initialStates=initialStates, probMatrix=probMatrix, Q=Q) #print(initialStates) try: global cur aux = None mean_time = [] vmax = 6 aux_m = 1 start = vrep.simxGetLastCmdTime(clientID) start_pred = start tempo = time.process_time() nL = 0 nR = 0 still_neg_R = 0 still_neg_L = 0 auxR = 0 auxL = 0 iniR = 0 iniL = 0 index_pred = 0 while (True): # Read sensor values for prior analysis
# Orientation Get errorCode, ori = vrep.simxGetObjectOrientation( clientID, car_handle, -1, vrep.simx_opmode_streaming) # Velocity errorCode, LinearV, AngularV = vrep.simxGetObjectVelocity( clientID, car_handle, vrep.simx_opmode_streaming if i is 1 else vrep.simx_opmode_buffer) # Distance errorCode, d = vrep.simxReadDistance(clientID, dist_handle, vrep.simx_opmode_streaming) # Simulation Time if t != vrep.simxGetLastCmdTime(clientID): t = vrep.simxGetLastCmdTime(clientID) if not i % 100: print(i) for col, data in enumerate([ pos[0], pos[1], pos[2], gyroX, gyroY, gyroZ, accelX, accelY, accelZ, ori[0], ori[1], ori[2], LinearV[0], LinearV[1], LinearV[2], t, d ]): worksheet.write(i, col, data) else: i -= 1 #vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) sleep(0.01)
xx = [] zz = [] z_vel, x_vel = input("velocity in Z and X: ") time1 = input("Time of flight: ") vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_z, z_vel, vrep.simx_opmode_blocking) vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_x, x_vel, vrep.simx_opmode_blocking) vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking) t_end = time.time() + time1 #while time.time() < t_end: for i in range(0,10): errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,'kinect_rgb',vrep.simx_opmode_blocking) #errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_blocking) img_time=vrep.simxGetLastCmdTime(clientID) #errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_blocking) errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_oneshot_wait) image_byte_array = array.array('b', image) image_buffer = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "BGR", 0, 1) image_buffer = image_buffer.transpose(Image.FLIP_LEFT_RIGHT) image_buffer = image_buffer.transpose(Image.ROTATE_180) img2 = np.asarray(image_buffer) img_time=vrep.simxGetLastCmdTime(clientID) #print(img_time) #cv2.imwrite(str(img_time)+'.png',img2)
pose_list = [] xx = [] zz = [] #######################################################Setting Velocity############################################################################### z_vel, x_vel, y_vel = 0.8, -5, -0.3 #z_vel, x_vel, y_vel = 0,0,0 vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_z, z_vel, vrep.simx_opmode_blocking) vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_x, x_vel, vrep.simx_opmode_blocking) # vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_y, y_vel, vrep.simx_opmode_blocking) vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking) for i in range(0,10): time1=vrep.simxGetLastCmdTime(clientID) pose = getPosition(clientID) pose_list.append(pose) if len(pose_list) > 0 & len(pose_list) <4: xyz = pose[1] xx.append(xyz[0]) zz.append(xyz[2]) if len(pose_list) >3: a,b,c = calc_parabola_vertex(xx[0], zz[0], xx[1], zz[1], xx[2], zz[2]) z_req= (a*(x_req**2))+(b*x_req)+c # 2D parabolic equation : z = a*x^2 + b* x + c if z_req < 0 : z_req = 0 print('Position from sim handle'+str(z_req))
vrep.simx_opmode_streaming) #print("Position : {:.5f} {:.5f} {:.5f}\n".format(pos[0], pos[1], pos[2])) # Orientation Get #errorCode, ori = vrep.simxGetObjectOrientation(clientID, car_handle, -1, vrep.simx_opmode_streaming) # Velocity errorCode, LinearV, AngularV = vrep.simxGetObjectVelocity( clientID, car_handle, vrep.simx_opmode_buffer) # Not Started if not sum(pos): continue # Simulation Time if time != vrep.simxGetLastCmdTime(clientID): time_pre = time time = vrep.simxGetLastCmdTime(clientID) else: continue # First Step if not ok: ok = True Previous_position = [pos[0], pos[1], pos[2]] Predict_position = Previous_position[:] time_pre = time continue t = (time - time_pre) / 1000 Predict_position = [
network = NN(ni,nj,nk) vrep.simxStartSimulation(client_id, vrep.simx_opmode_oneshot_wait) position = position_init # [x,y,theta] network_input = [0, 0, 0] for i in range(ni): network_input[i] = position[i]-target[i] prev_error = 100000 #while(distance(position, target)>0.001 or position[2]-target[2]>0.1): while(True): #current_time = time.time() current_time = vrep.simxGetLastCmdTime(client_id) command = network.runNN(network_input) vrep.simxSetJointTargetVelocity(client_id, left_motor, 2*command[0], vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetVelocity(client_id, right_motor, 2*command[1], vrep.simx_opmode_oneshot_wait) # wait for velocity stabilization (??ms) time.sleep(0.050) # calcul prochain input res, tmp = vrep.simxGetObjectPosition(client_id, pioneer, -1, vrep.simx_opmode_oneshot_wait) position[0] = tmp[0] position[1] = tmp[1] res, tmp = vrep.simxGetObjectOrientation(client_id, pioneer, -1, vrep.simx_opmode_oneshot_wait) position[2] = tmp[2] # en radian
vrep.simx_opmode_blocking) jointHandle[i] = returnHandle _, baseHandle = vrep.simxGetObjectHandle(clientID, baseName, vrep.simx_opmode_blocking) print('Handles available!') jointConfig = np.zeros((jointNum, )) for i in range(jointNum): _, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i], vrep.simx_opmode_streaming) jointConfig[i] = jpos # print('Handle avaliable') # simulation lastCmdTime = vrep.simxGetLastCmdTime(clientID) vrep.simxSynchronousTrigger(clientID) print('control') target_pos = np.array([80, 160, 80, 30, 20, 80]) while vrep.simxGetConnectionId(clientID) != -1: currCmdTime = vrep.simxGetLastCmdTime(clientID) dt = currCmdTime - lastCmdTime for i in range(jointNum): _, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i], vrep.simx_opmode_buffer) print('position', round(jpos * RAD2DEG, 2)) jointConfig[i] = jpos vrep.simxPauseCommunication(clientID, True)
def start(clientID, quads, targets, speed, proxs, path, endpos, leadfoll=False): """ Boids model main program :param clientID: ID of the VRep connection :param quads: quadrotor handles :param targets: quadrotor target handles :param speed: speed of quadrotors :param proxs: proximity sensor handles :param path: quadrotor path coordinates :param endpos: ending position of quadrotors :param leadfoll: True - leader/followers mode, False - all boids following path (default) """ # definition of constants quadsNum = len(quads) # number of quadrotors viewRange = 3 # view range of quadrotors smp = 0.2 # sampling period kS = [0.30, 2.0] # separation constants [multiplication const, power const] kC = [0.30, 0.0] # cohesion constants [multiplication const, power const] kA = [0.00, 0.0] # alignment constants [multiplication const, power const] kD = [speed, 1.0] # path following constants [multiplication const, power const] kO = [0.20, 2.0 ] # obstacle avoidance constants [multiplication const, power const] # data stream init for i in range(quadsNum): vrep.simxGetObjectPosition(clientID, quads[i], -1, vrep.simx_opmode_streaming) vrep.simxGetObjectVelocity(clientID, quads[i], vrep.simx_opmode_streaming) vrep.simxReadProximitySensor(clientID, proxs[i], vrep.simx_opmode_streaming) # variables init position = [[0 for _ in range(3)] for _ in range(quadsNum)] # position of quadrotors velocity = [[0 for _ in range(3)] for _ in range(quadsNum)] # velocity of quadrotors closest = [[0 for _ in range(3)] for _ in range(quadsNum)] # coords of closest obstacle to quads visibleQuads = [[0 for _ in range(quadsNum)] for _ in range(quadsNum)] # visible quadrotors individualTarget = [ 0 ] * quadsNum # current waypoint index for each quadrotor # get closest boid to starting point leader = 0 _, tmp = vrep.simxGetObjectPosition(clientID, quads[0], -1, vrep.simx_opmode_buffer) dist = ut.norm(ut.sub(path[1], tmp)) for i in range(1, quadsNum): _, tmp = vrep.simxGetObjectPosition(clientID, quads[i], -1, vrep.simx_opmode_buffer) nrm = ut.norm(ut.sub(path[1], tmp)) if nrm < dist: dist = nrm leader = i # main boids program t1 = 0 t2 = 0.0 finished = [False] * quadsNum count = 0 counting = False file = open('data.txt', 'wt', encoding='utf-8') while vrep.simxGetConnectionId(clientID) != -1: time.sleep(smp) separation = [[0 for _ in range(3)] for _ in range(quadsNum)] # separation force cohesion = [[0 for _ in range(3)] for _ in range(quadsNum)] # cohesion force alignment = [[0 for _ in range(3)] for _ in range(quadsNum)] # alignment force destination = [[0 for _ in range(3)] for _ in range(quadsNum)] # path following force avoidance = [[0 for _ in range(3)] for _ in range(quadsNum)] # obstacle avoidance force output = [[0 for _ in range(3)] for _ in range(quadsNum)] # output force # check if all quads finished if counting: if count >= 0: file.close() return (t2 - t1) / 1000 count += 1 else: for i in range(quadsNum): nrm = ut.norm(ut.sub(position[i][0:2], path[-1][0:2])) if nrm < 2 and not finished[i]: finished[i] = True print('Quad #' + str(i) + ' finished in ' + str((vrep.simxGetLastCmdTime(clientID) - t1) / 1000) + 's') if (leadfoll and finished[leader]) or ( not leadfoll and all(_ for _ in finished)): counting = True t2 = vrep.simxGetLastCmdTime(clientID) if endpos is None: file.close() return (t2 - t1) / 1000 # read data from VRep for i in range(quadsNum): _, position[i] = vrep.simxGetObjectPosition( clientID, quads[i], -1, vrep.simx_opmode_buffer) _, velocity[i], _ = vrep.simxGetObjectVelocity( clientID, quads[i], vrep.simx_opmode_buffer) _, res, closest[i], _, _ = vrep.simxReadProximitySensor( clientID, proxs[i], vrep.simx_opmode_buffer) if not res: closest[i] = [0, 0, 0] closest[i][2] = 0 # write into data file ct = vrep.simxGetLastCmdTime(clientID) file.write(str(ct)) for i in range(quadsNum): file.write(' ' + str(position[i][0]) + ' ' + str(position[i][1]) + ' ' + str(position[i][2])) file.write(' ' + str(velocity[i][0]) + ' ' + str(velocity[i][1]) + ' ' + str(velocity[i][2])) file.write(' ' + str(closest[i][0]) + ' ' + str(closest[i][1])) file.write('\n') # compute visible quadrotors for i in range(quadsNum): for j in range(quadsNum): if i != j: temp = ut.sub(position[i], position[j]) if ut.norm(temp) < viewRange: visibleQuads[i][j] = 1 else: visibleQuads[i][j] = 0 for i in range(quadsNum): # compute separation force for j in range(quadsNum): if i != j and visibleQuads[i][j] == 1: temp = ut.sub(position[i], position[j]) nrm = ut.norm(temp) if nrm != 0: temp = ut.mul(temp, kS[0] / (nrm**kS[1])) separation[i] = ut.add(separation[i], temp) # compute cohesion and alignment forces center = [0, 0, 0] # center of the swarm if sum(visibleQuads[i]) != 0: for j in range(quadsNum): if i != j and visibleQuads[i][j] == 1: temp = ut.mul(position[j], 1 / sum(visibleQuads[i])) center = ut.add(center, temp) temp = ut.mul(velocity[j], 1 / sum(visibleQuads[i])) alignment[i] = ut.add(alignment[i], temp) cohesion[i] = ut.sub(center, position[i]) nrm = ut.norm(cohesion[i]) if nrm != 0: cohesion[i] = ut.mul(cohesion[i], kC[0] / (nrm**kC[1])) nrm = ut.norm(alignment[i]) if nrm != 0: alignment[i] = ut.mul(alignment[i], kA[0] / (nrm**kA[1])) # compute path following force if not leadfoll or i == leader or counting: if not counting: nrm = ut.norm( ut.sub(position[i][0:2], path[individualTarget[i]][0:2])) if individualTarget[i] != 0: vec1 = ut.sub(path[individualTarget[i] - 1], path[individualTarget[i]]) vec2 = ut.sub(position[i], path[individualTarget[i]]) if individualTarget[i] < len(path) - 1 and ( nrm <= 1 or ut.angle(vec1, vec2) >= math.pi / 2): individualTarget[i] += 1 # if individualTarget[i] == 2 and min(individualTarget) == 2: # print((vrep.simxGetLastCmdTime(clientID)-tt)/1000) else: vec1 = ut.sub(path[individualTarget[i] + 1], path[individualTarget[i]]) vec2 = ut.sub(position[i], path[individualTarget[i]]) if nrm <= 1 or ut.angle(vec1, vec2) <= math.pi / 2: individualTarget[i] += 1 if t1 == 0 and min(individualTarget) == 1: t1 = vrep.simxGetLastCmdTime(clientID) # tt = vrep.simxGetLastCmdTime(clientID) destination[i] = ut.sub(path[individualTarget[i]], position[i]) else: destination[i] = ut.sub(endpos[i], position[i]) nrm = ut.norm(destination[i]) if nrm != 0: if finished[i]: destination[i] = ut.mul(destination[i], 0.1) else: destination[i] = ut.mul(destination[i], kD[0] / (nrm**kD[1])) # compute output force without obstacle avoidance output[i] = separation[i] output[i] = ut.add(output[i], cohesion[i]) output[i] = ut.add(output[i], alignment[i]) output[i] = ut.add(output[i], destination[i]) # compute obstacle avoidance force # angle = ut.angle(closest[i], output[i]) # if angle > math.pi/2+0.3: # avoidance[i] = [0, 0, 0] # else: avoidance[i] = ut.sub([0, 0, 0], closest[i]) nrm = ut.norm(avoidance[i]) if nrm != 0: avoidance[i] = ut.mul(avoidance[i], kO[0] / (nrm**kO[1])) # compute output force output[i] = ut.add(output[i], avoidance[i]) if position[i][2] < 0.5: output[i][2] = 0.05 # export output to VRep for i in range(quadsNum): vrep.simxSetObjectPosition(clientID, targets[i], quads[i], output[i], vrep.simx_opmode_streaming)
time.sleep(5000.0 / 1000.0) sys.exit(0) vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', portNb, True, True, 2000, 5) if clientID != -1: print('Connected to remote API server') driveBackStartTime = -99000 motorSpeeds = [0, 0] while vrep.simxGetConnectionId(clientID) != -1: (errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector) = vrep.simxReadProximitySensor( clientID, sensorHandle, vrep.simx_opmode_streaming) if errorCode == vrep.simx_return_ok: simulationTime = vrep.simxGetLastCmdTime(clientID) if simulationTime - driveBackStartTime < 3000: motorSpeeds[0] = -3.1415 * 0.5 motorSpeeds[1] = -3.1415 * 0.25 else: motorSpeeds[0] = 3.1415 motorSpeeds[1] = 3.1415 if detectionState: driveBackStartTime = simulationTime errorCode = vrep.simxSetJointTargetVelocity( clientID, leftMotorHandle, motorSpeeds[0], vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, motorSpeeds[1], vrep.simx_opmode_oneshot)
def getSimulationTime(): vrep.simxGetPingTime(robot.clientID) return vrep.simxGetLastCmdTime(robot.clientID) - connectionTime
def labirintAlgorithm(): vrep.simxFinish(-1) clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) while clientID <= -1: clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if clientID != -1: print('Successful !!!') else: print('connection not successful') #sys.exit('could not connect') print('connected to remote api server') #vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot) imu = IMU(clientID) regulator = pidControl(0.008, 0.01, 0.002) car = Robot(clientID, 0.04) sens = ProxSensor(clientID) map = Map(clientID) lineSensors = IRSensors(clientID) map.getCameraImage() map.getMap() map.spreadRay() #lineSensors.getBlackColorSignal() i = 0 velocity = 0.1 dt = 0.0001 sens.updateAllSensors() time.sleep(2) U = 0 while True: timeStart = vrep.simxGetLastCmdTime(clientID) #lineSensors.getBlackColorSignal() sens.updateAllSensors() #if(sens.rsVal<0.6 and sens.rsVal>0.35) and (sens.lsVal<0.6 and sens.lsVal>0.35): # regulator.pid(sens.rsVal-sens.lsVal,dt) # U=regulator.U #else: # U=0 car.move(velocity + U, velocity - U, 0, dt) #if(i==len(map.carWay)-1): # time.sleep(4) # break #print(sens.drsVal) if (sens.drsVal < 0.4 and sens.drsVal > 0.3) and (sens.dlsVal < 0.4 and sens.dlsVal > 0.3): if (map.carWay[i] == 1): car.rotate(80) else: car.rotate(-80) sens.updateAllSensors() time.sleep(0.5) i += 1 dt = (vrep.simxGetLastCmdTime(clientID) - timeStart) / 1000 car.stop() time.sleep(1) vrep.simxFinish(clientID)