def __init__(self, clientId, config):
self.clientID = clientId
errorCode, self.KinectRgbHandle = vrep.simxGetObjectHandle(
self.clientID, 'kinect_rgb', vrep.simx_opmode_blocking)
errorCode, self.KinectDepthHandle = vrep.simxGetObjectHandle(
self.clientID, 'kinect_depth', vrep.simx_opmode_oneshot_wait)
self.imgsize = (640, 480)
fs2 = cv2.FileStorage(config, cv2.FileStorage_READ)
self.intrinsic = fs2.getNode("intrinsic").mat()
self.dist = fs2.getNode("dist").mat()
fs2.release()
def __init__(self,clinetID):
self.clientID = clinetID
errorCode, self.dummyTargetHandle = vrep.simxGetObjectHandle(self.clientID, 'Dummy_target', vrep.simx_opmode_oneshot_wait)
errorCode, self.dummyTipHandle = vrep.simxGetObjectHandle(self.clientID, 'Dummy_tip', vrep.simx_opmode_oneshot_wait)
fs = cv2.FileStorage("../config/Robot_set.yml",cv2.FileStorage_READ)
self.mu_q = fs.getNode("mu_q").real()
self.simu_q = fs.getNode("simu_q").real()
self.mu_t = fs.getNode("mu_t").real()
self.simu_t = fs.getNode("simu_t").real()
fs.release()
errorcode, self.home_euler = vrep.simxGetObjectOrientation(self.clientID, self.dummyTipHandle, -1,
vrep.simx_opmode_oneshot_wait)
errorcode,self.home_position = vrep.simxGetObjectPosition(self.clientID, self.dummyTipHandle, -1,
vrep.simx_opmode_oneshot_wait)
def test():
print('Program started')
# 关闭潜在的连接
vrep.simxFinish(-1)
# 每隔0.2s检测一次，直到连接上V-rep
while True:
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID > -1:
break
else:
time.sleep(0.2)
print("Failed connecting to remote API server!")
print("Connection success!")
errorCode, dummyHandle = vrep.simxGetObjectHandle(
clientID, 'Dummy_target', vrep.simx_opmode_oneshot_wait)
print(errorCode)
errorCode, position = vrep.simxGetObjectPosition(
clientID, dummyHandle, -1, vrep.simx_opmode_oneshot_wait)
print(position)
vrep.simxSetObjectPosition(clientID, dummyHandle, -1, [-0.2, 0, 0.3],
vrep.simx_opmode_oneshot_wait)