def connect_robot(obj):
# Connect UR
rob = van_robot.Robot("192.168.0.1", True) # Set IP address
orig_tcp = (0, 0, 0.150, 0, 0, 0)
rob.set_tcp(orig_tcp) # Set Tool Center Point
rob.set_payload(0.5, (0, 0, 0)) # Set payload for joints
time.sleep(0.2) # leave some time to robot to process the setup commands
a, v = (0.03, 0.03) # acceleration and movement speed of robot
rob.movel([0, 0, 0.1, 0, 0, 0], a, v, relative=True)
# Connect Arduino
arduino = arduino_serial.Arduino("COM8")
print('arduino connected', datetime.datetime.now())
# Connect aruco
#aruco.connect()
print('aruco connected', datetime.datetime.now())
def robot_connect(self):
try:
self.rob = van_robot.Robot("192.168.0.1", True) # Set IP address
self.orig_tcp = (0, 0, 0.290, 0, 0, 0)
self.rob.set_tcp(self.orig_tcp) # Set Tool Center Point
self.rob.set_payload(0.6) # Set payload for joints
time.sleep(
0.2) # leave some time to robot to process the setup commands
self.a, self.v = (0.03, 0.03)
self.pos_start = self.rob.getl()
print('connected to robot', datetime.datetime.now())
self._connected_robot = True
return True
except:
print("Couldn't connect to robot", datetime.datetime.now())
self._connected_robot = False
return False
switch_Y = 2
switch_Z = 0
i = 1
# Declare size window and the IR stopping distance
delta_x = 0.2
delta_z = -0.3
x_off = 0.1
z_off = -0.2
safe_US_distance = 0.3
# Declare the port and baudrate for the Arduino
arduino = arduino_serial.Arduino("COM8")
distance = arduino.distance
duration = arduino.duration
rob = van_robot.Robot("192.168.0.1", True) # Set IP address
orig_tcp = (0, 0, 0.135, 0, 0, 0)
rob.set_tcp(orig_tcp) # Set Tool Center Point
rob.set_payload(0.5, (0, 0, 0)) # Set payload for joints
time.sleep(0.2) # leave some time to robot to process the setup commands
# start position of the robot
# assumed is a robot start position which is close to the centre of the measured plane
pos_start = rob.getl()
rob.movel(pos_start)
print("Current tool pose is: ", rob.getl())
rob.measure_triangle(arduino, 0.05)
rob.movel(pos_start)
def run(window_placement_x, window_placement_y, max_window_size_x,
max_window_size_y):
# board properties
# aruco example
board_params = {
"board_type": "aruco",
"dict_type": aruco.DICT_6X6_1000,
"markersX": 4,
"markersY": 5,
"markerLength": 3.75, # Provide length of the marker's side [cm]
"markerSeparation": 0.5, # Provide separation between markers [cm]
}
# For validating results, show aruco board to camera.
board_params["aruco_dict"] = aruco.getPredefinedDictionary(
board_params["dict_type"])
"""# charuco example
board_params_params = {
"board_type": "charuco",
"dict_type": aruco.DICT_4X4_1000,
"squaresX": 4,
"squaresY": 4,
"squareLength": 3, # Provide length of the square's side [cm]
"markerLength": 3 * 7 / 9, # Provide length of the marker's side [cm]
}
# For validating results, show aruco board to camera.
board_params["aruco_dict"] = aruco.getPredefinedDictionary(board_params["dict_type"])
"""
# create arUco board
if board_params["board_type"] == "aruco":
board = aruco.GridBoard_create(board_params["markersX"],
board_params["markersY"],
board_params["markerLength"],
board_params["markerSeparation"],
board_params["aruco_dict"])
elif board_params["board_type"] == "charuco":
board = aruco.CharucoBoard_create(board_params["squaresX"],
board_params["squaresY"],
board_params["squareLength"],
board_params["markerLength"],
board_params["aruco_dict"])
else:
raise ValueError("Couldn't find correct board type (aruco or charuco)")
"""uncomment following block to draw and show the board"""
# img = board.draw((864, 1080))
# cv2.imshow("aruco", img)
# cv2.imwrite("aruco.png", img)
arucoParams = aruco.DetectorParameters_create()
# Set second camera outside laptop
# cv2.CAP_DSHOW removes delay on logitech C920 webcam
camera = cv2.VideoCapture(1, cv2.CAP_DSHOW)
camera.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))
camera.set(cv2.CAP_PROP_FRAME_WIDTH, 960)
camera.set(cv2.CAP_PROP_FRAME_HEIGHT, 540)
# Turn the autofocus off
camera.set(cv2.CAP_PROP_AUTOFOCUS, 0)
# Open calibration file created with data_generation.py
with open('calibration.yaml') as f:
loadeddict = yaml.safe_load(f)
mtx = loadeddict.get('camera_matrix')
dist = loadeddict.get('dist_coeff')
mtx = np.array(mtx)
dist = np.array(dist)
ret, img = camera.read()
img_gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
h, w = img_gray.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1,
(w, h))
pose_r, pose_t = [], []
while True:
ret, img = camera.read()
img_aruco = img
im_gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
h, w = im_gray.shape[:2]
dst = cv2.undistort(im_gray, mtx, dist, None, newcameramtx)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
dst, board_params["aruco_dict"], parameters=arucoParams)
# cv2.imshow("original", img_gray)
if corners == None:
raise Exception("No aruco corners found!")
else:
rvec = None
tvec = None
ret, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board,
newcameramtx, dist, rvec,
tvec) # For a board
print("Rotation ", rvec, "\nTranslation", tvec, "\n=======")
if ret != 0:
img_aruco = aruco.drawDetectedMarkers(img, corners, ids,
(0, 255, 0))
img_aruco = aruco.drawAxis(
img_aruco, newcameramtx, dist, rvec, tvec, 10
) # axis length 100 can be changed according to your requirement
cv2.imshow("World co-ordinate frame axes", img_aruco)
if cv2.waitKey(0) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
# output from aruco
theta = Symbol('theta')
# aruco marker values [cm]
aruco_rvec = [rvec[0][0], rvec[1][0], rvec[2][0]]
aruco_tvec = [tvec[0][0], tvec[1][0], tvec[2][0]]
# x-value is off by 84 mm
# y-value is off by 4 mm
# z-value is off by 25 mm
aruco_x = aruco_tvec[0] / 100
aruco_y = aruco_tvec[
2] / 100 # y and z-axis are switched in the robot coordinates!!!
aruco_z = aruco_tvec[
1] / -100 # y and z-axis are switched in the robot coordinates!!! the minus is to compensate for axis orientation
aruco_angle = 0
theta = aruco_angle
x, y, z = 0.485, -0.295, 0.456 # coordinates of camera in respect to robot coord [m]
a, b, c = aruco_x, aruco_y, aruco_z # coordinates of marker in respect to the camera [m]
# translate 3d coordinate systems
Robot = CoordSys3D('Robot') # base coordinate system
Cam = Robot.locate_new('Cam', x * Robot.i + y * Robot.j + z * Robot.k)
Cam2 = Cam.orient_new_axis('Cam2', theta, Cam.i)
Marker = Cam2.locate_new('Marker', a * Cam2.i + b * Cam2.j + c * Cam2.k)
# Calculate marker position with respect to the robots base
mat = Marker.position_wrt(Robot).to_matrix(Robot)
mat = dense.matrix2numpy(mat, float)
fig = pyplot.figure()
ax = Axes3D(fig)
ax.scatter(0, 0, 0, label='Robot position')
ax.scatter(x, y, z, label='Camera position')
ax.scatter(mat[0][0], mat[1][0], mat[2][0], label='Label position')
ax.legend()
ax.set_xlabel("x-axis")
ax.set_ylabel("y-axis")
ax.set_zlabel("z-axis")
# Plot positions of robot_base, camera and position of the marker in 3D
# todo turn on plot for info (turned off for fast testing)
# pyplot.show()
# Robot movements
# todo: Moving x, y and z-plane robot
# todo: Line up with aruco Marker (0.2m y-offset from board)
rob = van_robot.Robot("192.168.0.1", True) # Set IP address
orig_tcp = (0, 0, 0.150, 0, 0, 0)
rob.set_tcp(orig_tcp) # Set Tool Center Point
rob.set_payload(0.5, (0, 0, 0)) # Set payload for joints
time.sleep(0.2) # leave some time to robot to process the setup commands
a, v = (0.05, 0.5) # acceleration and movementspeed of robot
# Start from the pose the robot is in
pos0 = rob.getl()
# Tool orientation [rx, ry, rz]
tool_orient = [0, -2.221441469, -2.221441469]
# Put the X, Y, Z of start pose in (last 3 coordinates are tool head positioning)
pos_start = [
pos0[0], pos0[1], pos0[2], tool_orient[0], tool_orient[1],
tool_orient[2]
]
# Take the Y-distance 0.10[m] out of the van
safe_dist = mat[1] - 0.12
# Move to the aruco marker point
# todo turn on move command
rob.movel((mat[0], safe_dist, mat[2], tool_orient[0], tool_orient[1],
tool_orient[2]), a, v)
# Calibration Y-plane and X,Z-Axis
# Declare the port and baudrate for the Arduino and connect to the Arduino
# todo CHECK COM!
arduino = arduino_serial.Arduino("COM3")
# start position of the robot (from aruco marker)
# assumed is a robot start position which is close to the centre of the measured plane
pos_start = (mat[0], safe_dist, mat[2], tool_orient[0], tool_orient[1],
tool_orient[2])
# Take safe offset from Y-plan after global aruco Y-value
measurement_distance = -0.03
board_distance = -0.1
rob.centre_for_triangle(arduino, measurement_distance, board_distance)
# Define Y-plane
rob.measure_triangle(arduino, 0.160, a, v)
# Define X,Z-axis
points1, points2 = rob.repetitive_def_axis(arduino, -0.02)
# Create origin
# coordinates in [X,Z]
# SIDE Coordinates [z-formula]
coor1 = [points1[0][2][0], points1[0][2][2]] # [X,Z]
coor2 = [points2[0][2][0], points2[0][2][2]]
# TOP Coordinates [x-formula]
coor3 = [points1[1][2][0], points1[1][2][2]]
coor4 = [points2[1][2][0], points2[1][2][2]]
# create_origin.formula_line('x',coor1,coor2)
# create_origin.formula_line('z',coor3,coor4)
line_x = create_origin.formula_line('x', coor1, coor2)
line_z = create_origin.formula_line('z', coor3, coor4)
origin = create_origin.intersect_lines(line_x, line_z)
print(origin) # [X,Z]
# Calculate translation to origin
cur_pos = rob.getl()
cur_pos[0] = origin[0]
cur_pos[2] = origin[1]
# Move to origin (first moves out, then in x,z-plane)
print("X,Z-Axis origin has been set.")
print("Moving to origin.")
rob.move_y_plane(-0.15, cur_pos)
#test stuff for rectangle
delta_x = window_placement_x # distance in meters
delta_z = window_placement_y # distance in meters
radius = 0.06
x_axis_offset = max_window_size_x # [m]
z_axis_offset = -max_window_size_y # [m]
rob.draw_square(delta_x, delta_z, radius, -0.012, x_axis_offset,
z_axis_offset)