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])
