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)