def receiveLabel(self, label):
self.player.setFreeze(False)
self.player.start()
self.grid.startDetection()
(x, y, z) = self.player.getPosition(label)
print(x, y, z)
if (not(x == 0 and y == 0 and z == 0)):
pos = np.matrix([[x],[y],[z],[1]])
point = self.transMat * pos
moveObj(point[0],point[1],point[2],0,21.22,12.86)
def mouseClick(event, y, x, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
capture.grab()
ok, real = capture.retrieve(0, cv2.CAP_OPENNI_POINT_CLOUD_MAP)
xw = 100 * real[x][y][0]
yw = 100 * real[x][y][1]
zw = 100 * real[x][y][2]
point = TRANS_MAT * np.matrix([[xw],[yw],[zw],[1]])
print point[0], point[1], point[2]
moveObj(point[0],point[1],point[2],0,21.22,12.86)
def go_to_position_mouse(event, y, x, flags, param):
global TRANS_MAT, cnt, done
if event == cv2.EVENT_LBUTTONDOWN:
print y, x
capture.grab()
ok, real = capture.retrieve(0, cv2.CAP_OPENNI_POINT_CLOUD_MAP)
xw = 100 * real[x][y][0]
yw = 100 * real[x][y][1]
zw = 100 * real[x][y][2]
print xw, yw, zw
if cnt < 16:
if cnt < 15:
GoToPos(points[cnt][0], points[cnt][1], points[cnt][2], 'close')
#time.sleep(5)
kinect_frame_pts.append([xw, yw, zw])
cnt += 1
else:
if not done:
done = True
Kinect_frame_matrix = np.matrix([[kinect_frame_pts[0][0],kinect_frame_pts[0][1],kinect_frame_pts[0][2],1],
[kinect_frame_pts[1][0],kinect_frame_pts[1][1],kinect_frame_pts[1][2],1],
[kinect_frame_pts[2][0],kinect_frame_pts[2][1],kinect_frame_pts[2][2],1],
[kinect_frame_pts[3][0],kinect_frame_pts[3][1],kinect_frame_pts[3][2],1],
[kinect_frame_pts[4][0],kinect_frame_pts[4][1],kinect_frame_pts[4][2],1],
[kinect_frame_pts[5][0],kinect_frame_pts[5][1],kinect_frame_pts[5][2],1],
[kinect_frame_pts[6][0],kinect_frame_pts[6][1],kinect_frame_pts[6][2],1],
[kinect_frame_pts[7][0],kinect_frame_pts[7][1],kinect_frame_pts[7][2],1],
[kinect_frame_pts[8][0],kinect_frame_pts[8][1],kinect_frame_pts[8][2],1],
[kinect_frame_pts[9][0],kinect_frame_pts[9][1],kinect_frame_pts[9][2],1],
[kinect_frame_pts[10][0],kinect_frame_pts[10][1],kinect_frame_pts[10][2],1],
[kinect_frame_pts[11][0],kinect_frame_pts[11][1],kinect_frame_pts[11][2],1],
[kinect_frame_pts[12][0],kinect_frame_pts[12][1],kinect_frame_pts[12][2],1],
[kinect_frame_pts[13][0],kinect_frame_pts[13][1],kinect_frame_pts[13][2],1],
[kinect_frame_pts[14][0],kinect_frame_pts[14][1],kinect_frame_pts[14][2],1],])
print kinect_frame_pts
#print Kinect_frame_matrix.transpose()
TRANS_MAT = getTransformationMat(Kinect_frame_matrix.transpose())
else:
TRANS_MAT = [[1.38884742e-01, 7.14152672e-02, -9.97560137e-01, 9.08830124e+01],
[1.20949309e+00, -9.34430346e-02, -1.09917642e-01, 2.06001291e+01],
[-6.30752771e-02, 1.29666723e+00, 8.56265774e-02, 8.16176389e+00],
[0.00000000e+00, 8.67361738e-18, -1.11022302e-16, 1.00000000e+00]]
point = TRANS_MAT * np.matrix([[xw],[yw],[zw],[1]])
print point[0],point[1],point[2]
moveObj(point[0],point[1],point[2],0,21.22,12.86)
def go_to_position_mouse(event, y, x, flags, param):
global TRANS_MAT, cnt, done
if event == cv2.EVENT_LBUTTONDOWN:
print y, x
capture.grab()
ok, real = capture.retrieve(0, cv2.CAP_OPENNI_POINT_CLOUD_MAP)
xw = 100 * real[x][y][0]
yw = 100 * real[x][y][1]
zw = 100 * real[x][y][2]
print xw, yw, zw
if cnt < 16:
if cnt < 15:
GoToPos(points[cnt][0], points[cnt][1], points[cnt][2], 'close')
#time.sleep(5)
kinect_frame_pts.append([xw, yw, zw + 1.5])
cnt += 1
else:
if not done:
done = True
Kinect_frame_matrix = np.matrix([[kinect_frame_pts[0][0],kinect_frame_pts[0][1],kinect_frame_pts[0][2],1],
[kinect_frame_pts[1][0],kinect_frame_pts[1][1],kinect_frame_pts[1][2],1],
[kinect_frame_pts[2][0],kinect_frame_pts[2][1],kinect_frame_pts[2][2],1],
[kinect_frame_pts[3][0],kinect_frame_pts[3][1],kinect_frame_pts[3][2],1],
[kinect_frame_pts[4][0],kinect_frame_pts[4][1],kinect_frame_pts[4][2],1],
[kinect_frame_pts[5][0],kinect_frame_pts[5][1],kinect_frame_pts[5][2],1],
[kinect_frame_pts[6][0],kinect_frame_pts[6][1],kinect_frame_pts[6][2],1],
[kinect_frame_pts[7][0],kinect_frame_pts[7][1],kinect_frame_pts[7][2],1],
[kinect_frame_pts[8][0],kinect_frame_pts[8][1],kinect_frame_pts[8][2],1],
[kinect_frame_pts[9][0],kinect_frame_pts[9][1],kinect_frame_pts[9][2],1],
[kinect_frame_pts[10][0],kinect_frame_pts[10][1],kinect_frame_pts[10][2],1],
[kinect_frame_pts[11][0],kinect_frame_pts[11][1],kinect_frame_pts[11][2],1],
[kinect_frame_pts[12][0],kinect_frame_pts[12][1],kinect_frame_pts[12][2],1],
[kinect_frame_pts[13][0],kinect_frame_pts[13][1],kinect_frame_pts[13][2],1],
[kinect_frame_pts[14][0],kinect_frame_pts[14][1],kinect_frame_pts[14][2],1],])
#print kinect_frame_pts
#print Kinect_frame_matrix.transpose()
TRANS_MAT = getTransformationMat(Kinect_frame_matrix.transpose())
print TRANS_MAT
else:
point = TRANS_MAT * np.matrix([[xw],[yw],[zw],[1]])
print point[0],point[1],point[2]
moveObj(point[0],point[1],point[2],0,21.22,12.86)
@author: Jacob
"""
import vrep
import numpy as np
from startup import startup
from moveObj import moveObj
import time
# open comms with the simulator and define the robot joints
clientID, bodyJoints, rightArm, leftArm = startup()
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
objHandle = int(
vrep.simxGetObjectHandle(clientID, 'ReferenceFrame',
vrep.simx_opmode_blocking)[1])
T = np.array([[1, 0, 0, .5], [0, 1, 0, .5], [0, 0, 1, .5], [0, 0, 0, 1]])
moveObj(T, clientID, objHandle)
time.sleep(10)
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)
# Close the connection to V-REP
vrep.simxFinish(clientID)
