diff --git a/daq/src/aaredaq/daq.py b/daq/src/aaredaq/daq.py index 3c1ad6c1..e16c92d5 100644 --- a/daq/src/aaredaq/daq.py +++ b/daq/src/aaredaq/daq.py @@ -1,6 +1,7 @@ import copy import json import math +import statistics import time from datetime import datetime from math import ceil @@ -12,6 +13,7 @@ import cv2 import numpy as np import redis from scipy import ndimage +from scipy.optimize import curve_fit from aaredaq import workflows from aaredaq.aaredb import AareWrapper @@ -243,7 +245,7 @@ class AareDAQ: self.__devs.smargon.move_home(wait=True) logger.info("moving aerotech to mount position") self.__devs.abr_pos = ABR_POS_MOUNT - time.sleep(0.5) + time.sleep(0.1) logger.info("checking beamstop") if self.__devs.bsz.value < 24.0: raise Exception("Beamstop Z below 24.0 mm - potentially unsafe with mounting") @@ -251,7 +253,7 @@ class AareDAQ: logger.info(f"Checking smargon position: {self.__devs.smargon.readback}") logger.info(f"Checking abr position: {self.__devs.abr_pos}") if self.__devs.magnet_position_sensor.value != 0: - time.sleep(5) + time.sleep(1) logger.warning("!!!!!!!!!!!!!!!!!!!!!MAGNET CONTROLLER BROKE AGAIN!!!!!!!!!!!!!!!!!!") logger.debug(f"Checking magnet position sensor positon: {self.__devs.magnet_position_sensor_readout.value}") logger.debug(f"Checking smargon position: {self.__devs.smargon.readback}") @@ -298,7 +300,6 @@ class AareDAQ: # reset zoom self.zoom = 1 # mount sample - logger.info(f"Moving tell to unmount") self.__aare.sample_unmounted(curr_sample) logger.info(f"Moving tell to mount") self.__devs.tell.mount( @@ -911,148 +912,116 @@ class AareDAQ: # angles in degrees -> (theta_rad, height) samples = [] for deg, box in boxes_by_angle.items(): - if not area: - h = self.box_height_from_tuple(box) - else: - h = self.box_area_from_tuple(box) - samples.append((math.radians(deg), h)) + v = self.box_area_from_tuple(box) if area else self.box_height_from_tuple(box) + samples.append((float(deg), v)) return samples - def fit_area_vs_angle(self, areas_by_angle_deg: List[Tuple[float, float]]) -> Tuple[Callable[[float], float], float, dict]: - """ - Fit area(θ) = A + B*cos(θ - φ) using a linear fit on cos/sin terms. - Input: - areas_by_angle_deg: { angle_deg: area } - Returns: - (area_fn, best_angle_deg, params) - area_fn(theta_deg) -> predicted area - best_angle_deg: angle (deg) maximizing fitted curve (in [0, 360)) - params: {"A": A, "B": B, "phi_rad": phi} - """ - if not areas_by_angle_deg: - return (lambda _: 0.0, 0.0, {"A": 0.0, "B": 0.0, "phi_rad": 0.0}) + @staticmethod + def _cos_model(theta_deg: float | np.ndarray, A: float, B: float, phi_rad: float): + return A + B * np.cos(np.deg2rad(theta_deg) - phi_rad) - samples = [(math.radians(deg), float(area)) for deg, area in areas_by_angle_deg] + @staticmethod + def _mad_filter(samples: List[Tuple[float, float]], k: float = 3.5) -> List[Tuple[float, float]]: + if not samples: + return samples + ys = [y for _, y in samples] + med = statistics.median(ys) + mad = statistics.median([abs(y - med) for y in ys]) or 1.0 + return [(d, y) for d, y in samples if abs(y - med) <= k * mad] - if len(samples) < 3: - # Fallback: constant model at mean, choose best measured angle - mean_area = sum(a for _, a in samples) / len(samples) - best_measured = max(areas_by_angle_deg, key=lambda kv: kv[1])[0] % 360 - return (lambda _: mean_area, best_measured, {"A": mean_area, "B": 0.0, "phi_rad": 0.0}) + def fit_area_vs_angle(self, areas_by_angle_deg: List[Tuple[float, float]]) -> dict: - # Accumulate sums for normal equations - n = len(samples) - sum1 = n - sum_cos = sum(math.cos(t) for t, _ in samples) - sum_sin = sum(math.sin(t) for t, _ in samples) - sum_y = sum(y for _, y in samples) - sum_cos2 = sum((math.cos(t)) ** 2 for t, _ in samples) - sum_sin2 = sum((math.sin(t)) ** 2 for t, _ in samples) - sum_cossin = sum(math.cos(t) * math.sin(t) for t, _ in samples) - sum_ycos = sum(y * math.cos(t) for t, y in samples) - sum_ysin = sum(y * math.sin(t) for t, y in samples) + pts = [(float(d), float(a)) for d, a in areas_by_angle_deg] + pts = self._mad_filter(pts, k=3.5) + degs = np.array([d for d, _ in pts], dtype=float) + ys = np.array([a for _, a in pts], dtype=float) - # Solve for [A, C, S] in: - # [ n sum_cos sum_sin ] [A] = [ sum_y ] - # [ sum_cos sum_cos2 sum_cossin ] [C] [ sum_ycos] - # [ sum_sin sum_cossin sum_sin2 ] [S] [ sum_ysin] - def det3(m): - return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) - - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) - + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0])) + # Initial guess via linearized fit + cosv = np.cos(np.deg2rad(degs)) + sinv = np.sin(np.deg2rad(degs)) + X = np.column_stack([np.ones_like(degs), cosv, sinv]) # [A, C, S] + try: + beta, _, _, _ = np.linalg.lstsq(X, ys, rcond=None) + A0, C0, S0 = beta.tolist() + except Exception: + A0, C0, S0 = float(np.mean(ys)), 0.0, 0.0 + B0 = float(math.hypot(C0, S0)) + phi0 = float(math.atan2(S0, C0)) - M = [ - [sum1, sum_cos, sum_sin], - [sum_cos, sum_cos2, sum_cossin], - [sum_sin, sum_cossin, sum_sin2], - ] - b = [sum_y, sum_ycos, sum_ysin] - - def replace_col(M, col, vec): - R = [row[:] for row in M] - for i in range(3): - R[i][col] = vec[i] - return R - - D = det3(M) or 1e-12 - A = det3(replace_col(M, 0, b)) / D - C = det3(replace_col(M, 1, b)) / D - S = det3(replace_col(M, 2, b)) / D - - B = math.hypot(C, S) - phi = math.atan2(S, C) # C = B cosφ, S = B sinφ + # Bounds + y_std = float(np.std(ys)) or 1.0 + B_max = 10.0 * y_std + bounds = ([-np.inf, 0.0, -math.pi], [np.inf, B_max, math.pi]) def area_fn(theta_deg: float) -> float: return A + B * math.cos(math.radians(theta_deg) - phi) + try: + popt, _ = curve_fit(self._cos_model, degs, ys, p0=[A0, B0, phi0], bounds=bounds, maxfev=10000) + A, B, phi = map(float, popt) + except Exception as e: + logger.info(f"error in curve fit {e}") + A, B, phi = A0, max(0.0, B0), phi0 - # Best angle occurs at θ = φ (convert to degrees, normalize) - best_angle_deg = (math.degrees(phi)) % 360 + #best_angle_deg = float((math.degrees(phi)) % 360) + return {"A": A, "B": max(0.0, B), "phi_rad": phi} - return area_fn, best_angle_deg, {"A": A, "B": B, "phi_rad": phi} - - def fit_cosine_height(self, samples: List[Tuple[float, float]]) -> Tuple[float, float, float]: - """ - Fit h(θ) = A + C*cosθ + S*sinθ via linear least squares, then convert to A + B*cos(θ - φ). - Returns (A, B, phi) where phi in radians. - """ + def fit_cosine_height(self, samples: List[Tuple[float, float]]) -> dict: + samples = self._mad_filter(samples, k=3.5) if len(samples) < 3: - # fallback: constant model - A = sum(h for _, h in samples) / max(1, len(samples)) + A = sum(y for _, y in samples) / max(1, len(samples)) return (A, 0.0, 0.0) - # Build normal equations for [A, C, S] - sum1 = len(samples) - sum_cos = sum(math.cos(t) for t, _ in samples) - sum_sin = sum(math.sin(t) for t, _ in samples) - sum_h = sum(h for _, h in samples) - sum_cos2 = sum(math.cos(t) ** 2 for t, _ in samples) - sum_sin2 = sum(math.sin(t) ** 2 for t, _ in samples) - sum_cossin = sum(math.cos(t) * math.sin(t) for t, _ in samples) - sum_hcos = sum(h * math.cos(t) for t, h in samples) - sum_hsin = sum(h * math.sin(t) for t, h in samples) + degs = np.array([d for d, _ in samples], dtype=float) + ys = np.array([y for _, y in samples], dtype=float) - # Solve 3x3 linear system: - # [ sum1 sum_cos sum_sin ] [A] = [ sum_h ] - # [ sum_cos sum_cos2 sum_cossin ] [C] [ sum_hcos ] - # [ sum_sin sum_cossin sum_sin2 ] [S] [ sum_hsin ] - # Use Cramer's rule or a tiny solver since numpy may not be allowed externally. - def det3(m): - return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) - - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) - + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0])) + # Initial guess via linearized cosine/sine fit + cosv = np.cos(np.deg2rad(degs)) + sinv = np.sin(np.deg2rad(degs)) + X = np.column_stack([np.ones_like(degs), cosv, sinv]) # [A, C, S] + try: + beta, _, _, _ = np.linalg.lstsq(X, ys, rcond=None) + A0, C0, S0 = beta.tolist() + except Exception: + A0, C0, S0 = float(np.mean(ys)), 0.0, 0.0 + B0 = float(math.hypot(C0, S0)) + phi0 = float(math.atan2(S0, C0)) - M = [ - [sum1, sum_cos, sum_sin], - [sum_cos, sum_cos2, sum_cossin], - [sum_sin, sum_cossin, sum_sin2], - ] - bA = [sum_h, sum_hcos, sum_hsin] + # Bounds to keep B reasonable and phi in [-pi, pi] + y_std = float(np.std(ys)) or 1.0 + B_max = 10.0 * y_std + bounds = ([-np.inf, 0.0, -math.pi], [np.inf, B_max, math.pi]) - # Matrices with columns replaced - def replace_col(M, col_idx, vec): - R = [row[:] for row in M] - for i in range(3): - R[i][col_idx] = vec[i] - return R + try: + popt, _ = curve_fit(self._cos_model, degs, ys, p0=[A0, B0, phi0], bounds=bounds, maxfev=10000) + A, B, phi = map(float, popt) + B = max(0.0, B) + except Exception as e: + # Fallback to initial + logger.info(f"error in curve fit {e}") + return {"A": A, "B": max(0.0, B), "phi_rad": phi} - D = det3(M) or 1e-12 - A = det3(replace_col(M, 0, bA)) / D - C = det3(replace_col(M, 1, bA)) / D - S = det3(replace_col(M, 2, bA)) / D + def face_detection(self) -> dict: + self.__cfg.try_set_busy(timeout=360) + try: + logger.info("running face detection sequence") + result = self.__face_detection_sequence() + except Exception as e: + logger.error(f"error in face detection sequence {e}") + result = { + "samples": None, + "height_fit": None, + "area_fit": None, + } + self.__cfg.state_busy = False + return result - # Convert A + C cosθ + S sinθ to A + B cos(θ - φ) - B = math.hypot(C, S) - phi = math.atan2(S, C) # since C = B cosφ, S = B sinφ - - return (A, B, phi) - - def __face_detection_sequence(self, zoom_value: int = 280): + def __face_detection_sequence(self) -> dict: self.__set_state(BeamlineStateEnum.SampleAlignment) self.__devs.lamp_light = 2.5 self.__cfg.zoom_mode = ZoomModeEnum.LoopCenter zoom_settings = self.__cfg.zoom_settings.z - zoom_value = 280 - print('new loop') + zoom_value = self.__devs.zoom + logger.info('face detection sequence') exposure = zoom_settings[zoom_value].exposure gain = zoom_settings[zoom_value].gain self.__devs.samcam_settings = SampleCameraSettings(exposure=exposure, gain=gain) @@ -1065,38 +1034,64 @@ class AareDAQ: box = self.__mlbox.predict(curr_image, "", pref_class = (3,0)) if box: cls, x1, y1, x2, y2 = box + boxes[angle] = (x1, y1, x2, y2) if cls == 0: #logger.info(f"loop all for angle {angle}") boxes[angle] = (x1, y1, x2, y2) + logger.info(f'box added to boxes: {boxes}') + elif cls == 3: + boxes[angle] = (x1, y1, x2, y2) + else: + logger.debug(f"no loop found only class: {cls} pin or xtal") + else: + logger.info(f'no box found for angle {angle}') if not boxes: - print("no boxes found") - return + logger.info(f'no boxes found: {boxes}') + logger.info('no boxes found') - samples = self.prepare_height_samples(boxes, area=True) - #logger.info(f"height vs angle samples: {samples}") - area_fn, best_angle_deg, params = self.fit_area_vs_angle(samples) - A, B, phi = self.fit_cosine_height(samples) - #logger.info(f"cosine fit: A={A}, B={B}, phi={phi}") - #A, B, phi = params["A"], params["B"], params["phi_rad"] + return { + "samples": None, + "height_fit": None, + "area_fit": None, + } - def height_deg(theta_deg: float) -> float: + samples_area = self.prepare_height_samples(boxes, area=True) + area_params = self.fit_area_vs_angle(samples_area) + samples_height = self.prepare_height_samples(boxes, area=False) + height_params= self.fit_cosine_height(samples_height) + + search_grid = np.linspace(-90, 90, 400) + def model(theta_deg: float, A:float, B: float, phi: float) -> float: return A + B * math.cos(math.radians(theta_deg) - phi) search_grid = range(-90, 90, 1) - best_fit_angle = max(search_grid, key=lambda d: height_deg(d)) - best_fit_height = height_deg(best_fit_angle) + best_fit_angle_area = max(search_grid, key=lambda d: model(d, area_params['A'], area_params['B'], area_params['phi_rad'])) + best_fit_angle_height = max(search_grid, key=lambda d: model(d, height_params['A'], height_params['B'], height_params['phi_rad'])) - flat_face_angle = max(boxes.keys(), key=lambda a: self.box_height_from_tuple(boxes[a])) - flat_face_box = boxes[flat_face_angle] - #logger.info(f'Best fitted angle: {best_fit_angle}, fitted height: {best_fit_height:.3f}') - #logger.info(f'Flat face angle: {flat_face_angle}, box: {flat_face_box}') + # Move to fitted best angle + logger.info(f"best angle by area: {best_fit_angle_area}") + logger.info(f"best angle by height: {best_fit_angle_height}") + candidates = [ + a for a in (best_fit_angle_area, best_fit_angle_height) + if a is not None and a in search_grid + ] + angle = candidates[0] if candidates else 0 + self.__devs.aerotech.move(angle, wait=True) - # Move to fitted best angle and return both measured and fitted info and predictor - self.__devs.aerotech.move(best_fit_angle, wait=True) - return + # Build return JSON + samples_out = [] + for deg, box in boxes.items(): + h = self.box_height_from_tuple(box) + a = self.box_area_from_tuple(box) + samples_out.append({"angle_deg": float(deg), "height": float(h), "area": float(a)}) + samples_out.sort(key=lambda x: x["angle_deg"]) - #return flat_face_angle, flat_face_box + return { + "samples": samples_out, + "height_fit": {"A": height_params["A"], "B": height_params["B"], "phi_rad": height_params["phi_rad"], "best_angle_deg": best_fit_angle_height}, + "area_fit": {"A": area_params["A"], "B": area_params["B"], "phi_rad": area_params["phi_rad"], "best_angle_deg": best_fit_angle_area}, + } def __loop_center_sequence(self, sample_id: int | None = None) -> bool: @@ -1112,17 +1107,15 @@ class AareDAQ: max_attempt = 2 attempt = 0 found = 0 - angles = (0, -45, -90) + angles = (0, -90) + if sample_id is not None: + self.save_screenshot_db(sample_id, f"pre_alc") while attempt < max_attempt: - print('new loop center settings') + logger.debug('new loop center settings') self.__devs.samcam_settings = SampleCameraSettings(exposure=exposure, gain=gain) self.__devs.zoom_sync(zoom_value) - print(f'zoom={zoom_value},gain={gain}, exp={exposure}') - - if sample_id is not None: - self.save_screenshot_db(sample_id, f"pre_alc") - print(sample_id) + logger.debug(f'zoom={zoom_value},gain={gain}, exp={exposure}') found_flag = False found_angle = None @@ -1131,12 +1124,13 @@ class AareDAQ: gn = int(gain) for loop, angle in enumerate(angles): - print(f"Moving to new omega: {angle}") - if sample_id is not None and i == 0: - self.save_screenshot_db(sample_id, f"pre_alc_{sample_id}_{angle}_{zoom_value:.0f}_{exp}_{gn}") - + logger.debug(f"Moving to new omega: {angle}") + # if sample_id is not None and i == 0: + # self.save_screenshot_db(sample_id, f"pre_alc_{sample_id}_{angle}_{zoom_value:.0f}_{exp}_{gn}") + time_to_move_aerotech= time.perf_counter() self.__devs.aerotech.move(angle, wait=True) - time.sleep(1) + logger.info(f"time to move: {time.perf_counter()-time_to_move_aerotech}") + #time.sleep(0.1) filename = None if sample_id is not None: filename = f"{sample_id}_{angle}_{zoom_value:.0f}_{exp}_{gn}" @@ -1163,22 +1157,22 @@ class AareDAQ: else: if found >= 3 or targets_found_this_attempt >= 3: - print(f"sucessfully found {found} or {targets_found_this_attempt} targets in {attempt} attempts") + logger.debug(f"sucessfully found {found} or {targets_found_this_attempt} targets in {attempt} attempts") break if found_flag is not None and found_angle is not None: - print(f"found a target at angle {found_angle} in attempt {attempt}") + logger.debug(f"found a target at angle {found_angle} in attempt {attempt}") angles = (found_angle, found_angle + 45, found_angle + 90) - attempt += 1 - print(f"attempt {attempt} of {max_attempt}") + attempt += 1 + logger.debug(f"attempt {attempt} of {max_attempt}") if attempt >= max_attempt: raise LoopCenteringFailed #i += 1 - print("alc success") - self.__face_detection_sequence() + logger.debug("alc success") + #self.face_detection_sequence() return True except Exception as e: - print(f"Error in loop centering: {e}") + logger.info(f"Error in loop centering: {e}") return False def auto_loop_center(self, sample_id: int | None = None) -> float: @@ -1236,10 +1230,16 @@ class AareDAQ: ) try: + logger.info(f"setting busy at {time.perf_counter() - start}") self.__cfg.try_set_busy(timeout=360) + logger.info(f"set busy at {time.perf_counter() - start}") geom = self.sample_geometry - print(f"{time.ctime()} starting mount {sample.db_id}") + start_mount=time.perf_counter() + logger.info(f"starting mount {sample.db_id} at {time.ctime()}") self.__mount(sample) + logger.info(f"mounting done at {time.perf_counter() - start_mount}, total time: {time.perf_counter() - start}") + alc_time = time.perf_counter() - start + logger.info(f"starting alc at {alc_time}") if not self.__loop_center_sequence(sample.db_id): self.__aare.alc_failed(sample) print("alc failed") @@ -1247,7 +1247,9 @@ class AareDAQ: end = time.perf_counter() return end - start #raise LoopCenteringFailed - + logger.info(f"alc done at {time.perf_counter() - start}") + self.__face_detection_sequence() + logger.info(f"face_detection done at {time.perf_counter() - start}") self.zoom = 500 time.sleep(0.5)