def __init__(self, ID):
self.clientID = ID
# get handles to robot drivers
err_code, self.left_front_handle = vrep.simxGetObjectHandle(
self.clientID, 'left_front', vrep.simx_opmode_blocking)
err_code, self.left_back_handle = vrep.simxGetObjectHandle(
self.clientID, 'left_back', vrep.simx_opmode_blocking)
err_code, self.right_back_handle = vrep.simxGetObjectHandle(
self.clientID, 'right_back', vrep.simx_opmode_blocking)
err_code, self.right_front_handle = vrep.simxGetObjectHandle(
self.clientID, 'right_front', vrep.simx_opmode_blocking)
self.side_handles = []
for l in 'rl':
for i in range(1, 7):
err_code, handle = vrep.simxGetObjectHandle(
self.clientID, 'sj_' + l + '_' + str(i),
vrep.simx_opmode_blocking)
self.side_handles.append(handle)
#initial velocity
self.leftvelocity = 0
self.rightvelocity = 0
self.MaxVel = 10
self.dVel = 1
def onConnect(self):
if self.clientID != -1:
return
self.clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
if self.clientID == -1:
messagebox.showinfo("No se puede conectar")
else:
for i in range(5):
j = i + 1
nom = self.base + str(j)
_, handle = sim.simxGetObjectHandle(self.clientID, nom,
sim.simx_opmode_blocking)
self.handles.append(handle)
_, self.j1 = sim.simxGetObjectHandle(self.clientID,
'youBotGripperJoint1',
sim.simx_opmode_blocking)
_, self.j2 = sim.simxGetObjectHandle(self.clientID,
'youBotGripperJoint2',
sim.simx_opmode_blocking)
print("Conectado y envio comando al motor")
print(self.j1, self.j2)
self.init_robot()
def create_dataset(path):
try:
shutil.rmtree(path + "/mav0")
except:
print("Removed before")
os.makedirs(path + "/mav0/cam0/data")
os.makedirs(path + "/mav0/cam1/data")
print('Argument List:', str(sys.argv[1]))
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
print('Connected to remote API server')
#Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all,
sim.simx_opmode_blocking)
er, t_rightWheel = sim.simxGetObjectHandle(clientID,
'Pioneer_p3dx_rightMotor',
sim.simx_opmode_blocking)
er, t_leftWheel = sim.simxGetObjectHandle(clientID,
'Pioneer_p3dx_leftMotor',
sim.simx_opmode_blocking)
er, cam_handle_left = sim.simxGetObjectHandle(
clientID, 'anaglyphStereoSensor_leftSensor',
sim.simx_opmode_blocking)
er, cam_handle_right = sim.simxGetObjectHandle(
clientID, 'anaglyphStereoSensor_rightSensor',
sim.simx_opmode_blocking)
i = 0
fl = open(path + "/mav0/timestamps.txt", "w")
while (True):
err, resolution_left, colorCam_left = sim.simxGetVisionSensorImage(
clientID, cam_handle_left, 0, sim.simx_opmode_oneshot_wait)
err, resolution_right, colorCam_right = sim.simxGetVisionSensorImage(
clientID, cam_handle_right, 0, sim.simx_opmode_oneshot_wait)
img_left_1 = np.array(colorCam_left, dtype=np.uint8)
img_right_1 = np.array(colorCam_right, dtype=np.uint8)
img_left_1.resize([resolution_left[1], resolution_left[0], 3])
img_right_1.resize([resolution_right[1], resolution_right[0], 3])
img_left_2 = np.flipud(img_left_1)
img_right_2 = np.flipud(img_right_1)
img_left_3 = img_left_2[..., ::-1].copy()
img_right_3 = img_right_2[..., ::-1].copy()
milli_time = int(round(time.time() * 1000)) * 1000000000
st = str(milli_time) + '\n'
st_left = path + '/mav0/cam0/data/{}.png'.format(milli_time)
st_right = path + '/mav0/cam1/data/{}.png'.format(milli_time)
cv2.imwrite(st_left, img_left_3)
cv2.imwrite(st_right, img_right_3)
cv2.waitKey(1)
i = i + 1
fl.write(st)
sim.simxSetJointTargetVelocity(clientID, t_rightWheel, 1,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID, t_leftWheel, 1,
sim.simx_opmode_streaming)
fl.close()
def __init__(self):
self.connect_to_server()
error, self.pioneerHandle = sim.simxGetObjectHandle(
self.clientId, 'Pioneer_p3dx', sim.simx_opmode_oneshot_wait)
if error == sim.simx_return_timeout_flag:
print(str(error) + '! ERROR: simxGetObjectHandle pioneer')
error, self.leftMotorHandle = sim.simxGetObjectHandle(
self.clientId, 'Pioneer_p3dx_leftMotor',
sim.simx_opmode_oneshot_wait)
if error == sim.simx_return_timeout_flag:
print(
str(error) +
'! ERROR: simxGetObjectHandle self.leftMotorHandle')
error, self.rightMotorHandle = sim.simxGetObjectHandle(
self.clientId, 'Pioneer_p3dx_rightMotor',
sim.simx_opmode_oneshot_wait)
if error == sim.simx_return_timeout_flag:
print(
str(error) +
'! ERROR: simxGetObjectHandle self.rightMotorHandle')
error, self.casterFreeHandle = sim.simxGetObjectHandle(
self.clientId, 'Pioneer_p3dx_caster_freeJoint1',
sim.simx_opmode_oneshot_wait)
if error == sim.simx_return_timeout_flag:
print(
str(error) +
'! ERROR: simxGetObjectHandle self.casterFreeHandle')
def __init__(self, clientID):
self.clientID = clientID
self.pioneer3DX_array = [None] * 19
self.position_coordX = [None] * 3
self.position_coordXc = [None] * 3
self.orientation = None
self.velocity = [None]* 2
self.ultrasonic = np.zeros((16, 3))
self.name = "Pioneer_p3dx"
self.left_motor = 'Pioneer_p3dx_leftMotor'
self.right_motor = 'Pioneer_p3dx_rightMotor'
self.ultrasonic_sensors = "Pioneer_p3dx_ultrasonicSensor"
### Load Mobile Robot Pioneer parameters
# self.pioneer3DX_array[0] represents the entire mobile robot block
error,self.pioneer3DX_array[0] = sim.simxGetObjectHandle(self.clientID,self.name,sim.simx_opmode_blocking)
# self.pioneer3DX_array[1] represents only the left motor
error,self.pioneer3DX_array[1] = sim.simxGetObjectHandle(self.clientID,self.left_motor,sim.simx_opmode_blocking)
# self.pioneer3DX_array[2] represents only the right motor
error,self.pioneer3DX_array[2] = sim.simxGetObjectHandle(self.clientID,self.right_motor,sim.simx_opmode_blocking)
# self.pioneer3DX_array[3:18] represents the 16 ultrasonic sensors
num = 1
while num < 17:
error,self.pioneer3DX_array[2+num] = sim.simxGetObjectHandle(self.clientID,self.ultrasonic_sensors+str(num),sim.simx_opmode_blocking)
error, DetectionState, Points ,detectedObjectHandle, Vector = sim.simxReadProximitySensor(self.clientID,self.pioneer3DX_array[2+num],sim.simx_opmode_streaming)
num+=1
error, linearVelocity, angularVelocity = sim.simxGetObjectVelocity(self.clientID,self.pioneer3DX_array[0], sim.simx_opmode_streaming)
error, position = sim.simxGetObjectPosition(self.clientID,self.pioneer3DX_array[0],-1, sim.simx_opmode_streaming)
error, angle = sim.simxGetObjectOrientation(self.clientID,self.pioneer3DX_array[0],-1,sim.simx_opmode_streaming)
print("Pioneer 3DX loaded")
def __init__(self, client, leftName, rightname, vis, followc1, followc2):
self.go = True
self.clientID = client
self.LSignalName = leftName
self.RSignalName = rightname
self.visionname = vis
self.LCycleFreq = BaseFreq
self.RCycleFreq = BaseFreq
self.orient = None
self.kp = 1
self.kd = -0.3
self.ki = 0.2
self.x = None
self.y = None
self.prevE = 0
self.totalE = 0
self.lower_blue = followc1
self.upper_blue = followc2
# for test :: to get the handle
self.cube = None
self.cube1 = None
e, self.cam = vrep.simxGetObjectHandle(clientID, self.visionname,
vrep.simx_opmode_blocking)
e, self.cube = vrep.simxGetObjectHandle(clientID, 'body#7',
vrep.simx_opmode_blocking)
e, self.cube1 = vrep.simxGetObjectHandle(clientID, 'GyroSensor#1',
vrep.simx_opmode_blocking)
def handleDroneObjects(clientID):
# --------------------------------- DRONE ---------------------------------
# Floor orthographic camera for exploration
res, camera_drone = sim.simxGetObjectHandle(clientID, 'Vision_sensor',
sim.simx_opmode_oneshot_wait)
if res != sim.simx_return_ok:
print('Could not get handle to Camera')
# Body
res, drone_base_handle = sim.simxGetObjectHandle(
clientID, 'Quadricopter_base', sim.simx_opmode_oneshot_wait)
if res != sim.simx_return_ok:
print('Could not get handle of drone base')
res, drone_target_handle = sim.simxGetObjectHandle(
clientID, 'Quadricopter_target', sim.simx_opmode_oneshot_wait)
if res != sim.simx_return_ok:
print('Could not get handle of drone target')
# --------------------------------- GENERAL ---------------------------------
# Floor
res, floor = sim.simxGetObjectHandle(clientID, 'ResizableFloor_5_25',
sim.simx_opmode_oneshot_wait)
if res != sim.simx_return_ok:
print('Could not get handle to Floor')
return camera_drone, drone_base_handle, drone_target_handle, floor
def init_elements(self):
if (self.activo == False):
return
#Guardar la referencia de la camara
_, self.camLeft = sim.simxGetObjectHandle(self.clientID, self.sen_Izq,
sim.simx_opmode_oneshot_wait)
_, self.camMid = sim.simxGetObjectHandle(self.clientID, self.sen_Mid,
sim.simx_opmode_oneshot_wait)
_, self.camRight = sim.simxGetObjectHandle(
self.clientID, self.sen_Der, sim.simx_opmode_oneshot_wait)
_, self.motorRight = sim.simxGetObjectHandle(
self.clientID, self.mot_Der, sim.simx_opmode_oneshot_wait)
_, self.motorLeft = sim.simxGetObjectHandle(
self.clientID, self.mot_Izq, sim.simx_opmode_oneshot_wait)
_, self.camSup = sim.simxGetObjectHandle(self.clientID,
self.camaraSuperior,
sim.simx_opmode_oneshot_wait)
_, self.ultra = sim.simxGetObjectHandle(self.clientID,
self.ultrasonido,
sim.simx_opmode_oneshot_wait)
_, self.laserI = sim.simxGetObjectHandle(self.clientID, self.laser_Izq,
sim.simx_opmode_oneshot_wait)
_, self.laserD = sim.simxGetObjectHandle(self.clientID, self.laser_Der,
sim.simx_opmode_oneshot_wait)
imgL = self.sensor_ir(self.camLeft)
imgM = self.sensor_ir(self.camMid)
imgR = self.sensor_ir(self.camRight)
img = self.get_image(self.camSup)
self.get_distance(self.laserI, self.maxlaser)
self.get_distance(self.laserD, self.maxlaser)
self.get_distance(self.ultra, self.dmax * 2)
time.sleep(1)
def initCamera(clientID):
_, cameraObject0 = sim.simxGetObjectHandle(clientID, "Vision_sensor0",
sim.simx_opmode_oneshot_wait)
_, cameraObject1 = sim.simxGetObjectHandle(clientID, "Vision_sensor1",
sim.simx_opmode_oneshot_wait)
_, resolution0, imageCamera0 = sim.simxGetVisionSensorImage(
clientID, cameraObject0, 0, sim.simx_opmode_streaming)
_, resolution1, imageCamera1 = sim.simxGetVisionSensorImage(
clientID, cameraObject1, 0, sim.simx_opmode_streaming)
time.sleep(1)
return cameraObject0, cameraObject1
def velocity(self,max1,max2):
#res,joint0 = sim.simxGetObjectHandle(clientID, "Revolute_joint0", sim.simx_opmode_oneshot_wait)
#res,joint1 = sim.simxGetObjectHandle(clientID, "Revolute_joint1", sim.simx_opmode_oneshot_wait)
res,main_handle = sim.simxGetObjectHandle(self.clientID, 'Cuboid0', sim.simx_opmode_blocking)
res,joint0 = sim.simxGetObjectHandle(self.clientID, "motor", sim.simx_opmode_oneshot_wait)
#res,joint1 = sim.simxGetObjectHandle(self.clientID, "right_motor", sim.simx_opmode_oneshot_wait)
sim.simxSynchronousTrigger(self.clientID)
left_Code =sim.simxSetJointTargetVelocity(self.clientID,joint0 ,max1,sim.simx_opmode_oneshot_wait)
#right_Code = sim.simxSetJointTargetVelocity(self.clientID,joint1 ,max2,sim.simx_opmode_oneshot_wait)
#right_Code = sim.simxSetJointTargetVelocity(clientID,joint1 ,max3,sim.simx_opmode_oneshot_wait)
print('finish velocity setting')
def get_ori_pos(obj, rel):
res, rel_handle = vrep.simxGetObjectHandle(clientID, rel,
vrep.simx_opmode_oneshot_wait)
res, obj_handle = vrep.simxGetObjectHandle(clientID, obj,
vrep.simx_opmode_oneshot_wait)
res, pos = vrep.simxGetObjectPosition(clientID, obj_handle, rel_handle,
vrep.simx_opmode_streaming)
res, ori = vrep.simxGetObjectOrientation(clientID, obj_handle, rel_handle,
vrep.simx_opmode_streaming)
res, quat = vrep.simxGetObjectQuaternion(clientID, obj_handle, rel_handle,
vrep.simx_opmode_streaming)
return pos, quat
def initConfigMotor(clientID):
#Guardamos los motores del robot:
error_izq, motor_izquierdo = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx_leftMotor", sim.simx_opmode_oneshot_wait)
error_der, motor_derecho = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx_rightMotor", sim.simx_opmode_oneshot_wait)
if error_izq or error_der:
sys.exit("Error: No se puede conectar con los motores")
else:
print("motor listo")
return motor_izquierdo, motor_derecho
def __init__(self):
jointNum = self.jointNum
baseName = self.baseName
rgName = self.rgName
jointName = self.jointName
camera_rgb_Name = self.camera_rgb_Name
camera_depth_Name = self.camera_depth_Name
print('Simulation started')
vrep.simxFinish(-1) # 关闭潜在的连接
# 每隔0.2s检测一次, 直到连接上V-rep
while True:
# simxStart的参数分别为：服务端IP地址(连接本机用127.0.0.1);端口号;是否等待服务端开启;连接丢失时是否尝试再次连接;超时时间(ms);数据传输间隔(越小越快)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID > -1:
print("Connection success!")
break
else:
time.sleep(0.2)
print("Failed connecting to remote API server!")
print("Maybe you forget to run the simulation on vrep...")
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) # 仿真初始化
# 读取Base和Joint的句柄
jointHandle = np.zeros((jointNum, 1), dtype=np.int)
for i in range(jointNum):
_, returnHandle = vrep.simxGetObjectHandle(
clientID, jointName + str(i + 1), vrep.simx_opmode_blocking)
jointHandle[i] = returnHandle
_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName,
vrep.simx_opmode_blocking)
_, rgHandle = vrep.simxGetObjectHandle(clientID, rgName,
vrep.simx_opmode_blocking)
_, cameraRGBHandle = vrep.simxGetObjectHandle(
clientID, camera_rgb_Name, vrep.simx_opmode_blocking)
_, cameraDepthHandle = vrep.simxGetObjectHandle(
clientID, camera_depth_Name, vrep.simx_opmode_blocking)
# 读取每个关节角度
jointConfig = np.zeros((jointNum, 1))
for i in range(jointNum):
_, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i],
vrep.simx_opmode_blocking)
jointConfig[i] = jpos
self.clientID = clientID
self.jointHandle = jointHandle
self.rgHandle = rgHandle
self.cameraRGBHandle = cameraRGBHandle
self.cameraDepthHandle = cameraDepthHandle
self.jointConfig = jointConfig
def init_elements(self):
if(self.activo == False):
return
#Guardar la referencia de la camara
_, self.camLeft = sim.simxGetObjectHandle(self.clientID, self.sen_Izq, sim.simx_opmode_oneshot_wait)
_, self.camMid = sim.simxGetObjectHandle(self.clientID,self.sen_Mid, sim.simx_opmode_oneshot_wait)
_, self.camRight = sim.simxGetObjectHandle(self.clientID, self.sen_Der, sim.simx_opmode_oneshot_wait)
imgL = self.get_image(self.camLeft)
imgM = self.get_image(self.camMid)
imgR = self.get_image(self.camRight)
time.sleep(1)
def _get_joint_handles(self):
self.green_box = sim.simxGetObjectHandle(self.clientID, "Green_Box",
blocking)[1]
self.red_box = sim.simxGetObjectHandle(self.clientID, "Red_Box",
blocking)[1]
self.lamp_end = sim.simxGetObjectHandle(self.clientID, "lamp_end",
blocking)[1]
self.joint_handles = [] # get joint handles
for i in range(1, self.num_joints + 1):
_, handle = sim.simxGetObjectHandle(self.clientID, f"m{i}",
blocking)
self.joint_handles.append(handle)
def get_dummies():
global target_handle
global ref_handle
for name in Target_names:
err, handle = sim.simxGetObjectHandle(clientID, name,
sim.simx_opmode_blocking)
target_handle.append(handle)
for name in Ref_names:
err, handle = sim.simxGetObjectHandle(clientID, name,
sim.simx_opmode_blocking)
ref_handle.append(handle)
print(target_handle)
print(ref_handle)
def __init__(self, clientID): self.clientID = clientID self.goal_velocity = 0.2 self.min_action = 0 self.max_action = 1 self.min_position = 0 self.max_position = 100 self.initial_position = [0,0,0] self.rotor_data = [0,0,0,0] self.goal_position = [2,1,1] err, self.quadHandle = sim.simxGetObjectHandle(self.clientID, "Quadricopter_base", sim.simx_opmode_blocking) err, target = sim.simxGetObjectHandle(self.clientID, "Sphere", sim.simx_opmode_oneshot_wait) target_pos = sim.simxSetObjectPosition(self.clientID, target, -1, self.goal_position, sim.simx_opmode_oneshot) self.low_state = np.array([self.min_position, self.min_position, self.min_position], dtype = np.float32) self.max_state = np.array([self.max_position, self.max_position, self.max_position], dtype = np.float32)
def __init__(self, simulationHandle):
self.simulation = simulationHandle.simulation
# motors
err_code, self.lb_motor_handle = sim.simxGetObjectHandle(
self.simulation, "LeftBackMotor", sim.simx_opmode_blocking)
err_code, self.rb_motor_handle = sim.simxGetObjectHandle(
self.simulation, "RightBackMotor", sim.simx_opmode_blocking)
err_code, self.lf_motor_handle = sim.simxGetObjectHandle(
self.simulation, "LeftFrontMotor", sim.simx_opmode_blocking)
err_code, self.rf_motor_handle = sim.simxGetObjectHandle(
self.simulation, "RightFrontMotor", sim.simx_opmode_blocking)
# wall for gyroscope
err_code, self.rf_wall_handle = sim.simxGetObjectHandle(
self.simulation, "240cmHighWall400cm0", sim.simx_opmode_blocking)
def findPosicionObjeto(clientID, figura, color):
posicion = []
_, HexaRojo = sim.simxGetObjectHandle(clientID, "Hexag2",
sim.simx_opmode_oneshot_wait)
posHexaRojo = getPosiciones(clientID, HexaRojo)[1]
_, HexaVerde = sim.simxGetObjectHandle(clientID, "Hexag3",
sim.simx_opmode_oneshot_wait)
posHexaVerde = getPosiciones(clientID, HexaVerde)[1]
_, HexaAzul = sim.simxGetObjectHandle(clientID, "Hexag4",
sim.simx_opmode_oneshot_wait)
posHexaAzul = getPosiciones(clientID, HexaAzul)[1]
_, TriangRojo = sim.simxGetObjectHandle(clientID, "triang2",
sim.simx_opmode_oneshot_wait)
posTriangRojo = getPosiciones(clientID, TriangRojo)[1]
_, TriangAzul = sim.simxGetObjectHandle(clientID, "triang3",
sim.simx_opmode_oneshot_wait)
posTriangAzul = getPosiciones(clientID, TriangAzul)[1]
_, TriangVerde = sim.simxGetObjectHandle(clientID, "triang4",
sim.simx_opmode_oneshot_wait)
posTriangVerde = getPosiciones(clientID, TriangVerde)[1]
_, CuadVerde = sim.simxGetObjectHandle(clientID, "Cuboid2",
sim.simx_opmode_oneshot_wait)
posCuadVerde = getPosiciones(clientID, CuadVerde)[1]
_, CuadAzul = sim.simxGetObjectHandle(clientID, "Cuboid3",
sim.simx_opmode_oneshot_wait)
posCuadAzul = getPosiciones(clientID, CuadAzul)[1]
_, CuadRojo = sim.simxGetObjectHandle(clientID, "Cuboid4",
sim.simx_opmode_oneshot_wait)
posCuadRojo = getPosiciones(clientID, CuadRojo)[1]
matriz = [["Hexagono", "Rojo", posHexaRojo],
["Hexagono", "Verde", posHexaVerde],
["Hexagono", "Azul", posHexaAzul],
["Triangulo", "Rojo", posTriangRojo],
["Triangulo", "Azul", posTriangAzul],
["Triangulo", "Verde", posTriangVerde],
["Cuadrado", "Verde", posCuadVerde],
["Cuadrado", "Azul", posCuadAzul],
["Cuadrado", "Rojo", posCuadRojo]]
for i in range(9):
for j in range(3):
if matriz[i][0] == figura and matriz[i][1] == color:
posicion = matriz[i][2]
return posicion
def __init__(self):
try:
sim.simxFinish(-1) #close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID != -1:
print('connect successfully')
else:
sys.exit("connect error")
except:
print('Check if CoppeliaSim is open')
_, Quadcopter_target = sim.simxGetObjectHandle(
clientID, 'Quadricopter_target', sim.simx_opmode_blocking)
_, targetPosition = sim.simxGetObjectPosition(clientID,
Quadcopter_target, -1,
sim.simx_opmode_buffer)
print(targetPosition)
ArrayPosition = [-0.18570, 0.99366, 0.615]
sim.simxSetObjectPosition(clientID, Quadcopter_target, -1,
ArrayPosition, sim.simx_opmode_oneshot)
self.clientID = clientID
self.Quadcopter_target = Quadcopter_target
self.targetPosition = targetPosition
def main():
Pioneer = Robot(clientID)
Pioneer.turnLeft()
time.sleep(2)
Pioneer.moveForward()
time.sleep(2)
Pioneer.turnRight()
time.sleep(2)
Pioneer.moveBackward()
time.sleep(2)
Pioneer.moveForward()
time.sleep(6)
Pioneer.stop()
# errorCode, left_motor_handle = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', sim.simx_opmode_oneshot_wait)
# errorCode, right_motor_handle = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', sim.simx_opmode_oneshot_wait)
# errorCode=sim.simxSetJointTargetVelocity(clientID, left_motor_handle, 0.8, sim.simx_opmode_streaming)
# errorCode=sim.simxSetJointTargetVelocity(clientID, right_motor_handle, 0.8, sim.simx_opmode_streaming)
# errorCode, sensor1 = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor1', sim.simx_opmode_oneshot_wait)
# errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
# clientID, sensor1, sim.simx_opmode_streaming)
# errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
# clientID, sensor1, sim.simx_opmode_buffer)
errorCam0, camera_handle = sim.simxGetObjectHandle(
clientID, cam_name, sim.simx_opmode_oneshot_wait)
# Start the Stream
errorCam1, res, image = sim.simxGetVisionSensorImage(
clientID, camera_handle, 0, sim.simx_opmode_streaming)
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 getPosBall(clientID, ref):
res, client_jointHandle = sim.simxGetObjectHandle(clientID, 'ball',
sim.simx_opmode_blocking)
res, alvoPos = sim.simxGetObjectPosition(clientID, client_jointHandle, ref,
sim.simx_opmode_streaming)
return 100 * alvoPos[0], 100 * alvoPos[1]
def vrepInterface(port):
global angles_handler
angles_handler = np.zeros(12)
global angles_error
angles_error = np.zeros(12)
global clientID
clientID = 0
global inital_name
print('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
ID = sim.simxStart('127.0.0.1', port, True, True, 5000,
5) # Connect to V-REP
print(ID)
if ID != -1:
print('Connected to remote API server')
# Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = sim.simxGetObjects(ID, sim.sim_handle_all,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
print('Number of objects in the scene: ', len(objs))
else:
print('Remote API function call returned with error code: ', res)
else:
print('DIDNOT CONNECT!!!')
intial_name = [
'ab3', 'bc3', 'cd3', 'ab4', 'bc4', 'cd4', 'ab1', 'bc1', 'cd1', 'ab2',
'bc2', 'cd2'
]
for i in range(angles_handler.shape[0]):
angles_error[i], angles_handler[i] = sim.simxGetObjectHandle(
clientID, intial_name[i], sim.simx_opmode_blocking)
return ID
def __init__(self, client_id):
self.client_id = client_id
self._handles = []
self._sensor_vals = np.zeros(16)
self.op_mode = sim.simx_opmode_oneshot_wait
self.sensor_locs = 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 i in range(1, 16 + 1):
# build list of handles
_, sensor_handle = sim.simxGetObjectHandle(
client_id,
'Pioneer_p3dx_ultrasonicSensor' + str(i),
self.op_mode)
self._handles.append(sensor_handle)
# Load Values
self.update_distances()
def move_right():
errorCode, player_x_handle = sim.simxGetObjectHandle(
clientID2, 'Pla_X_joint', sim.simx_opmode_oneshot_wait)
sim.simxSetJointTargetVelocity(clientID2, player_x_handle, -1,
sim.simx_opmode_oneshot_wait)
print("move_right")
return ("nothing")
def getHandleFromName(name):
returnCode, handle = sim.simxGetObjectHandle(clientID, name, sim.simx_opmode_blocking)
# print(name, handle)
if not returnCode:
return handle
else:
return returnCode
def resolveVision(_clientID, _sigValue):
global clientID
global sigValue
sigValue = _sigValue
clientID = _clientID
# Get the camera handle:
erro, camera = sim.simxGetObjectHandle(clientID, 'Vision_s',
sim.simx_opmode_oneshot_wait)
# Start the Stream
erro, res, image = sim.simxGetVisionSensorImage(clientID, camera, 0,
sim.simx_opmode_streaming)
frame, resol = getImage(camera)
src = frame.copy()
img = basicFilter(src, 0)
foundShape, foundColors, foundCenters = findUseful(src, img, 0.09)
foundColors = rgbToLetter(foundColors)
foundCubes = createArray(foundCenters, foundColors, resol[0], resol[1])
j = 0
for shapes in foundShape:
shp = np.reshape(shapes, (5, 1, 2))
cv2.drawContours(frame, shp.astype(int), -1, (255, 0, 255), 3)
j += 1
cv2.imwrite('./imgs/7Final.png', frame)
return foundCubes
def getCode(_clientID):
global clientID
clientID = _clientID
correction = 0
# Get the camera handle:
erro, camera = sim.simxGetObjectHandle(clientID, 'Vis_Num',
sim.simx_opmode_oneshot_wait)
# Start the Stream
erro, res, image = sim.simxGetVisionSensorImage(clientID, camera, 0,
sim.simx_opmode_streaming)
frame, resol = getImage(camera)
src = frame.copy()
#test(camera)
img = basicFilter(src, 2, correction)
isolImg, nres = isolateFace(frame.copy(), img, resol, 1)
if (isolImg.size == 0):
return -1
while ((nres[0] > 240 or nres[0] < 80 or nres[1] > 240 or nres[1] < 80 or
(abs(nres[0] - nres[1]) > 70)) and correction != 3):
correction = correction + 1
img = basicFilter(src, 2, correction)
isolImg, nres = isolateFace(frame.copy(), img, resol, 1)
print(nres)
cv2.imwrite('./imgs/7new.png', isolImg)
op2 = compareFaces.compareBar(isolImg, nres)
return op2
def __init__(self, robot, host="127.0.0.1", port=19997):
self.robot = robot
self.sim_joints = [0, 0, 0, 0, 0, 0]
sim.simxFinish(-1) # just in case, close all opened connections
self.clientID = sim.simxStart(host, port, True, True, 5000,
5) # Connect to CoppeliaSim
if self.clientID != -1:
print('Connected to remote API server')
else:
print("coppeliasim connection failed")
return
res, objs = sim.simxGetObjects(self.clientID, sim.sim_handle_all,
sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
print('Number of objects in the scene: ', len(objs))
else:
print('Remote API function call returned with error code: ', res)
self.jh = [0, 0, 0, 0, 0, 0]
for i in range(6):
res, self.jh[i] = sim.simxGetObjectHandle(
self.clientID, 'UR5_joint{}'.format(i + 1),
sim.simx_opmode_blocking)
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
