def stepSimulation(): if client.runInSynchronousMode: currentStep = client.stepCounter sim.simxSynchronousTrigger(client.id) while client.stepCounter == currentStep: retCode, s = sim.simxGetIntegerSignal( client.id, client.intSignalName, sim.simx_opmode_buffer) if retCode == sim.simx_return_ok: client.stepCounter = s retCode, res, img = sim.simxGetVisionSensorImage( client.id, client.visionSensorHandle, 0, sim.simx_opmode_buffer) client.lastImageAcquisitionTime = sim.simxGetLastCmdTime( client.id) if retCode == sim.simx_return_ok: sim.simxSetVisionSensorImage( client.id, client.passiveVisionSensorHandle, img, 0, sim.simx_opmode_oneshot) else: retCode, res, img = sim.simxGetVisionSensorImage( client.id, client.visionSensorHandle, 0, sim.simx_opmode_buffer) if retCode == sim.simx_return_ok: imageSimTime = sim.simxGetLastCmdTime(client.id) if client.lastImageAcquisitionTime != imageSimTime: client.lastImageAcquisitionTime = imageSimTime sim.simxSetVisionSensorImage( client.id, client.passiveVisionSensorHandle, img, 0, sim.simx_opmode_oneshot)
def get_vision_sensor_image(): """ Purpose: --- This function should first get the handle of the Vision Sensor object from the scene. After that it should get the Vision Sensor's image array from the CoppeliaSim scene. Input Arguments: --- None Returns: --- `vision_sensor_image` : [ list ] the image array returned from the get vision sensor image remote API `image_resolution` : [ list ] the image resolution returned from the get vision sensor image remote API `return_code` : [ integer ] the return code generated from the remote API Example call: --- vision_sensor_image, image_resolution, return_code = get_vision_sensor_image() NOTE: This function will be automatically called by test_task_2a executable at regular intervals. """ global client_id vision_sensor_image = [] image_resolution = [] return_code = 0 ############## ADD YOUR CODE HERE ############## ret, handle = sim.simxGetObjectHandle(client_id, 'vision_sensor_1', sim.simx_opmode_blocking) #imageAcquisitionTime=sim.simxGetLastCmdTime(client_id) #print(imageAcquisitionTime) return_code, image_resolution, vision_sensor_image = sim.simxGetVisionSensorImage( client_id, handle, 0, sim.simx_opmode_streaming) while (return_code != 0): return_code, image_resolution, vision_sensor_image = sim.simxGetVisionSensorImage( client_id, handle, 0, sim.simx_opmode_buffer) #if(return_code==0):break #print(vision_sensor_image) temp = np.array(vision_sensor_image) while (np.all(temp == 0)): return_code, image_resolution, vision_sensor_image = sim.simxGetVisionSensorImage( client_id, handle, 0, sim.simx_opmode_buffer) temp = np.array(vision_sensor_image) imageAcquisitionTime = sim.simxGetLastCmdTime(client_id) ################################################## return vision_sensor_image, image_resolution, return_code
def simtime(self): return sim.simxGetLastCmdTime(self.client_id)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) baseName = 'Body' jointName = 'Tran' _, baseHandle = vrep.simxGetObjectHandle(clientID, baseName, vrep.simx_opmode_blocking) _, jointHandle = vrep.simxGetObjectHandle(clientID, jointName, vrep.simx_opmode_blocking) print('Handles available!') # _, base_pos = vrep.simxGetObjectPosition(clientID, baseHandle, -1, vrep.simx_opmode_streaming) # _, jointConfig = vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming) lastCmdTime = vrep.simxGetLastCmdTime(clientID) # 记录当前时间 # vrep.simxSynchronousTrigger(clientID) # 让仿真走一步 N = 20 dT = 0.1 controller = HeightControllerMPC(N, dT) p_target = [1, 0] x_init = [0, 0] u_init = [0] #用于测试的计数 index = 0 # 开始仿真 while vrep.simxGetConnectionId(clientID) != -1:
_,MotorHandle_Left = sim.simxGetObjectHandle(clientID,'Revolute_left',sim.simx_opmode_blocking) _,MotorHandle_Right = sim.simxGetObjectHandle(clientID,'Revolute_right',sim.simx_opmode_blocking) _,TCP = sim.simxGetObjectHandle(clientID,'TCP',sim.simx_opmode_blocking) #_, baseHandle = sim.simxGetObjectHandle(clientID, 'CarBody', sim.simx_opmode_blocking) print('Handles Found!') # read the initial value of joints jointConfig = np.zeros((jointNum,)) for i in range(jointNum): _, jpos = sim.simxGetJointPosition(clientID, jointHandle[i], sim.simx_opmode_streaming) jointConfig[i] = jpos lastCmdTime=sim.simxGetLastCmdTime(clientID) # record the current time sim.simxSynchronousTrigger(clientID) #iii = 0 #loop index #increase_flag = 0 # trigger the sim while sim.simxGetConnectionId(clientID) != -1: currCmdTime=sim.simxGetLastCmdTime(clientID) dt = currCmdTime - lastCmdTime # delta time # read the current status of joints for every step for i in range(jointNum): _, jpos = sim.simxGetJointPosition(clientID, jointHandle[i], sim.simx_opmode_buffer) # print(round(jpos * 180 / math.pi, 2)) jointConfig[i] = jpos
clientID, "Pioneer_p3dx_rightMotor", sim.simx_opmode_blocking) returnCode = sim.simxSetJointTargetVelocity(clientID, leftWheel, 0, sim.simx_opmode_oneshot) returnCode = sim.simxSetJointTargetVelocity(clientID, rightWheel, 0, sim.simx_opmode_oneshot) # start simulation and initialize vision sensor status = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) #^ 改为不断查询直至获得图像 returnCode = 1 while returnCode != sim.simx_return_ok: returnCode, resolution, image = sim.simxGetVisionSensorImage( clientID, visionSensorHandle, 1, sim.simx_opmode_streaming) timeStart = time.time() last_time = sim.simxGetLastCmdTime(clientID) idx = 0 #^ 改为同步模式 sim.simxSynchronous(clientID, 1) try: while True: returnCode, resolution, image = sim.simxGetVisionSensorImage( clientID, visionSensorHandle, 1, sim.simx_opmode_buffer) image = np.flipud( np.array(image, dtype=np.uint8).reshape( [resolution[1], resolution[0]])) # if idx<=100: # plt.imsave(f'full{idx}.jpg',np.stack([image]*3, axis=2)) # idx+=1 cropImage = crop(image, mainSizeX, mainSizeY) #^ 大scale就是原图像下半部分
def getSimTimeMs(clientID): return sim.simxGetLastCmdTime(clientID)
backUntilTime = -1 # Forward or back mode # start time t = time.time() while (time.time() - t) < 45: # Update odometers # odometer.update_motors() # print(f"{odometer}") # Loop Execution resCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector \ = sim.simxReadProximitySensor(clientID, noseSensor, op_mode) result = np.linalg.norm(detectedPoint) if result > 0: backUntilTime = sim.simxGetLastCmdTime(clientID) + 4 # read the line detection sensors: sensorReading = [0, 0, 0] sensorBool = [False, False, False] for i, vision_handle in enumerate(floorSensorHandles): result, detection_state, aux_packets = sim.simxReadVisionSensor( clientID, vision_handle, op_mode) print( f"result: {result}, ds: {detection_state}, packets: {aux_packets}") if result >= 0: sensorReading[i] = aux_packets[0][10] sensorBool[i] = aux_packets[0][ 10] < 0.3 # data[11] is the average of intensity of the image # print(data[11])