import sys import time from pytrinamic.tmcl import TMCLCommand from calibot import Calibot from steamvr import LighthouseConsole, ImuCalibrationProcess, PhotoDiodeCalibrationProcess, PhotoDiodeSolution, ImuSolution from steamvr.lighthouse_console import LighthouseDeviceNotFound from random import Random from functools import partial from threading import Thread from threading import Lock import logging import imgui import glfw import OpenGL.GL as GL from imgui.integrations.glfw import GlfwRenderer import load_init_json O1 = 1/360*2 # offset 1 O2 = 1/360*0.1 #0.0000 # offset 2 RAND = Random() logging.getLogger().addHandler(logging.StreamHandler(sys.stdout)) #logging.basicConfig(filename="./calibot.log", level=logging.DEBUG) logging.getLogger().setLevel(logging.DEBUG) log = logging.getLogger(__name__) logger_lock = Lock() log.info("TEst") def write_imu_calibration(config, solve: ImuSolution): config['imu']['acc_bias'] = solve.imu.acc_bias config['imu']['acc_scale'] = solve.imu.acc_scale config['imu']['gyro_bias'] = solve.imu.gyro_bias def write_pd_calibration(config, solve: PhotoDiodeSolution): config['lighthouse_config']['channelMap'] = solve.lighthouse_config.channelMap config['lighthouse_config']['modelPoints'] = solve.lighthouse_config.modelPoints config['lighthouse_config']['modelNormals'] = solve.lighthouse_config.modelNormals def perform_random_motion_path(arm: Calibot): while True: # -0.05 < turn1 < 0.8 and -0.05 < turn2 < 0.55 a = RAND.uniform(-0.05, 0.8) b = RAND.uniform(-0.05, 0.55) arm.go_position(a, b) def perform_imu_motion_path(arm: Calibot): arm.go_position(0.5, 0.25) # pos 1 time.sleep(3) arm.go_position(0.75, 0.25) # pos 2 time.sleep(3) arm.go_position(0.5, 0) # pos 3 time.sleep(3) arm.go_position(0.5, 0.25) # pos 4 time.sleep(3) arm.go_position(0.25, 0.25) # pos 4 time.sleep(3) arm.go_position(0.25, 0.5) # pos 5 time.sleep(3) arm.go_position(0, 0.25) # pos 6 (load2) time.sleep(3) class CalibrationManager(Thread): def __init__(self, calibot): super().__init__() self.exc = None self.calibot = calibot self.lighthouse = LighthouseConsole() self.lighthouse.open() self.lighthouse_lock = Lock() self.connected_devices = self.lighthouse.list_devices() def on_imu_result(self, serial, solution: ImuSolution): with self.lighthouse_lock: log_ = log.getChild(serial + "_imu") with logger_lock: log_.info(f"IMU Calibration of {serial} complete") self.lighthouse.select_device(serial) conf = self.lighthouse.download_config() write_imu_calibration(conf, solution) log_.info(f"uploading calibration to {serial}") self.lighthouse.upload_config(conf) assert conf == self.lighthouse.download_config() log_.info(f"upload complete") def on_lh_result(self, serial, solution: PhotoDiodeSolution): with self.lighthouse_lock: log_ = log.getChild(serial + "_lighthouse") with logger_lock: log_.info(f"LH Calibration of {serial} complete") self.lighthouse.select_device(serial) conf = self.lighthouse.download_config() write_pd_calibration(conf, solution) log_.info(f"uploading calibration to {serial}") self.lighthouse.upload_config(conf) assert conf == self.lighthouse.download_config() log_.info(f"upload complete") def create_imu_calibration_threads(self): calibration_threads = [] for device in self.connected_devices: #t = ImuCalibrationProcess(device, 240, on_completion=partial(self.on_imu_result, device)) #t.start() #calibration_threads.append(t) t = PhotoDiodeCalibrationProcess(device, min_observations=80, min_hit_per_sensor=20, on_completion=partial(self.on_lh_result, device)) t.start() calibration_threads.append(t) return calibration_threads def start_calibration(self): log.info("Starting Calibration") #perform_imu_motion_path(self.calibot) self.calibot.module1.connection.send(TMCLCommand.SAP, 5, 0, 100) # acceleration self.calibot.module2.connection.send(TMCLCommand.SAP, 5, 0, 100) # acceleration # assert len(self.imu_calibrations) == 4 #perform_random_motion_path(self.calibot) t = Thread(target=perform_random_motion_path, args=[self.calibot], daemon=True) t.start() def run(self): super().run() try: self.calibot.go_position(0.5, 0.25) # pos 1 (load1) time.sleep(3) cal_threads = self.create_imu_calibration_threads() self.start_calibration() for t in cal_threads: t.join() log.info("Calibration complete") except BaseException as e: self.exc = e def join(self, timeout=None): super().join(timeout) if self.exc: raise self.exc def impl_glfw_init(): width, height = 1280, 720 window_name = "ByD control panel" if not glfw.init(): print("Could not initialize OpenGL context") exit(1) # OS X supports only forward-compatible core profiles from 3.2 glfw.window_hint(glfw.CONTEXT_VERSION_MAJOR, 3) glfw.window_hint(glfw.CONTEXT_VERSION_MINOR, 3) glfw.window_hint(glfw.OPENGL_PROFILE, glfw.OPENGL_CORE_PROFILE) glfw.window_hint(glfw.OPENGL_FORWARD_COMPAT, GL.GL_TRUE) # Create a windowed mode window and its OpenGL context window = glfw.create_window( int(width), int(height), window_name, None, None ) glfw.make_context_current(window) if not window: glfw.terminate() print("Could not initialize Window") exit(1) return window def mainGUI(): calibot = None try: imgui.create_context() window = impl_glfw_init() impl = GlfwRenderer(window) cal_protocol = None while not glfw.window_should_close(window): glfw.poll_events() impl.process_inputs() imgui.new_frame() try: if calibot is None: calibot = Calibot(O1, O2) log.info("Homing Calibot") #calibot.go_home() calibot.go_position(0.5, 0.25) # pos 1 if imgui.button("Start Calibration"): cal_protocol = CalibrationManager(calibot) cal_protocol.start() cal_protocol.join() except LighthouseDeviceNotFound: log.warning("No lighthouse devices connected") GL.glClearColor(73 / 255, 75 / 255, 92 / 255, 1) GL.glClear(GL.GL_COLOR_BUFFER_BIT) imgui.render() impl.render(imgui.get_draw_data()) glfw.swap_buffers(window) impl.shutdown() glfw.terminate() except KeyboardInterrupt: calibot.e_stop() log.warning('safe stop') if __name__ == "__main__": mainGUI()