From 9a43207ed4690d714659e405a94c3497e59bcff2 Mon Sep 17 00:00:00 2001 From: "mattia.gallacchi" Date: Tue, 12 Nov 2024 15:36:32 +0100 Subject: [PATCH 1/5] Add sync and async move Signed-off-by: mattia.gallacchi --- pyspj/spj.py | 237 +++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 222 insertions(+), 15 deletions(-) diff --git a/pyspj/spj.py b/pyspj/spj.py index d5b73d0..12d3d39 100644 --- a/pyspj/spj.py +++ b/pyspj/spj.py @@ -2,19 +2,57 @@ import numpy as np import serial from dataclasses import dataclass import time +from threading import Thread, Lock, Event +import weakref + +class SpjException(Exception): + pass + +class NotConnected(SpjException): + pass @dataclass class WorldPosition: rz : float = 0.0 ry : float = 0.0 - rz1 : float = 0.0 + rx : float = 0.0 + +def thread_func(weak_self): + """Thread function to constantly read the position + """ + + while True: + + self = weak_self() + # When main class fall out of scope + if self is None: + break + + if self._thread_stop_event.is_set(): + break + + new_pos = self._ser.read_until() + + if len(new_pos) > 0: + steps = new_pos.decode("utf-8").strip().split(",") + steps = [float(x) for x in steps] + + if self._position_thread_lock.acquire(blocking=False): + self._current_pos = steps + self._position_thread_lock.release() + + del self + time.sleep(0.001) class SphericalParallelJoint: + + STORE_POS = WorldPosition(-60, 0, 0) + HOME_POS = WorldPosition(0, 0, 0) - def __init__(self, port : str = "/dev/ttyACM0", baud : int = 115200) -> None: - self._ser = serial.Serial(port, baudrate=baud, timeout=1) - self._curr_position = WorldPosition() + def __init__(self) -> None: + """Constructor""" + self._rot_axis = np.array([ [ -1.0 / np.sqrt(3.0), -1.0 / np.sqrt(3.0), - 1.0 / np.sqrt(3.0) ], [ -1.0 / np.sqrt(2.0), 1.0 / np.sqrt(2.0), 0.0 ], @@ -22,11 +60,66 @@ class SphericalParallelJoint: #[ -1.0 / np.sqrt(3.0), -1.0 / np.sqrt(3.0), - 1.0 / np.sqrt(3.0) ] #ZYZ' version. Z' = Z ]) + + self._current_pos: list[float] = [] + self._thread_stop_event = Event() + self._position_thread_lock = Lock() + self._ser = None + self._connected = False + + def connect(self, port : str = "/dev/ttyACM0", baud : int = 115200): + """Connect to the device + + Parameters + ---------- + port: str + COM port for serial communication. For linux /dev/tty, for windows COM + baud: int, optional + Serial baudrate + + Raise + ----- + Runtime error + """ + + self._ser = serial.Serial(port, baudrate=baud, timeout=1) + if not self.check_online(): raise RuntimeError("Failed to communicate with device") + + self._connected = True + + # Empty the read buffer + self._ser.read_all() + + # Start the thread + self._thread_stop_event.clear() + self._position_thread = Thread(target=thread_func, args=(weakref.ref(self), )) + self._position_thread.start() + + def __check_connected(self): + + if not self._connected: + raise NotConnected("Use the connect method to establish a connection to the device") + + # def __position_thread_func(self): + def __get_rotational_matrix(self, index : int, angle : float) -> np.ndarray: - + """Compute the rotational matrix for one axis + + Parameters + ---------- + index: int + Axis index (Rz = 0, Ry = 1, Rx = 2) + angle: float + Axis angle in degree + + Return + ------ + Rotational matrix for that axis + """ + angle = np.deg2rad(angle) ux, uy, uz = self._rot_axis[index] @@ -50,11 +143,22 @@ class SphericalParallelJoint: return mat - def compute_steps(self, new_pos : WorldPosition) -> list[int]: - + def compute_steps(self, new_pos : WorldPosition) -> list[float]: + """Compute the steps for each motor based on Rz, Ry and Rx coordinates + + Parameters + ---------- + new_pos: WorldPosition + Position to compute the steps for + + Return + ------ + Steps for each motor + """ + first_rot_mat = self.__get_rotational_matrix(0, new_pos.rz) second_rot_mat = self.__get_rotational_matrix(1, new_pos.ry) - third_rot_mat = self.__get_rotational_matrix(2, new_pos.rz1) + third_rot_mat = self.__get_rotational_matrix(2, new_pos.rx) # rot_mat = third_rot_mat * (second_rot_mat * first_rot_mat) rot_mat = np.matmul(third_rot_mat, np.matmul(second_rot_mat, first_rot_mat)) @@ -69,15 +173,69 @@ class SphericalParallelJoint: return[step1, step2, step3] - def move(self, pos : WorldPosition) -> list[float]: + def __move(self, pos : WorldPosition) -> list[float]: + self.__check_connected() + steps = self.compute_steps(pos) self._ser.write(f"{steps[0]},{steps[1]},{steps[2]}\n".encode("utf-8")) return [float(steps[0]),float(steps[1]),float(steps[2])] + def move_sync(self, pos: WorldPosition) -> bool: + """Move the device synchronously. This call will return when the movement is finished + + Parameters + ---------- + pos: WorldPosition + Position to move to + + Return + ------ + True if move was successful, False otherwise + """ + + max_errors = 100 + end_pos = self.__move(pos) + + try: + end_pos = list(map(int, end_pos)) + except ValueError as ex: + print(f"Failed to convert end position to int: {end_pos}") + return False + + while True: + + if max_errors < 1: + break + + try: + current_pos = list(map(int, self.get_current_position())) + except ValueError: + max_errors -= 1 + continue + + # print(current_pos, end_pos) + if current_pos == end_pos: + break + + if max_errors < 1: + return False + + return True + + def move_async(self, pos: WorldPosition) -> list[float]: + + return self.__move(pos) + def check_online(self) -> bool: - + """Check if the device is online and working + + Return + ------ + True if online + """ + msg = self._ser.read_until().decode("utf-8").strip() if msg == "Wait": @@ -95,13 +253,62 @@ class SphericalParallelJoint: return False def get_current_position(self) -> list[float]: + """Get the current position of the motors in steps - pos = self._ser.read_until() - steps = pos.decode("utf-8").strip().split(",") - steps = [float(x) for x in steps] - + Return + ------ + A list in the format (stepZ, stepY, stepX) + """ + + self.__check_connected() + + if self._position_thread_lock.acquire(blocking=False): + steps = self._current_pos + self._position_thread_lock.release() + + # Sleep to avoid overloading the lock + time.sleep(0.001) + return steps + + def close(self): + """End communication with the device + """ + if self._position_thread.is_alive(): + self._thread_stop_event.set() + self._position_thread.join() + self._ser.close() + self._ser = None + self._connected = False - \ No newline at end of file + def __del__(self): + """Destructor + """ + print("Destructor") + if self._connected: + self.close() + +if "__main__" == __name__: + + robot = SphericalParallelJoint() + robot.connect() + + print("Move sync") + if not robot.move_sync(robot.HOME_POS): + print("Move failed") + + print("Move async") + robot.move_async(robot.STORE_POS) + time.sleep(4) + robot.close() + + robot.connect() + + robot.move_sync(robot.HOME_POS) + robot.move_async(robot.STORE_POS) + + # robot.close() + + print("Bye") \ No newline at end of file -- GitLab From 4044ec2e83206dd4ff938f50bb38ce9fbd3032ad Mon Sep 17 00:00:00 2001 From: "mattia.gallacchi" Date: Wed, 13 Nov 2024 17:40:28 +0100 Subject: [PATCH 2/5] Add readme Fix some bugs Signed-off-by: mattia.gallacchi --- README.md | 28 +++ frontend/README.md | 0 frontend/genui.sh | 20 ++ frontend/main.py | 169 +++++++++++--- frontend/ui/__init__.py | 0 frontend/ui/robot.py | 299 ++++++++++++++++++++++++ frontend/ui/robot.ui | 487 ++++++++++++++++++++++++---------------- frontend/ui/robot_ui.py | 211 ----------------- pyproject.toml | 2 +- pyspj/__init__.py | 0 pyspj/spj.py | 278 +++++++++++++++++------ 11 files changed, 977 insertions(+), 517 deletions(-) create mode 100644 frontend/README.md create mode 100755 frontend/genui.sh create mode 100644 frontend/ui/__init__.py create mode 100644 frontend/ui/robot.py delete mode 100644 frontend/ui/robot_ui.py create mode 100644 pyspj/__init__.py diff --git a/README.md b/README.md index 85fe74f..10c53cc 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,31 @@ # Spherical Parallel Joint Robot +This repository contains a python package named **pyspj** that allows to control the Spherical Parallel Joint device from Skyentific. +It also contains a small GUI application to control the device as well as the controller firmware. + +## Install python dependencies + +```bash +poetry install +``` + +## Add **pyspj** package to your project + +If your project uses Poetry: + +1. Add this repository as a source + +```bash +poetry source add -s igib https://labinfo.ing.he-arc.ch/gitlab/api/v4/projects/2491/packages/pypi/simple +``` + +2. Add the package from the added source + +```bash +poetry add pyspj --source igib +``` + +## GUI application + + diff --git a/frontend/README.md b/frontend/README.md new file mode 100644 index 0000000..e69de29 diff --git a/frontend/genui.sh b/frontend/genui.sh new file mode 100755 index 0000000..cf6155c --- /dev/null +++ b/frontend/genui.sh @@ -0,0 +1,20 @@ +#!/bin/bash + +SCRIPTPATH="$( cd "$(dirname "$0")" >/dev/null 2>&1 ; pwd -P )" + +cd $SCRIPTPATH + +UI_FILES=`(find ui/ -type f -iname "*.ui")` + +# Generate qt designer files +for f in ${UI_FILES}; do + echo $f > .tmp + sed -i "s/.ui/.py/g" .tmp + + PYHTON_FILE=`(cat .tmp)` + echo "$PYHTON_FILE $f" + + rm .tmp + + poetry run pyside6-uic -o $PYHTON_FILE $f +done \ No newline at end of file diff --git a/frontend/main.py b/frontend/main.py index ec60ead..716692b 100644 --- a/frontend/main.py +++ b/frontend/main.py @@ -1,9 +1,11 @@ from PySide6.QtCore import Qt -from frontend.ui.robot_ui import Ui_MainWindow +import serial.tools +import serial.tools.list_ports +from frontend.ui.robot import Ui_MainWindow from pyspj.spj import SphericalParallelJoint, WorldPosition from PySide6.QtWidgets import ( - QApplication, QMainWindow, QMessageBox, QFileDialog, QWidget + QApplication, QMainWindow, QMessageBox, QPushButton, QSpinBox, QComboBox ) from PySide6.QtGui import QCloseEvent, QImage, QPixmap @@ -11,6 +13,9 @@ from PySide6.QtCore import QObject, Qt, QThread, Signal as pyqtSignal import time, sys from functools import partial from enum import Enum +import serial +import platform +from pathlib import Path POS_DEFAULT = 0.0 VEL_DEFAULT = 12.0 @@ -21,21 +26,20 @@ class JOG(Enum): RZ_POS = 1 RY_NEG = 2 RY_POS = 3 - RZ1_NEG = 4 - RZ1_POS = 5 + RX_NEG = 4 + RX_POS = 5 -class CommThread(QThread): +class RobotThread(QThread): update_positions = pyqtSignal(list) _stop = False - def __init__(self, serial : str, parent = None) -> None: + def __init__(self, spj: SphericalParallelJoint, parent = None) -> None: super().__init__(parent) - self._spj = SphericalParallelJoint(serial) - # values = self._spj.get_current_position() + self._spj = spj def send_pos(self, move : WorldPosition): - self._spj.move(move) + self._spj.move_async(move) def stop(self): self._stop = True @@ -46,20 +50,51 @@ class CommThread(QThread): # Get current position pos = self._spj.get_current_position() self.update_positions.emit(pos) - + class Window(QMainWindow, Ui_MainWindow): def __init__(self, parent = None) -> None: + super().__init__(parent) + self.setupUi(self) - self.robot = CommThread("COM9") + + self._com_ports: list[tuple[str, str]] = [] + self.__setup_comports() + self.__setup_signals() - self.robot.start() - time.sleep(0.5) + self.step_size_cb.addItems(["1", "5", "10", "50"]) + + self._spj: SphericalParallelJoint = SphericalParallelJoint() + self._angle_lut : list[list] = None + self._step_lut: list[list] = None + + # self._angle_lut, self._step_lut= self._spj.load_lookup_table(Path("../pyspj/spj_lut.json")) + self.current_pos = None - self.store() - + + self.__enable_robot_controls(False) + + self._com_thread: RobotThread = None + + def __setup_comports(self): + + self.comports_comboBox.clear() + + coms = sorted(serial.tools.list_ports.comports()) + + for com in coms: + if com.hwid != "n/a": + match platform.system(): + case "Linux": + self._com_ports.append((f"/dev/{com.name}", com.description)) + case "Windows": + self._com_ports.append(com.name, com.description) + + self.comports_comboBox.addItems([x[0] for x in self._com_ports]) + self.comport_description_lineEdit.setText(self._com_ports[0][1]) + def __setup_signals(self): # UI @@ -71,11 +106,14 @@ class Window(QMainWindow, Ui_MainWindow): self.jog_rz_pos_pb.pressed.connect(partial(self.jog_callback, JOG.RZ_POS)) self.jog_ry_neg_pb.pressed.connect(partial(self.jog_callback, JOG.RY_NEG)) self.jog_ry_pos_pb.pressed.connect(partial(self.jog_callback, JOG.RY_POS)) - self.jog_rz1_neg_pb.pressed.connect(partial(self.jog_callback, JOG.RZ1_NEG)) - self.jog_rz1_pos_pb.pressed.connect(partial(self.jog_callback, JOG.RZ1_POS)) + self.jog_rx_neg_pb.pressed.connect(partial(self.jog_callback, JOG.RX_NEG)) + self.jog_rx_pos_pb.pressed.connect(partial(self.jog_callback, JOG.RX_POS)) + self.comports_comboBox.currentIndexChanged.connect(self.__update_comport_description) + self.comport_connect_pushButton.clicked.connect(self.__connect_robot) + # Thread - self.robot.update_positions.connect(self.update_position) + # self.robot.update_positions.connect(self.update_position) def __update_spinbox_step_size(self): @@ -83,16 +121,63 @@ class Window(QMainWindow, Ui_MainWindow): self.move_ry_sb.setSingleStep(int(self.step_size_cb.currentText())) self.move_rz_sb.setSingleStep(int(self.step_size_cb.currentText())) + def __update_comport_description(self): + + self.comport_description_lineEdit.setText(self._com_ports[self.comports_comboBox.currentIndex()][1]) + + def __enable_robot_controls(self, enable: bool = True): + + for button in self.robot_control_gridLayout.parentWidget().findChildren(QPushButton): + _button: QPushButton = button + _button.setEnabled(enable) + + for comboBox in self.robot_control_gridLayout.parentWidget().findChildren(QComboBox): + _comboBox: QComboBox = comboBox + _comboBox.setEnabled(enable) + + for spinBox in self.robot_control_gridLayout.parentWidget().findChildren(QSpinBox): + _spinbox: QSpinBox = spinBox + _spinbox.setEnabled(enable) + + def __connect_robot(self): + + match self.comport_connect_pushButton.text(): + case "Connect": + + try: + self._spj.connect(self._com_ports[self.comports_comboBox.currentIndex()][0]) + except RuntimeError as ex: + self.err_msg(ex.__str__()) + return + + # Unlock all robot controls + self.__enable_robot_controls(True) + self.comport_connect_pushButton.setText("Disconnect") + self._com_thread = RobotThread(self._spj) + self._com_thread.update_positions.connect(self.update_position) + self._com_thread.start() + self.home() + + case "Disconnect": + + self.store() + self._com_thread.stop() + self._com_thread.wait() + self._spj.close() + self.__enable_robot_controls(False) + self.comport_connect_pushButton.setText("Connect") + def home(self): - self.move(WorldPosition(0.0, 0.0, 0.0)) + self.move(SphericalParallelJoint.home_pos()) def store(self): - self.move(WorldPosition(-60.0, 0.0, 0.0)) + self.move(SphericalParallelJoint.store_pos()) def jog_callback(self, jog: JOG): new_pos = self.current_pos step = int(self.step_size_cb.currentText()) + match jog: case JOG.RZ_NEG: new_pos.rz -= step @@ -102,10 +187,10 @@ class Window(QMainWindow, Ui_MainWindow): new_pos.ry -= step case JOG.RY_POS: new_pos.ry += step - case JOG.RZ1_NEG: - new_pos.rz1 -= step - case JOG.RZ1_POS: - new_pos.rz1 += step + case JOG.RX_NEG: + new_pos.rx -= step + case JOG.RX_POS: + new_pos.rx += step self.move(new_pos) @@ -122,37 +207,49 @@ class Window(QMainWindow, Ui_MainWindow): self.current_pos = new_pos self.update_spinboxes(new_pos) - self.robot.send_pos(new_pos) + self._com_thread.send_pos(new_pos) - print(self.current_pos) + # print(self.current_pos) def update_spinboxes(self, pos : WorldPosition): - self.move_rx_sb.setValue(int(pos.rz)) + self.move_rx_sb.setValue(int(pos.rx)) self.move_ry_sb.setValue(int(pos.ry)) - self.move_rz_sb.setValue(int(pos.rz1)) + self.move_rz_sb.setValue(int(pos.rz)) def update_position(self, pos : list[float]): + # pos = list(map(int, pos)) + # index = self._step_lut.index(pos) + # angle = self._angle_lut[index] + # try: - self.current_rx_le.setText(str(int(pos[0]))) + self.current_rx_le.setText(str(int(pos[2]))) self.current_ry_le.setText(str(int(pos[1]))) - self.current_rz_le.setText(str(int(pos[2]))) + self.current_rz_le.setText(str(int(pos[0]))) except ValueError as ex: - print(ex) + # print(ex) + return def closeEvent(self, a0: QCloseEvent) -> None: self.store() - self.robot.stop() - - # Wait for thread to stop - while(self.robot.isRunning()): - time.sleep(0.001) + self._com_thread.stop() + self._com_thread.wait() + self._spj.close() return super().closeEvent(a0) + def err_msg(self, msg: str): + + QMessageBox.critical(self, "Error", msg) + if __name__ == "__main__": + + import os + + os.chdir(os.path.dirname(__file__)) + app = QApplication(sys.argv) win = Window() win.show() diff --git a/frontend/ui/__init__.py b/frontend/ui/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/frontend/ui/robot.py b/frontend/ui/robot.py new file mode 100644 index 0000000..0dc7875 --- /dev/null +++ b/frontend/ui/robot.py @@ -0,0 +1,299 @@ +# -*- coding: utf-8 -*- + +################################################################################ +## Form generated from reading UI file 'robot.ui' +## +## Created by: Qt User Interface Compiler version 6.7.2 +## +## WARNING! All changes made in this file will be lost when recompiling UI file! +################################################################################ + +from PySide6.QtCore import (QCoreApplication, QDate, QDateTime, QLocale, + QMetaObject, QObject, QPoint, QRect, + QSize, QTime, QUrl, Qt) +from PySide6.QtGui import (QBrush, QColor, QConicalGradient, QCursor, + QFont, QFontDatabase, QGradient, QIcon, + QImage, QKeySequence, QLinearGradient, QPainter, + QPalette, QPixmap, QRadialGradient, QTransform) +from PySide6.QtWidgets import (QApplication, QComboBox, QFormLayout, QGridLayout, + QGroupBox, QHBoxLayout, QLabel, QLayout, + QLineEdit, QMainWindow, QMenuBar, QPushButton, + QSizePolicy, QSpacerItem, QSpinBox, QStatusBar, + QVBoxLayout, QWidget) + +class Ui_MainWindow(object): + def setupUi(self, MainWindow): + if not MainWindow.objectName(): + MainWindow.setObjectName(u"MainWindow") + MainWindow.resize(532, 408) + self.centralwidget = QWidget(MainWindow) + self.centralwidget.setObjectName(u"centralwidget") + self.verticalLayout = QVBoxLayout(self.centralwidget) + self.verticalLayout.setObjectName(u"verticalLayout") + self.groupBox = QGroupBox(self.centralwidget) + self.groupBox.setObjectName(u"groupBox") + self.horizontalLayout_5 = QHBoxLayout(self.groupBox) + self.horizontalLayout_5.setObjectName(u"horizontalLayout_5") + self.horizontalLayout_4 = QHBoxLayout() + self.horizontalLayout_4.setObjectName(u"horizontalLayout_4") + self.formLayout = QFormLayout() + self.formLayout.setObjectName(u"formLayout") + self.label_7 = QLabel(self.groupBox) + self.label_7.setObjectName(u"label_7") + + self.formLayout.setWidget(0, QFormLayout.LabelRole, self.label_7) + + self.horizontalSpacer_2 = QSpacerItem(40, 20, QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Minimum) + + self.formLayout.setItem(2, QFormLayout.LabelRole, self.horizontalSpacer_2) + + self.comport_connect_pushButton = QPushButton(self.groupBox) + self.comport_connect_pushButton.setObjectName(u"comport_connect_pushButton") + + self.formLayout.setWidget(2, QFormLayout.FieldRole, self.comport_connect_pushButton) + + self.comport_description_lineEdit = QLineEdit(self.groupBox) + self.comport_description_lineEdit.setObjectName(u"comport_description_lineEdit") + self.comport_description_lineEdit.setReadOnly(True) + + self.formLayout.setWidget(1, QFormLayout.FieldRole, self.comport_description_lineEdit) + + self.comports_comboBox = QComboBox(self.groupBox) + self.comports_comboBox.setObjectName(u"comports_comboBox") + + self.formLayout.setWidget(0, QFormLayout.FieldRole, self.comports_comboBox) + + self.label_9 = QLabel(self.groupBox) + self.label_9.setObjectName(u"label_9") + + self.formLayout.setWidget(1, QFormLayout.LabelRole, self.label_9) + + + self.horizontalLayout_4.addLayout(self.formLayout) + + self.horizontalLayout_4.setStretch(0, 1) + + self.horizontalLayout_5.addLayout(self.horizontalLayout_4) + + + self.verticalLayout.addWidget(self.groupBox) + + self.groupBox_2 = QGroupBox(self.centralwidget) + self.groupBox_2.setObjectName(u"groupBox_2") + self.groupBox_2.setAlignment(Qt.AlignmentFlag.AlignLeading|Qt.AlignmentFlag.AlignLeft|Qt.AlignmentFlag.AlignVCenter) + self.groupBox_2.setFlat(False) + self.gridLayout = QGridLayout(self.groupBox_2) + self.gridLayout.setObjectName(u"gridLayout") + self.robot_control_gridLayout = QGridLayout() + self.robot_control_gridLayout.setObjectName(u"robot_control_gridLayout") + self.robot_control_gridLayout.setSizeConstraint(QLayout.SizeConstraint.SetDefaultConstraint) + self.move_rz_sb = QSpinBox(self.groupBox_2) + self.move_rz_sb.setObjectName(u"move_rz_sb") + self.move_rz_sb.setMinimum(-180) + self.move_rz_sb.setMaximum(180) + + self.robot_control_gridLayout.addWidget(self.move_rz_sb, 3, 2, 1, 1) + + self.horizontalLayout_2 = QHBoxLayout() + self.horizontalLayout_2.setObjectName(u"horizontalLayout_2") + self.jog_ry_neg_pb = QPushButton(self.groupBox_2) + self.jog_ry_neg_pb.setObjectName(u"jog_ry_neg_pb") + + self.horizontalLayout_2.addWidget(self.jog_ry_neg_pb) + + self.jog_ry_pos_pb = QPushButton(self.groupBox_2) + self.jog_ry_pos_pb.setObjectName(u"jog_ry_pos_pb") + + self.horizontalLayout_2.addWidget(self.jog_ry_pos_pb) + + self.horizontalLayout_2.setStretch(0, 1) + self.horizontalLayout_2.setStretch(1, 1) + + self.robot_control_gridLayout.addLayout(self.horizontalLayout_2, 2, 3, 1, 1) + + self.label_5 = QLabel(self.groupBox_2) + self.label_5.setObjectName(u"label_5") + + self.robot_control_gridLayout.addWidget(self.label_5, 2, 0, 1, 1) + + self.current_ry_le = QLineEdit(self.groupBox_2) + self.current_ry_le.setObjectName(u"current_ry_le") + self.current_ry_le.setEnabled(True) + self.current_ry_le.setReadOnly(True) + + self.robot_control_gridLayout.addWidget(self.current_ry_le, 2, 1, 1, 1) + + self.step_size_cb = QComboBox(self.groupBox_2) + self.step_size_cb.setObjectName(u"step_size_cb") + + self.robot_control_gridLayout.addWidget(self.step_size_cb, 4, 3, 1, 1) + + self.label_4 = QLabel(self.groupBox_2) + self.label_4.setObjectName(u"label_4") + self.label_4.setAlignment(Qt.AlignmentFlag.AlignCenter) + + self.robot_control_gridLayout.addWidget(self.label_4, 0, 2, 1, 1) + + self.move_rx_sb = QSpinBox(self.groupBox_2) + self.move_rx_sb.setObjectName(u"move_rx_sb") + self.move_rx_sb.setMinimum(-180) + self.move_rx_sb.setMaximum(180) + + self.robot_control_gridLayout.addWidget(self.move_rx_sb, 1, 2, 1, 1) + + self.horizontalLayout = QHBoxLayout() + self.horizontalLayout.setObjectName(u"horizontalLayout") + self.jog_rx_neg_pb = QPushButton(self.groupBox_2) + self.jog_rx_neg_pb.setObjectName(u"jog_rx_neg_pb") + + self.horizontalLayout.addWidget(self.jog_rx_neg_pb) + + self.jog_rx_pos_pb = QPushButton(self.groupBox_2) + self.jog_rx_pos_pb.setObjectName(u"jog_rx_pos_pb") + + self.horizontalLayout.addWidget(self.jog_rx_pos_pb) + + self.horizontalLayout.setStretch(0, 1) + self.horizontalLayout.setStretch(1, 1) + + self.robot_control_gridLayout.addLayout(self.horizontalLayout, 1, 3, 1, 1) + + self.store_pb = QPushButton(self.groupBox_2) + self.store_pb.setObjectName(u"store_pb") + + self.robot_control_gridLayout.addWidget(self.store_pb, 4, 0, 1, 1) + + self.label_6 = QLabel(self.groupBox_2) + self.label_6.setObjectName(u"label_6") + + self.robot_control_gridLayout.addWidget(self.label_6, 3, 0, 1, 1) + + self.label_3 = QLabel(self.groupBox_2) + self.label_3.setObjectName(u"label_3") + + self.robot_control_gridLayout.addWidget(self.label_3, 1, 0, 1, 1) + + self.current_rz_le = QLineEdit(self.groupBox_2) + self.current_rz_le.setObjectName(u"current_rz_le") + self.current_rz_le.setEnabled(True) + self.current_rz_le.setReadOnly(True) + + self.robot_control_gridLayout.addWidget(self.current_rz_le, 3, 1, 1, 1) + + self.move_pb = QPushButton(self.groupBox_2) + self.move_pb.setObjectName(u"move_pb") + + self.robot_control_gridLayout.addWidget(self.move_pb, 4, 2, 1, 1) + + self.label = QLabel(self.groupBox_2) + self.label.setObjectName(u"label") + self.label.setAlignment(Qt.AlignmentFlag.AlignCenter) + + self.robot_control_gridLayout.addWidget(self.label, 0, 3, 1, 1) + + self.horizontalLayout_3 = QHBoxLayout() + self.horizontalLayout_3.setObjectName(u"horizontalLayout_3") + self.jog_rz_neg_pb = QPushButton(self.groupBox_2) + self.jog_rz_neg_pb.setObjectName(u"jog_rz_neg_pb") + + self.horizontalLayout_3.addWidget(self.jog_rz_neg_pb) + + self.jog_rz_pos_pb = QPushButton(self.groupBox_2) + self.jog_rz_pos_pb.setObjectName(u"jog_rz_pos_pb") + + self.horizontalLayout_3.addWidget(self.jog_rz_pos_pb) + + self.horizontalLayout_3.setStretch(0, 1) + self.horizontalLayout_3.setStretch(1, 1) + + self.robot_control_gridLayout.addLayout(self.horizontalLayout_3, 3, 3, 1, 1) + + self.home_pb = QPushButton(self.groupBox_2) + self.home_pb.setObjectName(u"home_pb") + + self.robot_control_gridLayout.addWidget(self.home_pb, 4, 1, 1, 1) + + self.move_ry_sb = QSpinBox(self.groupBox_2) + self.move_ry_sb.setObjectName(u"move_ry_sb") + self.move_ry_sb.setMinimum(-180) + self.move_ry_sb.setMaximum(180) + + self.robot_control_gridLayout.addWidget(self.move_ry_sb, 2, 2, 1, 1) + + self.current_rx_le = QLineEdit(self.groupBox_2) + self.current_rx_le.setObjectName(u"current_rx_le") + self.current_rx_le.setEnabled(True) + self.current_rx_le.setReadOnly(True) + + self.robot_control_gridLayout.addWidget(self.current_rx_le, 1, 1, 1, 1) + + self.label_2 = QLabel(self.groupBox_2) + self.label_2.setObjectName(u"label_2") + sizePolicy = QSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Preferred) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.label_2.sizePolicy().hasHeightForWidth()) + self.label_2.setSizePolicy(sizePolicy) + self.label_2.setAlignment(Qt.AlignmentFlag.AlignCenter) + + self.robot_control_gridLayout.addWidget(self.label_2, 0, 1, 1, 1) + + self.label_8 = QLabel(self.groupBox_2) + self.label_8.setObjectName(u"label_8") + + self.robot_control_gridLayout.addWidget(self.label_8, 0, 0, 1, 1) + + self.robot_control_gridLayout.setRowStretch(0, 1) + self.robot_control_gridLayout.setRowStretch(1, 1) + self.robot_control_gridLayout.setRowStretch(2, 1) + self.robot_control_gridLayout.setRowStretch(3, 1) + self.robot_control_gridLayout.setRowStretch(4, 1) + self.robot_control_gridLayout.setColumnStretch(0, 1) + + self.gridLayout.addLayout(self.robot_control_gridLayout, 0, 0, 1, 1) + + + self.verticalLayout.addWidget(self.groupBox_2) + + self.verticalLayout.setStretch(0, 2) + self.verticalLayout.setStretch(1, 3) + MainWindow.setCentralWidget(self.centralwidget) + self.menubar = QMenuBar(MainWindow) + self.menubar.setObjectName(u"menubar") + self.menubar.setGeometry(QRect(0, 0, 532, 23)) + MainWindow.setMenuBar(self.menubar) + self.statusbar = QStatusBar(MainWindow) + self.statusbar.setObjectName(u"statusbar") + MainWindow.setStatusBar(self.statusbar) + + self.retranslateUi(MainWindow) + + QMetaObject.connectSlotsByName(MainWindow) + # setupUi + + def retranslateUi(self, MainWindow): + MainWindow.setWindowTitle(QCoreApplication.translate("MainWindow", u"MainWindow", None)) + self.groupBox.setTitle(QCoreApplication.translate("MainWindow", u"Settings", None)) + self.label_7.setText(QCoreApplication.translate("MainWindow", u"Serial port", None)) + self.comport_connect_pushButton.setText(QCoreApplication.translate("MainWindow", u"Connect", None)) + self.label_9.setText(QCoreApplication.translate("MainWindow", u"Description", None)) + self.groupBox_2.setTitle(QCoreApplication.translate("MainWindow", u"Device controls", None)) + self.jog_ry_neg_pb.setText(QCoreApplication.translate("MainWindow", u"-", None)) + self.jog_ry_pos_pb.setText(QCoreApplication.translate("MainWindow", u"+", None)) + self.label_5.setText(QCoreApplication.translate("MainWindow", u"Ry", None)) + self.step_size_cb.setCurrentText("") + self.label_4.setText(QCoreApplication.translate("MainWindow", u"Move to", None)) + self.jog_rx_neg_pb.setText(QCoreApplication.translate("MainWindow", u"-", None)) + self.jog_rx_pos_pb.setText(QCoreApplication.translate("MainWindow", u"+", None)) + self.store_pb.setText(QCoreApplication.translate("MainWindow", u"Store", None)) + self.label_6.setText(QCoreApplication.translate("MainWindow", u"Rz", None)) + self.label_3.setText(QCoreApplication.translate("MainWindow", u"Rx", None)) + self.move_pb.setText(QCoreApplication.translate("MainWindow", u"Move", None)) + self.label.setText(QCoreApplication.translate("MainWindow", u"Jog", None)) + self.jog_rz_neg_pb.setText(QCoreApplication.translate("MainWindow", u"-", None)) + self.jog_rz_pos_pb.setText(QCoreApplication.translate("MainWindow", u"+", None)) + self.home_pb.setText(QCoreApplication.translate("MainWindow", u"Home", None)) + self.label_2.setText(QCoreApplication.translate("MainWindow", u"Current postion", None)) + self.label_8.setText(QCoreApplication.translate("MainWindow", u"Axis", None)) + # retranslateUi + diff --git a/frontend/ui/robot.ui b/frontend/ui/robot.ui index 6c537bb..5de7039 100644 --- a/frontend/ui/robot.ui +++ b/frontend/ui/robot.ui @@ -6,222 +6,313 @@ 0 0 - 503 - 218 + 532 + 408 MainWindow - - - - 10 - 10 - 481 - 152 - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Rz - - - - - - - Current postion - - - - - - - Rz' - - - - - - - Ry - - - - - - - Home - - - - - - - -180 - - - 180 - - - - - - - Move - - - - - - - false - - - - - - - Store - - - - - - - -180 - - - 180 - - - - - - - false - - - - - - - -180 - - - 180 - - - - - - - Move to - - - - - - - false - - - - - - - Jog - - - - - - - - - - - - + + + + + Settings + + - - - - - - - - - - - + - - + + + + + + + Serial port + + + + + + + Qt::Orientation::Horizontal + + + + 40 + 20 + + + + + + + + Connect + + + + + + + true + + + + + + + + + + Description + + + + + + - - - - - - - - - - - - - - - + - - - - - - - - - - - - - - - - - - - + + + + + + + Device controls + + + Qt::AlignmentFlag::AlignLeading|Qt::AlignmentFlag::AlignLeft|Qt::AlignmentFlag::AlignVCenter + + + false + + + + + + QLayout::SizeConstraint::SetDefaultConstraint - + + + + -180 + + + 180 + + + + + + + + + - + + + + + + + + + + + + + + + + + Ry + + + + + + + true + + + true + + + + + + + + + + + + + + Move to + + + Qt::AlignmentFlag::AlignCenter + + + + + + + -180 + + + 180 + + + + + + + + + - + + + + + + + + + + + + + + + + + Store + + + + + + + Rz + + + + + + + Rx + + + + + + + true + + + true + + + + + + + Move + + + + + + + Jog + + + Qt::AlignmentFlag::AlignCenter + + + + + + + + + - + + + + + + + + + + + + + + + + + Home + + + + + + + -180 + + + 180 + + + + + + + true + + + true + + + + + + + + 0 + 0 + + + + Current postion + + + Qt::AlignmentFlag::AlignCenter + + + + + + + Axis + + + + - - - + + + 0 0 - 503 - 22 + 532 + 23 diff --git a/frontend/ui/robot_ui.py b/frontend/ui/robot_ui.py deleted file mode 100644 index 9549b58..0000000 --- a/frontend/ui/robot_ui.py +++ /dev/null @@ -1,211 +0,0 @@ -# -*- coding: utf-8 -*- - -################################################################################ -## Form generated from reading UI file 'robot.ui' -## -## Created by: Qt User Interface Compiler version 6.7.2 -## -## WARNING! All changes made in this file will be lost when recompiling UI file! -################################################################################ - -from PySide6.QtCore import (QCoreApplication, QDate, QDateTime, QLocale, - QMetaObject, QObject, QPoint, QRect, - QSize, QTime, QUrl, Qt) -from PySide6.QtGui import (QBrush, QColor, QConicalGradient, QCursor, - QFont, QFontDatabase, QGradient, QIcon, - QImage, QKeySequence, QLinearGradient, QPainter, - QPalette, QPixmap, QRadialGradient, QTransform) -from PySide6.QtWidgets import (QApplication, QComboBox, QGridLayout, QHBoxLayout, - QLabel, QLineEdit, QMainWindow, QMenuBar, - QPushButton, QSizePolicy, QSpacerItem, QSpinBox, - QStatusBar, QWidget) - -class Ui_MainWindow(object): - def setupUi(self, MainWindow): - if not MainWindow.objectName(): - MainWindow.setObjectName(u"MainWindow") - MainWindow.resize(503, 218) - self.centralwidget = QWidget(MainWindow) - self.centralwidget.setObjectName(u"centralwidget") - self.gridLayoutWidget = QWidget(self.centralwidget) - self.gridLayoutWidget.setObjectName(u"gridLayoutWidget") - self.gridLayoutWidget.setGeometry(QRect(10, 10, 481, 152)) - self.gridLayout = QGridLayout(self.gridLayoutWidget) - self.gridLayout.setObjectName(u"gridLayout") - self.gridLayout.setContentsMargins(0, 0, 0, 0) - self.horizontalSpacer = QSpacerItem(40, 20, QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Minimum) - - self.gridLayout.addItem(self.horizontalSpacer, 0, 0, 1, 1) - - self.label_3 = QLabel(self.gridLayoutWidget) - self.label_3.setObjectName(u"label_3") - - self.gridLayout.addWidget(self.label_3, 1, 0, 1, 1) - - self.label_2 = QLabel(self.gridLayoutWidget) - self.label_2.setObjectName(u"label_2") - - self.gridLayout.addWidget(self.label_2, 0, 1, 1, 1) - - self.label_6 = QLabel(self.gridLayoutWidget) - self.label_6.setObjectName(u"label_6") - - self.gridLayout.addWidget(self.label_6, 3, 0, 1, 1) - - self.label_5 = QLabel(self.gridLayoutWidget) - self.label_5.setObjectName(u"label_5") - - self.gridLayout.addWidget(self.label_5, 2, 0, 1, 1) - - self.home_pb = QPushButton(self.gridLayoutWidget) - self.home_pb.setObjectName(u"home_pb") - - self.gridLayout.addWidget(self.home_pb, 4, 1, 1, 1) - - self.move_rz_sb = QSpinBox(self.gridLayoutWidget) - self.move_rz_sb.setObjectName(u"move_rz_sb") - self.move_rz_sb.setMinimum(-180) - self.move_rz_sb.setMaximum(180) - - self.gridLayout.addWidget(self.move_rz_sb, 3, 2, 1, 1) - - self.move_pb = QPushButton(self.gridLayoutWidget) - self.move_pb.setObjectName(u"move_pb") - - self.gridLayout.addWidget(self.move_pb, 4, 2, 1, 1) - - self.current_ry_le = QLineEdit(self.gridLayoutWidget) - self.current_ry_le.setObjectName(u"current_ry_le") - self.current_ry_le.setEnabled(False) - - self.gridLayout.addWidget(self.current_ry_le, 2, 1, 1, 1) - - self.store_pb = QPushButton(self.gridLayoutWidget) - self.store_pb.setObjectName(u"store_pb") - - self.gridLayout.addWidget(self.store_pb, 4, 0, 1, 1) - - self.move_ry_sb = QSpinBox(self.gridLayoutWidget) - self.move_ry_sb.setObjectName(u"move_ry_sb") - self.move_ry_sb.setMinimum(-180) - self.move_ry_sb.setMaximum(180) - - self.gridLayout.addWidget(self.move_ry_sb, 2, 2, 1, 1) - - self.current_rx_le = QLineEdit(self.gridLayoutWidget) - self.current_rx_le.setObjectName(u"current_rx_le") - self.current_rx_le.setEnabled(False) - - self.gridLayout.addWidget(self.current_rx_le, 1, 1, 1, 1) - - self.move_rx_sb = QSpinBox(self.gridLayoutWidget) - self.move_rx_sb.setObjectName(u"move_rx_sb") - self.move_rx_sb.setMinimum(-180) - self.move_rx_sb.setMaximum(180) - - self.gridLayout.addWidget(self.move_rx_sb, 1, 2, 1, 1) - - self.label_4 = QLabel(self.gridLayoutWidget) - self.label_4.setObjectName(u"label_4") - - self.gridLayout.addWidget(self.label_4, 0, 2, 1, 1) - - self.current_rz_le = QLineEdit(self.gridLayoutWidget) - self.current_rz_le.setObjectName(u"current_rz_le") - self.current_rz_le.setEnabled(False) - - self.gridLayout.addWidget(self.current_rz_le, 3, 1, 1, 1) - - self.label = QLabel(self.gridLayoutWidget) - self.label.setObjectName(u"label") - - self.gridLayout.addWidget(self.label, 0, 3, 1, 1) - - self.step_size_cb = QComboBox(self.gridLayoutWidget) - self.step_size_cb.setObjectName(u"step_size_cb") - - self.gridLayout.addWidget(self.step_size_cb, 4, 3, 1, 1) - - self.horizontalLayout = QHBoxLayout() - self.horizontalLayout.setObjectName(u"horizontalLayout") - self.jog_rz_neg_pb = QPushButton(self.gridLayoutWidget) - self.jog_rz_neg_pb.setObjectName(u"jog_rz_neg_pb") - - self.horizontalLayout.addWidget(self.jog_rz_neg_pb) - - self.jog_rz_pos_pb = QPushButton(self.gridLayoutWidget) - self.jog_rz_pos_pb.setObjectName(u"jog_rz_pos_pb") - - self.horizontalLayout.addWidget(self.jog_rz_pos_pb) - - - self.gridLayout.addLayout(self.horizontalLayout, 1, 3, 1, 1) - - self.horizontalLayout_2 = QHBoxLayout() - self.horizontalLayout_2.setObjectName(u"horizontalLayout_2") - self.jog_ry_neg_pb = QPushButton(self.gridLayoutWidget) - self.jog_ry_neg_pb.setObjectName(u"jog_ry_neg_pb") - - self.horizontalLayout_2.addWidget(self.jog_ry_neg_pb) - - self.jog_ry_pos_pb = QPushButton(self.gridLayoutWidget) - self.jog_ry_pos_pb.setObjectName(u"jog_ry_pos_pb") - - self.horizontalLayout_2.addWidget(self.jog_ry_pos_pb) - - - self.gridLayout.addLayout(self.horizontalLayout_2, 2, 3, 1, 1) - - self.horizontalLayout_3 = QHBoxLayout() - self.horizontalLayout_3.setObjectName(u"horizontalLayout_3") - self.jog_rz1_neg_pb = QPushButton(self.gridLayoutWidget) - self.jog_rz1_neg_pb.setObjectName(u"jog_rz1_neg_pb") - - self.horizontalLayout_3.addWidget(self.jog_rz1_neg_pb) - - self.jog_rz1_pos_pb = QPushButton(self.gridLayoutWidget) - self.jog_rz1_pos_pb.setObjectName(u"jog_rz1_pos_pb") - - self.horizontalLayout_3.addWidget(self.jog_rz1_pos_pb) - - - self.gridLayout.addLayout(self.horizontalLayout_3, 3, 3, 1, 1) - - self.gridLayout.setColumnStretch(0, 1) - self.gridLayout.setColumnStretch(1, 1) - self.gridLayout.setColumnStretch(2, 1) - self.gridLayout.setColumnStretch(3, 1) - MainWindow.setCentralWidget(self.centralwidget) - self.menubar = QMenuBar(MainWindow) - self.menubar.setObjectName(u"menubar") - self.menubar.setGeometry(QRect(0, 0, 503, 22)) - MainWindow.setMenuBar(self.menubar) - self.statusbar = QStatusBar(MainWindow) - self.statusbar.setObjectName(u"statusbar") - MainWindow.setStatusBar(self.statusbar) - - self.retranslateUi(MainWindow) - - QMetaObject.connectSlotsByName(MainWindow) - # setupUi - - def retranslateUi(self, MainWindow): - MainWindow.setWindowTitle(QCoreApplication.translate("MainWindow", u"MainWindow", None)) - self.label_3.setText(QCoreApplication.translate("MainWindow", u"Rz", None)) - self.label_2.setText(QCoreApplication.translate("MainWindow", u"Current postion", None)) - self.label_6.setText(QCoreApplication.translate("MainWindow", u"Rz'", None)) - self.label_5.setText(QCoreApplication.translate("MainWindow", u"Ry", None)) - self.home_pb.setText(QCoreApplication.translate("MainWindow", u"Home", None)) - self.move_pb.setText(QCoreApplication.translate("MainWindow", u"Move", None)) - self.store_pb.setText(QCoreApplication.translate("MainWindow", u"Store", None)) - self.label_4.setText(QCoreApplication.translate("MainWindow", u"Move to", None)) - self.label.setText(QCoreApplication.translate("MainWindow", u"Jog", None)) - self.step_size_cb.setCurrentText("") - self.jog_rz_neg_pb.setText(QCoreApplication.translate("MainWindow", u"-", None)) - self.jog_rz_pos_pb.setText(QCoreApplication.translate("MainWindow", u"+", None)) - self.jog_ry_neg_pb.setText(QCoreApplication.translate("MainWindow", u"-", None)) - self.jog_ry_pos_pb.setText(QCoreApplication.translate("MainWindow", u"+", None)) - self.jog_rz1_neg_pb.setText(QCoreApplication.translate("MainWindow", u"-", None)) - self.jog_rz1_pos_pb.setText(QCoreApplication.translate("MainWindow", u"+", None)) - # retranslateUi - diff --git a/pyproject.toml b/pyproject.toml index 380b99e..58c38d9 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [tool.poetry] name = "pyspj" -version = "0.1.0" +version = "0.1.1" description = "This pakages is used to control the Spherical Parallel Joint Robot" authors = ["mattia.gallacchi "] readme = "README.md" diff --git a/pyspj/__init__.py b/pyspj/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyspj/spj.py b/pyspj/spj.py index 12d3d39..9e31015 100644 --- a/pyspj/spj.py +++ b/pyspj/spj.py @@ -2,56 +2,119 @@ import numpy as np import serial from dataclasses import dataclass import time -from threading import Thread, Lock, Event -import weakref +from threading import Thread, Lock, Event, main_thread +from itertools import combinations_with_replacement +import json +from pathlib import Path class SpjException(Exception): + """Generic class exception""" pass class NotConnected(SpjException): + """Serial device not connected + """ pass @dataclass class WorldPosition: - + """World position Rz, Ry, Rx + """ + rz : float = 0.0 ry : float = 0.0 rx : float = 0.0 -def thread_func(weak_self): - """Thread function to constantly read the position - """ +_ser: serial.Serial = None + +class WorkerThread: + + def __init__(self): + + self._stop_event = Event() + self._thread: Thread = None + self._lock: Lock = Lock() + self._current_pos: list[float] = [0, 0, 0] + + def start(self) -> None: + + self._stop_event.clear() + self._thread = Thread(target=self.__run) + self._thread.start() + + def stop(self) -> None: - while True: + self._stop_event.set() + self._thread.join() + + @property + def running(self) -> bool: + return self._thread.is_alive() + + @property + def current_pos(self) -> list[float]: + + if self._lock.acquire(blocking=False): + steps = self._current_pos + self._lock.release() + else: + steps = [0.0, 0.0, 0.0] + + return steps - self = weak_self() - # When main class fall out of scope - if self is None: - break + def __run(self): - if self._thread_stop_event.is_set(): - break + global _ser - new_pos = self._ser.read_until() + while not self._stop_event.is_set() and main_thread().is_alive(): - if len(new_pos) > 0: - steps = new_pos.decode("utf-8").strip().split(",") - steps = [float(x) for x in steps] + new_pos = _ser.read_until() - if self._position_thread_lock.acquire(blocking=False): - self._current_pos = steps - self._position_thread_lock.release() - - del self - time.sleep(0.001) + if len(new_pos) > 0: + steps = new_pos.decode("utf-8").strip().split(",") + steps = [float(x) for x in steps] + if self._lock.acquire(blocking=False): + self._current_pos = steps + self._lock.release() + + + time.sleep(0.001) +_worker_thread: WorkerThread = None + class SphericalParallelJoint: - STORE_POS = WorldPosition(-60, 0, 0) - HOME_POS = WorldPosition(0, 0, 0) + + @staticmethod + def home_pos() -> WorldPosition: + return WorldPosition(0, 0, 0) + + @staticmethod + def store_pos() -> WorldPosition: + return WorldPosition(-60, 0, 0) def __init__(self) -> None: - """Constructor""" + """Constructor + + Example + ------- + .. code-block:: python + + robot = SphericalParallelJoint() + robot.connect() + + print("Move sync") + if not robot.move_sync(robot.HOME_POS): + print("Move failed") + + print("Move async") + robot.move_async(robot.STORE_POS) + time.sleep(4) + + print(f"Current position: {robot.get_current_position()}") + + robot.close() + """ self._rot_axis = np.array([ [ -1.0 / np.sqrt(3.0), -1.0 / np.sqrt(3.0), - 1.0 / np.sqrt(3.0) ], @@ -61,10 +124,7 @@ class SphericalParallelJoint: ]) - self._current_pos: list[float] = [] - self._thread_stop_event = Event() - self._position_thread_lock = Lock() - self._ser = None + self._current_pos: list[float] = self.store_pos() self._connected = False def connect(self, port : str = "/dev/ttyACM0", baud : int = 115200): @@ -82,7 +142,10 @@ class SphericalParallelJoint: Runtime error """ - self._ser = serial.Serial(port, baudrate=baud, timeout=1) + global _worker_thread + global _ser + + _ser = serial.Serial(port, baudrate=baud, timeout=1) if not self.check_online(): raise RuntimeError("Failed to communicate with device") @@ -90,21 +153,25 @@ class SphericalParallelJoint: self._connected = True # Empty the read buffer - self._ser.read_all() + _ser.read_all() # Start the thread - self._thread_stop_event.clear() - self._position_thread = Thread(target=thread_func, args=(weakref.ref(self), )) - self._position_thread.start() + _worker_thread= WorkerThread() + _worker_thread.start() + time.sleep(0.01) + self._current_pos = self.get_current_position() def __check_connected(self): + """Check if the connect method was called + + Raise + ----- + NotConnected + """ if not self._connected: raise NotConnected("Use the connect method to establish a connection to the device") - # def __position_thread_func(self): - - def __get_rotational_matrix(self, index : int, angle : float) -> np.ndarray: """Compute the rotational matrix for one axis @@ -174,11 +241,27 @@ class SphericalParallelJoint: return[step1, step2, step3] def __move(self, pos : WorldPosition) -> list[float]: - + """Do move the device + + Parameters + ---------- + pos: WorldPosition + Position to move to + + Return + ------ + End position in steps + """ + + global _ser + self.__check_connected() steps = self.compute_steps(pos) - self._ser.write(f"{steps[0]},{steps[1]},{steps[2]}\n".encode("utf-8")) + _bytes =_ser.write(f"{steps[0]},{steps[1]},{steps[2]}\n".encode("utf-8")) + if _bytes < 1: + print("Failed to write") + steps = self._current_pos return [float(steps[0]),float(steps[1]),float(steps[2])] @@ -215,7 +298,6 @@ class SphericalParallelJoint: max_errors -= 1 continue - # print(current_pos, end_pos) if current_pos == end_pos: break @@ -225,6 +307,17 @@ class SphericalParallelJoint: return True def move_async(self, pos: WorldPosition) -> list[float]: + """Move the robot asynchronously. Return before movement ends. + + Parameters + ---------- + pos: WorldPosition + Position to move to + + Return + ------ + End position in steps + """ return self.__move(pos) @@ -234,15 +327,17 @@ class SphericalParallelJoint: Return ------ True if online - """ + """ - msg = self._ser.read_until().decode("utf-8").strip() + global _ser + + msg = _ser.read_until().decode("utf-8").strip() if msg == "Wait": - self._ser.timeout = 5 - self._ser.write("\n".encode("utf-8")) - msg = self._ser.read_until().decode("utf-8").strip() - self._ser.timeout = 1 + _ser.timeout = 5 + _ser.write("\n".encode("utf-8")) + msg = _ser.read_until().decode("utf-8").strip() + _ser.timeout = 1 if msg == "Ready": return True return False @@ -260,55 +355,96 @@ class SphericalParallelJoint: A list in the format (stepZ, stepY, stepX) """ + global _worker_thread + self.__check_connected() - if self._position_thread_lock.acquire(blocking=False): - steps = self._current_pos - self._position_thread_lock.release() - + self._current_pos = _worker_thread._current_pos + # Sleep to avoid overloading the lock time.sleep(0.001) - return steps + return self._current_pos def close(self): """End communication with the device """ - if self._position_thread.is_alive(): - self._thread_stop_event.set() - self._position_thread.join() + global _worker_thread + global _ser + + if not self._connected: + return + + if _worker_thread.running: + _worker_thread.stop() - self._ser.close() - self._ser = None + _ser.close() self._connected = False def __del__(self): """Destructor """ - print("Destructor") + if self._connected: self.close() + def save_lookup_table(self): + + angles = [x for x in range(-180, 181)] + + # print(comb) + lut_steps: list[tuple] = [] + lut_angles: list[tuple] = list(combinations_with_replacement(angles, 3)) + + for c in lut_angles: + pos = WorldPosition(*c) + lut_steps.append(tuple(map(int, self.compute_steps(pos)))) + + with open("spj_lut.json", "w") as file: + data = { + "angles" : lut_angles, + "steps" : lut_steps + } + + file.write(json.dumps(data, indent=4)) + # index = lut_angles.index((0,0,0)) + # print(index) + # print(lut_steps[index]) + + def load_lookup_table(self, filepath: Path) -> tuple[list[tuple],list[tuple]]: + + with open(filepath, "r") as file: + data = json.loads(file.read()) + + return (data["angles"], data["steps"]) + if "__main__" == __name__: + + import os + os.chdir(os.path.dirname(__file__)) + robot = SphericalParallelJoint() - robot.connect() + angles, steps = robot.load_lookup_table("spj_lut.json") - print("Move sync") - if not robot.move_sync(robot.HOME_POS): - print("Move failed") + print("Searching angle") + # print(angles) + index = angles.index([0,0,0]) + print(index) - print("Move async") - robot.move_async(robot.STORE_POS) - time.sleep(4) - robot.close() + print(steps[index]) + # robot.connect() - robot.connect() + # print("Move sync") + # if not robot.move_sync(robot.HOME_POS): + # print("Move failed") - robot.move_sync(robot.HOME_POS) - robot.move_async(robot.STORE_POS) + # print("Move async") + # robot.move_async(robot.STORE_POS) + # time.sleep(4) - # robot.close() + # print(f"Current position: {robot.get_current_position()}") - print("Bye") \ No newline at end of file + # robot.close() + \ No newline at end of file -- GitLab From c23315852f29225c8b404f979de0d7db03369577 Mon Sep 17 00:00:00 2001 From: "mattia.gallacchi" Date: Thu, 14 Nov 2024 08:43:48 +0100 Subject: [PATCH 3/5] Lint and Black Signed-off-by: mattia.gallacchi --- poetry.lock | 380 +++++++++++++++++++++++++++++++++++------------- pyproject.toml | 10 +- pyspj/spj.py | 385 ++++++++++++++++++++++++------------------------- 3 files changed, 480 insertions(+), 295 deletions(-) diff --git a/poetry.lock b/poetry.lock index 215921d..439e81a 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1,3 +1,95 @@ +[[package]] +name = "astroid" +version = "3.3.5" +description = "An abstract syntax tree for Python with inference support." +category = "dev" +optional = false +python-versions = ">=3.9.0" + +[package.dependencies] +typing-extensions = {version = ">=4.0.0", markers = "python_version < \"3.11\""} + +[[package]] +name = "black" +version = "24.10.0" +description = "The uncompromising code formatter." +category = "dev" +optional = false +python-versions = ">=3.9" + +[package.dependencies] +click = ">=8.0.0" +mypy-extensions = ">=0.4.3" +packaging = ">=22.0" +pathspec = ">=0.9.0" +platformdirs = ">=2" +tomli = {version = ">=1.1.0", markers = "python_version < \"3.11\""} +typing-extensions = {version = ">=4.0.1", markers = "python_version < \"3.11\""} + +[package.extras] +colorama = ["colorama (>=0.4.3)"] +d = ["aiohttp (>=3.10)"] +jupyter = ["ipython (>=7.8.0)", "tokenize-rt (>=3.2.0)"] +uvloop = ["uvloop (>=0.15.2)"] + +[[package]] +name = "click" +version = "8.1.7" +description = "Composable command line interface toolkit" +category = "dev" +optional = false +python-versions = ">=3.7" + +[package.dependencies] +colorama = {version = "*", markers = "platform_system == \"Windows\""} + +[[package]] +name = "colorama" +version = "0.4.6" +description = "Cross-platform colored terminal text." +category = "dev" +optional = false +python-versions = "!=3.0.*,!=3.1.*,!=3.2.*,!=3.3.*,!=3.4.*,!=3.5.*,!=3.6.*,>=2.7" + +[[package]] +name = "dill" +version = "0.3.9" +description = "serialize all of Python" +category = "dev" +optional = false +python-versions = ">=3.8" + +[package.extras] +graph = ["objgraph (>=1.7.2)"] +profile = ["gprof2dot (>=2022.7.29)"] + +[[package]] +name = "isort" +version = "5.13.2" +description = "A Python utility / library to sort Python imports." +category = "dev" +optional = false +python-versions = ">=3.8.0" + +[package.extras] +colors = ["colorama (>=0.4.6)"] + +[[package]] +name = "mccabe" +version = "0.7.0" +description = "McCabe checker, plugin for flake8" +category = "dev" +optional = false +python-versions = ">=3.6" + +[[package]] +name = "mypy-extensions" +version = "1.0.0" +description = "Type system extensions for programs checked with the mypy type checker." +category = "dev" +optional = false +python-versions = ">=3.5" + [[package]] name = "numpy" version = "2.1.1" @@ -7,12 +99,59 @@ optional = false python-versions = ">=3.10" [[package]] -name = "pygame" -version = "2.6.0" -description = "Python Game Development" -category = "main" +name = "packaging" +version = "24.2" +description = "Core utilities for Python packages" +category = "dev" optional = false -python-versions = ">=3.6" +python-versions = ">=3.8" + +[[package]] +name = "pathspec" +version = "0.12.1" +description = "Utility library for gitignore style pattern matching of file paths." +category = "dev" +optional = false +python-versions = ">=3.8" + +[[package]] +name = "platformdirs" +version = "4.3.6" +description = "A small Python package for determining appropriate platform-specific dirs, e.g. a `user data dir`." +category = "dev" +optional = false +python-versions = ">=3.8" + +[package.extras] +docs = ["furo (>=2024.8.6)", "proselint (>=0.14)", "sphinx (>=8.0.2)", "sphinx-autodoc-typehints (>=2.4)"] +test = ["appdirs (==1.4.4)", "covdefaults (>=2.3)", "pytest (>=8.3.2)", "pytest-cov (>=5)", "pytest-mock (>=3.14)"] +type = ["mypy (>=1.11.2)"] + +[[package]] +name = "pylint" +version = "3.3.1" +description = "python code static checker" +category = "dev" +optional = false +python-versions = ">=3.9.0" + +[package.dependencies] +astroid = ">=3.3.4,<=3.4.0-dev0" +colorama = {version = ">=0.4.5", markers = "sys_platform == \"win32\""} +dill = [ + {version = ">=0.2", markers = "python_version < \"3.11\""}, + {version = ">=0.3.6", markers = "python_version >= \"3.11\""}, + {version = ">=0.3.7", markers = "python_version >= \"3.12\""}, +] +isort = ">=4.2.5,<5.13.0 || >5.13.0,<6" +mccabe = ">=0.6,<0.8" +platformdirs = ">=2.2.0" +tomli = {version = ">=1.1.0", markers = "python_version < \"3.11\""} +tomlkit = ">=0.10.1" + +[package.extras] +spelling = ["pyenchant (>=3.2,<4.0)"] +testutils = ["gitpython (>3)"] [[package]] name = "pyserial" @@ -27,54 +166,130 @@ cp2110 = ["hidapi"] [[package]] name = "PySide6" -version = "6.7.2" +version = "6.8.0.2" description = "Python bindings for the Qt cross-platform application and UI framework" -category = "main" +category = "dev" optional = false -python-versions = "<3.13,>=3.9" +python-versions = "<3.14,>=3.9" [package.dependencies] -PySide6-Addons = "6.7.2" -PySide6-Essentials = "6.7.2" -shiboken6 = "6.7.2" +PySide6-Addons = "6.8.0.2" +PySide6-Essentials = "6.8.0.2" +shiboken6 = "6.8.0.2" [[package]] name = "PySide6-Addons" -version = "6.7.2" +version = "6.8.0.2" description = "Python bindings for the Qt cross-platform application and UI framework (Addons)" -category = "main" +category = "dev" optional = false -python-versions = "<3.13,>=3.9" +python-versions = "<3.14,>=3.9" [package.dependencies] -PySide6-Essentials = "6.7.2" -shiboken6 = "6.7.2" +PySide6-Essentials = "6.8.0.2" +shiboken6 = "6.8.0.2" [[package]] name = "PySide6-Essentials" -version = "6.7.2" +version = "6.8.0.2" description = "Python bindings for the Qt cross-platform application and UI framework (Essentials)" -category = "main" +category = "dev" optional = false -python-versions = "<3.13,>=3.9" +python-versions = "<3.14,>=3.9" [package.dependencies] -shiboken6 = "6.7.2" +shiboken6 = "6.8.0.2" [[package]] name = "shiboken6" -version = "6.7.2" +version = "6.8.0.2" description = "Python/C++ bindings helper module" -category = "main" +category = "dev" +optional = false +python-versions = "<3.14,>=3.9" + +[[package]] +name = "tomli" +version = "2.1.0" +description = "A lil' TOML parser" +category = "dev" optional = false -python-versions = "<3.13,>=3.9" +python-versions = ">=3.8" + +[[package]] +name = "tomlkit" +version = "0.13.2" +description = "Style preserving TOML library" +category = "dev" +optional = false +python-versions = ">=3.8" + +[[package]] +name = "typing-extensions" +version = "4.12.2" +description = "Backported and Experimental Type Hints for Python 3.8+" +category = "dev" +optional = false +python-versions = ">=3.8" [metadata] lock-version = "1.1" python-versions = ">=3.10,<3.13" -content-hash = "fae3f257d24cbe79ed14dbbce1cb53d082ca03dbbddb693ec39fd3b4bbeb0b3e" +content-hash = "d111e74c491b2525c3502af1768abda4018c1153674223e28ff9d064ebafe6f3" [metadata.files] +astroid = [ + {file = "astroid-3.3.5-py3-none-any.whl", hash = "sha256:a9d1c946ada25098d790e079ba2a1b112157278f3fb7e718ae6a9252f5835dc8"}, + {file = "astroid-3.3.5.tar.gz", hash = "sha256:5cfc40ae9f68311075d27ef68a4841bdc5cc7f6cf86671b49f00607d30188e2d"}, +] +black = [ + {file = "black-24.10.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:e6668650ea4b685440857138e5fe40cde4d652633b1bdffc62933d0db4ed9812"}, + {file = "black-24.10.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:1c536fcf674217e87b8cc3657b81809d3c085d7bf3ef262ead700da345bfa6ea"}, + {file = "black-24.10.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:649fff99a20bd06c6f727d2a27f401331dc0cc861fb69cde910fe95b01b5928f"}, + {file = "black-24.10.0-cp310-cp310-win_amd64.whl", hash = "sha256:fe4d6476887de70546212c99ac9bd803d90b42fc4767f058a0baa895013fbb3e"}, + {file = "black-24.10.0-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:5a2221696a8224e335c28816a9d331a6c2ae15a2ee34ec857dcf3e45dbfa99ad"}, + {file = "black-24.10.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:f9da3333530dbcecc1be13e69c250ed8dfa67f43c4005fb537bb426e19200d50"}, + {file = "black-24.10.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:4007b1393d902b48b36958a216c20c4482f601569d19ed1df294a496eb366392"}, + {file = "black-24.10.0-cp311-cp311-win_amd64.whl", hash = "sha256:394d4ddc64782e51153eadcaaca95144ac4c35e27ef9b0a42e121ae7e57a9175"}, + {file = "black-24.10.0-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:b5e39e0fae001df40f95bd8cc36b9165c5e2ea88900167bddf258bacef9bbdc3"}, + {file = "black-24.10.0-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:d37d422772111794b26757c5b55a3eade028aa3fde43121ab7b673d050949d65"}, + {file = "black-24.10.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:14b3502784f09ce2443830e3133dacf2c0110d45191ed470ecb04d0f5f6fcb0f"}, + {file = "black-24.10.0-cp312-cp312-win_amd64.whl", hash = "sha256:30d2c30dc5139211dda799758559d1b049f7f14c580c409d6ad925b74a4208a8"}, + {file = "black-24.10.0-cp313-cp313-macosx_10_13_x86_64.whl", hash = "sha256:1cbacacb19e922a1d75ef2b6ccaefcd6e93a2c05ede32f06a21386a04cedb981"}, + {file = "black-24.10.0-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:1f93102e0c5bb3907451063e08b9876dbeac810e7da5a8bfb7aeb5a9ef89066b"}, + {file = "black-24.10.0-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:ddacb691cdcdf77b96f549cf9591701d8db36b2f19519373d60d31746068dbf2"}, + {file = "black-24.10.0-cp313-cp313-win_amd64.whl", hash = "sha256:680359d932801c76d2e9c9068d05c6b107f2584b2a5b88831c83962eb9984c1b"}, + {file = "black-24.10.0-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:17374989640fbca88b6a448129cd1745c5eb8d9547b464f281b251dd00155ccd"}, + {file = "black-24.10.0-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:63f626344343083322233f175aaf372d326de8436f5928c042639a4afbbf1d3f"}, + {file = "black-24.10.0-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:ccfa1d0cb6200857f1923b602f978386a3a2758a65b52e0950299ea014be6800"}, + {file = "black-24.10.0-cp39-cp39-win_amd64.whl", hash = "sha256:2cd9c95431d94adc56600710f8813ee27eea544dd118d45896bb734e9d7a0dc7"}, + {file = "black-24.10.0-py3-none-any.whl", hash = "sha256:3bb2b7a1f7b685f85b11fed1ef10f8a9148bceb49853e47a294a3dd963c1dd7d"}, + {file = "black-24.10.0.tar.gz", hash = "sha256:846ea64c97afe3bc677b761787993be4991810ecc7a4a937816dd6bddedc4875"}, +] +click = [ + {file = "click-8.1.7-py3-none-any.whl", hash = "sha256:ae74fb96c20a0277a1d615f1e4d73c8414f5a98db8b799a7931d1582f3390c28"}, + {file = "click-8.1.7.tar.gz", hash = "sha256:ca9853ad459e787e2192211578cc907e7594e294c7ccc834310722b41b9ca6de"}, +] +colorama = [ + {file = "colorama-0.4.6-py2.py3-none-any.whl", hash = "sha256:4f1d9991f5acc0ca119f9d443620b77f9d6b33703e51011c16baf57afb285fc6"}, + {file = "colorama-0.4.6.tar.gz", hash = "sha256:08695f5cb7ed6e0531a20572697297273c47b8cae5a63ffc6d6ed5c201be6e44"}, +] +dill = [ + {file = "dill-0.3.9-py3-none-any.whl", hash = "sha256:468dff3b89520b474c0397703366b7b95eebe6303f108adf9b19da1f702be87a"}, + {file = "dill-0.3.9.tar.gz", hash = "sha256:81aa267dddf68cbfe8029c42ca9ec6a4ab3b22371d1c450abc54422577b4512c"}, +] +isort = [ + {file = "isort-5.13.2-py3-none-any.whl", hash = "sha256:8ca5e72a8d85860d5a3fa69b8745237f2939afe12dbf656afbcb47fe72d947a6"}, + {file = "isort-5.13.2.tar.gz", hash = "sha256:48fdfcb9face5d58a4f6dde2e72a1fb8dcaf8ab26f95ab49fab84c2ddefb0109"}, +] +mccabe = [ + {file = "mccabe-0.7.0-py2.py3-none-any.whl", hash = "sha256:6c2d30ab6be0e4a46919781807b4f0d834ebdd6c6e3dca0bda5a15f863427b6e"}, + {file = "mccabe-0.7.0.tar.gz", hash = "sha256:348e0240c33b60bbdf4e523192ef919f28cb2c3d7d5c7794f74009290f236325"}, +] +mypy-extensions = [ + {file = "mypy_extensions-1.0.0-py3-none-any.whl", hash = "sha256:4392f6c0eb8a5668a69e23d168ffa70f0be9ccfd32b5cc2d26a34ae5b844552d"}, + {file = "mypy_extensions-1.0.0.tar.gz", hash = "sha256:75dbf8955dc00442a438fc4d0666508a9a97b6bd41aa2f0ffe9d2f2725af0782"}, +] numpy = [ {file = "numpy-2.1.1-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:c8a0e34993b510fc19b9a2ce7f31cb8e94ecf6e924a40c0c9dd4f62d0aac47d9"}, {file = "numpy-2.1.1-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:7dd86dfaf7c900c0bbdcb8b16e2f6ddf1eb1fe39c6c8cca6e94844ed3152a8fd"}, @@ -130,90 +345,59 @@ numpy = [ {file = "numpy-2.1.1-pp310-pypy310_pp73-win_amd64.whl", hash = "sha256:99f4a9ee60eed1385a86e82288971a51e71df052ed0b2900ed30bc840c0f2e39"}, {file = "numpy-2.1.1.tar.gz", hash = "sha256:d0cf7d55b1051387807405b3898efafa862997b4cba8aa5dbe657be794afeafd"}, ] -pygame = [ - {file = "pygame-2.6.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:e5707aa9d029752495b3eddc1edff62e0e390a02f699b0f1ce77fe0b8c70ea4f"}, - {file = "pygame-2.6.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:d3ed0547368733b854c0d9981c982a3cdfabfa01b477d095c57bf47f2199da44"}, - {file = "pygame-2.6.0-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:6050f3e95f1f16602153d616b52619c6a2041cee7040eb529f65689e9633fc3e"}, - {file = "pygame-2.6.0-cp310-cp310-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:89be55b7e9e22e0eea08af9d6cfb97aed5da780f0b3a035803437d481a16d972"}, - {file = "pygame-2.6.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:7d65fb222eea1294cfc8206d9e5754d476a1673eb2783c03c4f70e0455320274"}, - {file = "pygame-2.6.0-cp310-cp310-win32.whl", hash = "sha256:71eebb9803cb350298de188fb7cdd3ebf13299f78d59a71c7e81efc649aae348"}, - {file = "pygame-2.6.0-cp310-cp310-win_amd64.whl", hash = "sha256:1551852a2cd5b4139a752888f6cbeeb4a96fc0fe6e6f3f8b9d9784eb8fceab13"}, - {file = "pygame-2.6.0-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:f6e5e6c010b1bf429388acf4d41d7ab2f7ad8fbf241d0db822102d35c9a2eb84"}, - {file = "pygame-2.6.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:99902f4a2f6a338057200d99b5120a600c27a9f629ca012a9b0087c045508d08"}, - {file = "pygame-2.6.0-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:6a284664978a1989c1e31a0888b2f70cfbcbafdfa3bb310e750b0d3366416225"}, - {file = "pygame-2.6.0-cp311-cp311-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:829623cee298b3dbaa1dd9f52c3051ae82f04cad7708c8c67cb9a1a4b8fd3c0b"}, - {file = "pygame-2.6.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6acf7949ed764487d51123f4f3606e8f76b0df167fef12ef73ef423c35fdea39"}, - {file = "pygame-2.6.0-cp311-cp311-win32.whl", hash = "sha256:3f809560c99bd1fb4716610eca0cd36412528f03da1a63841a347b71d0c604ee"}, - {file = "pygame-2.6.0-cp311-cp311-win_amd64.whl", hash = "sha256:6897ab87f9193510a774a3483e00debfe166f340ca159f544ef99807e2a44ec4"}, - {file = "pygame-2.6.0-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:b834711ebc8b9d0c2a5f9bfae4403dd277b2c61bcb689e1aa630d01a1ebcf40a"}, - {file = "pygame-2.6.0-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:b5ac288655e8a31a303cc286e79cc57979ed2ba19c3a14042d4b6391c1d3bed2"}, - {file = "pygame-2.6.0-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:d666667b7826b0a7921b8ce0a282ba5281dfa106976c1a3b24e32a0af65ad3b1"}, - {file = "pygame-2.6.0-cp312-cp312-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:fd8848a37a7cee37854c7efb8d451334477c9f8ce7ac339c079e724dc1334a76"}, - {file = "pygame-2.6.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:315e7b3c1c573984f549ac5da9778ac4709b3b4e3a4061050d94eab63fa4fe31"}, - {file = "pygame-2.6.0-cp312-cp312-win32.whl", hash = "sha256:e44bde0840cc21a91c9d368846ac538d106cf0668be1a6030f48df139609d1e8"}, - {file = "pygame-2.6.0-cp312-cp312-win_amd64.whl", hash = "sha256:1c429824b1f881a7a5ce3b5c2014d3d182aa45a22cea33c8347a3971a5446907"}, - {file = "pygame-2.6.0-cp36-cp36m-macosx_10_9_x86_64.whl", hash = "sha256:b832200bd8b6fc485e087bf3ef7ec1a21437258536413a5386088f5dcd3a9870"}, - {file = "pygame-2.6.0-cp36-cp36m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:098029d01a46ea4e30620dfb7c28a577070b456c8fc96350dde05f85c0bf51b5"}, - {file = "pygame-2.6.0-cp36-cp36m-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:a858bbdeac5ec473ec9e726c55fb8fbdc2f4aad7c55110e899883738071c7c9b"}, - {file = "pygame-2.6.0-cp36-cp36m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6f908762941fd99e1f66d1211d26383184f6045c45673443138b214bf48a89aa"}, - {file = "pygame-2.6.0-cp36-cp36m-win32.whl", hash = "sha256:4a63daee99d050f47d6ec7fa7dbd1c6597b8f082cdd58b6918d382d2bc31262d"}, - {file = "pygame-2.6.0-cp36-cp36m-win_amd64.whl", hash = "sha256:ace471b3849d68968e5427fc01166ef5afaf552a5c442fc2c28d3b7226786f55"}, - {file = "pygame-2.6.0-cp37-cp37m-macosx_10_9_x86_64.whl", hash = "sha256:fea019713d0c89dfd5909225aa933010100035d1cd30e6c936e8b6f00529fb80"}, - {file = "pygame-2.6.0-cp37-cp37m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:249dbf2d51d9f0266009a380ccf0532e1a57614a1528bb2f89a802b01d61f93e"}, - {file = "pygame-2.6.0-cp37-cp37m-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:8cb51533ee3204e8160600b0de34eaad70eb913a182c94a7777b6051e8fc52f1"}, - {file = "pygame-2.6.0-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4f637636a44712e94e5601ec69160a080214626471983dfb0b5b68aa0c61563d"}, - {file = "pygame-2.6.0-cp37-cp37m-win32.whl", hash = "sha256:e432156b6f346f4cc6cab03ce9657600093390f4c9b10bf458716b25beebfe33"}, - {file = "pygame-2.6.0-cp37-cp37m-win_amd64.whl", hash = "sha256:a0194652db7874bdde7dfc69d659ca954544c012e04ae527151325bfb970f423"}, - {file = "pygame-2.6.0-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:eae3ee62cc172e268121d5bd9dc406a67094d33517de3a91de3323d6ae23eb02"}, - {file = "pygame-2.6.0-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:f6a58b0a5a8740a3c2cf6fc5366888bd4514561253437f093c12a9ab4fb3ecae"}, - {file = "pygame-2.6.0-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:c71da36997dc7b9b4ee973fa3a5d4a6cfb2149161b5b1c08b712d2f13a63ccfe"}, - {file = "pygame-2.6.0-cp38-cp38-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:b86771801a7fc10d9a62218f27f1d5c13341c3a27394aa25578443a9cd199830"}, - {file = "pygame-2.6.0-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4928f3acf5a9ce5fbab384c21f1245304535ffd5fb167ae92a6b4d3cdb55a3b6"}, - {file = "pygame-2.6.0-cp38-cp38-win32.whl", hash = "sha256:4faab2df9926c4d31215986536b112f0d76f711cf02f395805f1ff5df8fd55fc"}, - {file = "pygame-2.6.0-cp38-cp38-win_amd64.whl", hash = "sha256:afbb8d97aed93dfb116fe105603dacb68f8dab05b978a40a9e4ab1b6c1f683fd"}, - {file = "pygame-2.6.0-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:d11f3646b53819892f4a731e80b8589a9140343d0d4b86b826802191b241228c"}, - {file = "pygame-2.6.0-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:5ef92ed93c354eabff4b85e457d4d6980115004ec7ff52a19fd38b929c3b80fb"}, - {file = "pygame-2.6.0-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:9bc1795f2e36302882546faacd5a0191463c4f4ae2b90e7c334a7733aa4190d2"}, - {file = "pygame-2.6.0-cp39-cp39-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:e92294fcc85c4955fe5bc6a0404e4cc870808005dc8f359e881544e3cc214108"}, - {file = "pygame-2.6.0-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:c0cb7bdf3ee0233a3ac02ef777c01dfe315e6d4670f1312c83b91c1ef124359a"}, - {file = "pygame-2.6.0-cp39-cp39-win32.whl", hash = "sha256:ac906478ae489bb837bf6d2ae1eb9261d658aa2c34fa5b283027a04149bda81a"}, - {file = "pygame-2.6.0-cp39-cp39-win_amd64.whl", hash = "sha256:92cf12a9722f6f0bdc5520d8925a8f085cff9c054a2ea462fc409cba3781be27"}, - {file = "pygame-2.6.0-pp36-pypy36_pp73-win32.whl", hash = "sha256:a6636f452fdaddf604a060849feb84c056930b6a3c036214f607741f16aac942"}, - {file = "pygame-2.6.0-pp37-pypy37_pp73-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:3dc242dc15d067d10f25c5b12a1da48ca9436d8e2d72353eaf757e83612fba2f"}, - {file = "pygame-2.6.0-pp37-pypy37_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f82df23598a281c8c342d3c90be213c8fe762a26c15815511f60d0aac6e03a70"}, - {file = "pygame-2.6.0-pp38-pypy38_pp73-macosx_10_9_x86_64.whl", hash = "sha256:2ed2539bb6bd211fc570b1169dc4a64a74ec5cd95741e62a0ab46bd18fe08e0d"}, - {file = "pygame-2.6.0-pp38-pypy38_pp73-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:904aaf29710c6b03a7e1a65b198f5467ed6525e8e60bdcc5e90ff8584c1d54ea"}, - {file = "pygame-2.6.0-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:fcd28f96f0fffd28e71a98773843074597e10d7f55a098e2e5bcb2bef1bdcbf5"}, - {file = "pygame-2.6.0-pp39-pypy39_pp73-macosx_10_9_x86_64.whl", hash = "sha256:4fad1ab33443ecd4f958dbbb67fc09fcdc7a37e26c34054e3296fb7e26ad641e"}, - {file = "pygame-2.6.0-pp39-pypy39_pp73-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:e909186d4d512add39b662904f0f79b73028fbfc4fbfdaf6f9412aed4e500e9c"}, - {file = "pygame-2.6.0-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:79abcbf6d12fce51a955a0652ccd50b6d0a355baa27799535eaf21efb43433dd"}, - {file = "pygame-2.6.0.tar.gz", hash = "sha256:722d33ae676aa8533c1f955eded966411298831346b8d51a77dad22e46ba3e35"}, +packaging = [ + {file = "packaging-24.2-py3-none-any.whl", hash = "sha256:09abb1bccd265c01f4a3aa3f7a7db064b36514d2cba19a2f694fe6150451a759"}, + {file = "packaging-24.2.tar.gz", hash = "sha256:c228a6dc5e932d346bc5739379109d49e8853dd8223571c7c5b55260edc0b97f"}, +] +pathspec = [ + {file = "pathspec-0.12.1-py3-none-any.whl", hash = "sha256:a0d503e138a4c123b27490a4f7beda6a01c6f288df0e4a8b79c7eb0dc7b4cc08"}, + {file = "pathspec-0.12.1.tar.gz", hash = "sha256:a482d51503a1ab33b1c67a6c3813a26953dbdc71c31dacaef9a838c4e29f5712"}, +] +platformdirs = [ + {file = "platformdirs-4.3.6-py3-none-any.whl", hash = "sha256:73e575e1408ab8103900836b97580d5307456908a03e92031bab39e4554cc3fb"}, + {file = "platformdirs-4.3.6.tar.gz", hash = "sha256:357fb2acbc885b0419afd3ce3ed34564c13c9b95c89360cd9563f73aa5e2b907"}, +] +pylint = [ + {file = "pylint-3.3.1-py3-none-any.whl", hash = "sha256:2f846a466dd023513240bc140ad2dd73bfc080a5d85a710afdb728c420a5a2b9"}, + {file = "pylint-3.3.1.tar.gz", hash = "sha256:9f3dcc87b1203e612b78d91a896407787e708b3f189b5fa0b307712d49ff0c6e"}, ] pyserial = [ {file = "pyserial-3.5-py2.py3-none-any.whl", hash = "sha256:c4451db6ba391ca6ca299fb3ec7bae67a5c55dde170964c7a14ceefec02f2cf0"}, {file = "pyserial-3.5.tar.gz", hash = "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb"}, ] PySide6 = [ - {file = "PySide6-6.7.2-cp39-abi3-macosx_11_0_universal2.whl", hash = "sha256:602debef9ec159b0db48f83b38a0e43e2dad3961f7d99f708d98620f04e9112b"}, - {file = "PySide6-6.7.2-cp39-abi3-manylinux_2_28_x86_64.whl", hash = "sha256:15e7696a09072ee977f6e6179ab1e48184953df8417bcaa83cfadf0b79747242"}, - {file = "PySide6-6.7.2-cp39-abi3-manylinux_2_31_aarch64.whl", hash = "sha256:6e0acb471535de303f56e3077aa86f53496b4de659b99ecce80520bcee508a63"}, - {file = "PySide6-6.7.2-cp39-abi3-win_amd64.whl", hash = "sha256:f73ae0de77d67f51ca3ce8207b12d3a5fa0107d3d5b6e4aeb3b53ee842b0927a"}, + {file = "PySide6-6.8.0.2-cp39-abi3-macosx_12_0_universal2.whl", hash = "sha256:cecc6ce1da6cb04542ff5a0887734f63e6ecf54258d1786285b9c7904abd9b01"}, + {file = "PySide6-6.8.0.2-cp39-abi3-manylinux_2_28_x86_64.whl", hash = "sha256:3258f3c63dc5053b8d5b8d2588caca8bb3a36e2f74413511e4676df0e73b6f1e"}, + {file = "PySide6-6.8.0.2-cp39-abi3-manylinux_2_31_aarch64.whl", hash = "sha256:6a25cf784f978fa2a23b4d089970b27ebe14d26adcaf38b2819cb04483de4ce9"}, + {file = "PySide6-6.8.0.2-cp39-abi3-win_amd64.whl", hash = "sha256:3e8fffca9a934e30c07c3f34bb572f84bfcf02385acbc715e65fbdd9746ecc2b"}, ] PySide6-Addons = [ - {file = "PySide6_Addons-6.7.2-cp39-abi3-macosx_11_0_universal2.whl", hash = "sha256:90b995efce61058d995c603ea480a9a3054fe8206739dcbc273fc3b53d40650f"}, - {file = "PySide6_Addons-6.7.2-cp39-abi3-manylinux_2_28_x86_64.whl", hash = "sha256:94b9bf6a2a4a7ac671e1776633e50d51326c86f4184f1c6e556f4dd5498fd52a"}, - {file = "PySide6_Addons-6.7.2-cp39-abi3-manylinux_2_31_aarch64.whl", hash = "sha256:22979b1aa09d9cf1d7a86c8a9aa0cb4791d6bd1cc94f96c5b6780c5ef8a9e34e"}, - {file = "PySide6_Addons-6.7.2-cp39-abi3-win_amd64.whl", hash = "sha256:ebf549eb25998665d8e4ec24014fbbd37bebc5ecdcb050b34db1e1c03e1bf81d"}, + {file = "PySide6_Addons-6.8.0.2-cp39-abi3-macosx_12_0_universal2.whl", hash = "sha256:30c9ca570dd18ffbfd34ee95e0a319c34313a80425c4011d6ccc9f4cca0dc4c8"}, + {file = "PySide6_Addons-6.8.0.2-cp39-abi3-manylinux_2_28_x86_64.whl", hash = "sha256:754a9822ab2dc313f9998edef69d8a12bc9fd61727543f8d30806ed272ae1e52"}, + {file = "PySide6_Addons-6.8.0.2-cp39-abi3-manylinux_2_31_aarch64.whl", hash = "sha256:553f3fa412f423929b5cd8b7d43fd5f02161851f10a438174a198b0f1a044df7"}, + {file = "PySide6_Addons-6.8.0.2-cp39-abi3-win_amd64.whl", hash = "sha256:ae4377a3e10fe720a9119677b31d8de13e2a5221c06b332df045af002f5f4c3d"}, ] PySide6-Essentials = [ - {file = "PySide6_Essentials-6.7.2-cp39-abi3-macosx_11_0_universal2.whl", hash = "sha256:4d13666e796ec140ecfb432c4f3d7baef6dfafc11929985a83b22c0025532fb7"}, - {file = "PySide6_Essentials-6.7.2-cp39-abi3-manylinux_2_28_x86_64.whl", hash = "sha256:a1a4c09f1e916b9cfe53151fe4a503a6acb1f6621ba28204d1bfe636f80d6780"}, - {file = "PySide6_Essentials-6.7.2-cp39-abi3-manylinux_2_31_aarch64.whl", hash = "sha256:9135513e1c4c6e2fbb1e4f9afcb3d42e54708b0d9ed870cb3213ea4874cafa1e"}, - {file = "PySide6_Essentials-6.7.2-cp39-abi3-win_amd64.whl", hash = "sha256:0111d5fa8cf826de3ca9d82fed54726cce116d57f454f88a6467578652032d69"}, + {file = "PySide6_Essentials-6.8.0.2-cp39-abi3-macosx_12_0_universal2.whl", hash = "sha256:3df4ed75bbb74d74ac338b330819b1a272e7f5cec206765c7176a197c8bc9c79"}, + {file = "PySide6_Essentials-6.8.0.2-cp39-abi3-manylinux_2_28_x86_64.whl", hash = "sha256:7df6d6c1da4858dbdea77c74d7270d9c68e8d1bbe3362892abd1a5ade3815a50"}, + {file = "PySide6_Essentials-6.8.0.2-cp39-abi3-manylinux_2_31_aarch64.whl", hash = "sha256:cf490145d18812a6cff48b0b0afb0bfaf7066744bfbd09eb071c3323f1d6d00d"}, + {file = "PySide6_Essentials-6.8.0.2-cp39-abi3-win_amd64.whl", hash = "sha256:d2f029b8c9f0106f57b26aa8c435435d7f509c80525075343e07177b283f862e"}, ] shiboken6 = [ - {file = "shiboken6-6.7.2-cp39-abi3-macosx_11_0_universal2.whl", hash = "sha256:50c33ac6317b673a1eb97a9abaafccb162c4ba0c9ca658a8e449c49a8aadc379"}, - {file = "shiboken6-6.7.2-cp39-abi3-manylinux_2_28_x86_64.whl", hash = "sha256:70e80737b27cd5d83504b373013b55e70462bd4a27217d919ff9a83958731990"}, - {file = "shiboken6-6.7.2-cp39-abi3-manylinux_2_31_aarch64.whl", hash = "sha256:98bedf9a15f1d8ba1af3e4d1e7527f7946ce36da541e08074fd9dc9ab5ff1adf"}, - {file = "shiboken6-6.7.2-cp39-abi3-win_amd64.whl", hash = "sha256:9024e6afb2af1568ebfc8a5d07e4ff6c8829f40923eeb28901f535463e2b6b65"}, + {file = "shiboken6-6.8.0.2-cp39-abi3-macosx_12_0_universal2.whl", hash = "sha256:9019e1fcfeed8bb350222e981748ef05a2fec11e31ddf616657be702f0b7a468"}, + {file = "shiboken6-6.8.0.2-cp39-abi3-manylinux_2_28_x86_64.whl", hash = "sha256:fa7d411c3c67b4296847b3f5f572268e219d947d029ff9d8bce72fe6982d92bc"}, + {file = "shiboken6-6.8.0.2-cp39-abi3-manylinux_2_31_aarch64.whl", hash = "sha256:1aaa8b7f9138818322ef029b2c487d1c6e00dc3f53084e62e1d11bdea47e47c2"}, + {file = "shiboken6-6.8.0.2-cp39-abi3-win_amd64.whl", hash = "sha256:b11e750e696bb565d897e0f5836710edfb86bd355f87b09988bd31b2aad404d3"}, +] +tomli = [ + {file = "tomli-2.1.0-py3-none-any.whl", hash = "sha256:a5c57c3d1c56f5ccdf89f6523458f60ef716e210fc47c4cfb188c5ba473e0391"}, + {file = "tomli-2.1.0.tar.gz", hash = "sha256:3f646cae2aec94e17d04973e4249548320197cfabdf130015d023de4b74d8ab8"}, +] +tomlkit = [ + {file = "tomlkit-0.13.2-py3-none-any.whl", hash = "sha256:7a974427f6e119197f670fbbbeae7bef749a6c14e793db934baefc1b5f03efde"}, + {file = "tomlkit-0.13.2.tar.gz", hash = "sha256:fff5fe59a87295b278abd31bec92c15d9bc4a06885ab12bcea52c71119392e79"}, +] +typing-extensions = [ + {file = "typing_extensions-4.12.2-py3-none-any.whl", hash = "sha256:04e5ca0351e0f3f85c6853954072df659d0d13fac324d0072316b67d7794700d"}, + {file = "typing_extensions-4.12.2.tar.gz", hash = "sha256:1a7ead55c7e559dd4dee8856e3a88b41225abfe1ce8df57b7c13915fe121ffb8"}, ] diff --git a/pyproject.toml b/pyproject.toml index 58c38d9..d5c0491 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -9,10 +9,16 @@ readme = "README.md" python = ">=3.10,<3.13" numpy = "^2.0.0" pyserial = "^3.5" -pygame = "^2.6.0" -PySide6 = "^6.7.2" +[tool.poetry.group.lint.dependencies] +black = "^24.10.0" +pylint = "^3.3.1" + + +[tool.poetry.group.ui.dependencies] +PySide6 = "^6.8.0.2" + [build-system] requires = ["poetry-core"] build-backend = "poetry.core.masonry.api" diff --git a/pyspj/spj.py b/pyspj/spj.py index 9e31015..c82bce6 100644 --- a/pyspj/spj.py +++ b/pyspj/spj.py @@ -1,228 +1,262 @@ -import numpy as np -import serial +""" +This module is used to control the spherical parallel joint device +from Skyentific +""" + +from threading import Thread, Lock, Event, main_thread from dataclasses import dataclass import time -from threading import Thread, Lock, Event, main_thread -from itertools import combinations_with_replacement -import json -from pathlib import Path +import numpy as np +import serial + class SpjException(Exception): """Generic class exception""" - pass + class NotConnected(SpjException): - """Serial device not connected - """ - pass + """Serial device not connected""" + @dataclass class WorldPosition: - """World position Rz, Ry, Rx - """ - - rz : float = 0.0 - ry : float = 0.0 - rx : float = 0.0 + """World position Rz, Ry, Rx""" + + rz: float = 0.0 + ry: float = 0.0 + rx: float = 0.0 + _ser: serial.Serial = None + class WorkerThread: - + """Worker thread to continuously acquire device position""" + def __init__(self): - + """Constructor""" + self._stop_event = Event() self._thread: Thread = None self._lock: Lock = Lock() self._current_pos: list[float] = [0, 0, 0] - + def start(self) -> None: - + """Start thread""" + self._stop_event.clear() self._thread = Thread(target=self.__run) self._thread.start() - + def stop(self) -> None: - + """Stop thread and join""" + self._stop_event.set() self._thread.join() @property def running(self) -> bool: + """Thread running + + Return + ------ + True if thread is running + """ + return self._thread.is_alive() - + @property def current_pos(self) -> list[float]: - + """Get current device position + + Return + List with current motor steps [Rz, Ry, Rx] + """ + if self._lock.acquire(blocking=False): steps = self._current_pos self._lock.release() else: steps = [0.0, 0.0, 0.0] - + return steps - + def __run(self): - + """Thread function""" + global _ser - + while not self._stop_event.is_set() and main_thread().is_alive(): - + new_pos = _ser.read_until() - + if len(new_pos) > 0: steps = new_pos.decode("utf-8").strip().split(",") steps = [float(x) for x in steps] if self._lock.acquire(blocking=False): self._current_pos = steps self._lock.release() - - - time.sleep(0.001) + + time.sleep(0.001) + _worker_thread: WorkerThread = None - + + class SphericalParallelJoint: - - + """Class to control the SPJ device""" + @staticmethod def home_pos() -> WorldPosition: + """Get the home position""" + return WorldPosition(0, 0, 0) - + @staticmethod def store_pos() -> WorldPosition: + """Get the store position""" + return WorldPosition(-60, 0, 0) def __init__(self) -> None: """Constructor - + Example ------- .. code-block:: python - + robot = SphericalParallelJoint() robot.connect() - + print("Move sync") if not robot.move_sync(robot.HOME_POS): print("Move failed") - + print("Move async") robot.move_async(robot.STORE_POS) time.sleep(4) - + print(f"Current position: {robot.get_current_position()}") - + robot.close() """ - - self._rot_axis = np.array([ - [ -1.0 / np.sqrt(3.0), -1.0 / np.sqrt(3.0), - 1.0 / np.sqrt(3.0) ], - [ -1.0 / np.sqrt(2.0), 1.0 / np.sqrt(2.0), 0.0 ], - [ 1.0 / np.sqrt(6.0), 1.0 / np.sqrt(6.0), - 2.0 / np.sqrt(6.0) ] #ZYX version. Values calculated with cross product of ZxY - #[ -1.0 / np.sqrt(3.0), -1.0 / np.sqrt(3.0), - 1.0 / np.sqrt(3.0) ] #ZYZ' version. Z' = Z - ]) - - + + self._rot_axis = np.array( + [ + [-1.0 / np.sqrt(3.0), -1.0 / np.sqrt(3.0), -1.0 / np.sqrt(3.0)], + [-1.0 / np.sqrt(2.0), 1.0 / np.sqrt(2.0), 0.0], + [ + 1.0 / np.sqrt(6.0), + 1.0 / np.sqrt(6.0), + -2.0 / np.sqrt(6.0), + ], # ZYX version. Values calculated with cross product of ZxY + # [ -1.0 / np.sqrt(3.0), -1.0 / np.sqrt(3.0), - 1.0 / np.sqrt(3.0) ] #ZYZ' version. Z' = Z + ] + ) + self._current_pos: list[float] = self.store_pos() self._connected = False - - def connect(self, port : str = "/dev/ttyACM0", baud : int = 115200): + + def connect(self, port: str = "/dev/ttyACM0", baud: int = 115200): """Connect to the device - + Parameters ---------- port: str COM port for serial communication. For linux /dev/tty, for windows COM baud: int, optional Serial baudrate - + Raise ----- Runtime error """ - + global _worker_thread global _ser - + _ser = serial.Serial(port, baudrate=baud, timeout=1) - + if not self.check_online(): raise RuntimeError("Failed to communicate with device") - + self._connected = True - + # Empty the read buffer _ser.read_all() - + # Start the thread - _worker_thread= WorkerThread() + _worker_thread = WorkerThread() _worker_thread.start() time.sleep(0.01) self._current_pos = self.get_current_position() - + def __check_connected(self): """Check if the connect method was called - + Raise ----- NotConnected """ - + if not self._connected: - raise NotConnected("Use the connect method to establish a connection to the device") - - def __get_rotational_matrix(self, index : int, angle : float) -> np.ndarray: + raise NotConnected( + "Use the connect method to establish a connection to the device" + ) + + def __get_rotational_matrix(self, index: int, angle: float) -> np.ndarray: """Compute the rotational matrix for one axis - + Parameters ---------- index: int Axis index (Rz = 0, Ry = 1, Rx = 2) angle: float Axis angle in degree - + Return ------ Rotational matrix for that axis """ - + angle = np.deg2rad(angle) ux, uy, uz = self._rot_axis[index] - mat = np.array([ - [ - np.cos(angle) + ux ** 2 * (1.0 - np.cos(angle)), - ux * uy * (1.0 - np.cos(angle)) - uz * np.sin(angle), - ux * uz * (1.0 - np.cos(angle)) + uy * np.sin(angle) - ], - [ - uy * uz * (1.0 - np.cos(angle)) + uz * np.sin(angle), - np.cos(angle) + uy ** 2 * (1.0 - np.cos(angle)), - uy * ux * (1.0 - np.cos(angle)) + ux * np.sin(angle) - ], + mat = np.array( [ - uz * ux * (1.0 - np.cos(angle)) - uy * np.sin(angle), - uz * uy * (1.0 - np.cos(angle)) + ux * np.sin(angle), - np.cos(angle) + uz ** 2 * (1.0 - np.cos(angle)) + [ + np.cos(angle) + ux**2 * (1.0 - np.cos(angle)), + ux * uy * (1.0 - np.cos(angle)) - uz * np.sin(angle), + ux * uz * (1.0 - np.cos(angle)) + uy * np.sin(angle), + ], + [ + uy * uz * (1.0 - np.cos(angle)) + uz * np.sin(angle), + np.cos(angle) + uy**2 * (1.0 - np.cos(angle)), + uy * ux * (1.0 - np.cos(angle)) + ux * np.sin(angle), + ], + [ + uz * ux * (1.0 - np.cos(angle)) - uy * np.sin(angle), + uz * uy * (1.0 - np.cos(angle)) + ux * np.sin(angle), + np.cos(angle) + uz**2 * (1.0 - np.cos(angle)), + ], ] - ]) + ) return mat - - def compute_steps(self, new_pos : WorldPosition) -> list[float]: + + def compute_steps(self, new_pos: WorldPosition) -> list[float]: """Compute the steps for each motor based on Rz, Ry and Rx coordinates - + Parameters ---------- new_pos: WorldPosition Position to compute the steps for - + Return ------ Steps for each motor """ - + first_rot_mat = self.__get_rotational_matrix(0, new_pos.rz) second_rot_mat = self.__get_rotational_matrix(1, new_pos.ry) third_rot_mat = self.__get_rotational_matrix(2, new_pos.rx) @@ -238,98 +272,98 @@ class SphericalParallelJoint: step2 = (108.0 / 20.0) * (theta2 - np.pi / 4.0) * (200.0 / (2 * np.pi)) step3 = (108.0 / 20.0) * (theta3 - np.pi / 4.0) * (200.0 / (2 * np.pi)) - return[step1, step2, step3] + return [step1, step2, step3] - def __move(self, pos : WorldPosition) -> list[float]: + def __move(self, pos: WorldPosition) -> list[float]: """Do move the device - + Parameters ---------- pos: WorldPosition Position to move to - + Return ------ End position in steps """ - + global _ser - + self.__check_connected() - + steps = self.compute_steps(pos) - _bytes =_ser.write(f"{steps[0]},{steps[1]},{steps[2]}\n".encode("utf-8")) + _bytes = _ser.write(f"{steps[0]},{steps[1]},{steps[2]}\n".encode("utf-8")) if _bytes < 1: print("Failed to write") steps = self._current_pos - - return [float(steps[0]),float(steps[1]),float(steps[2])] + + return [float(steps[0]), float(steps[1]), float(steps[2])] def move_sync(self, pos: WorldPosition) -> bool: """Move the device synchronously. This call will return when the movement is finished - + Parameters ---------- pos: WorldPosition Position to move to - + Return ------ True if move was successful, False otherwise """ - + max_errors = 100 end_pos = self.__move(pos) - + try: end_pos = list(map(int, end_pos)) - except ValueError as ex: + except ValueError: print(f"Failed to convert end position to int: {end_pos}") return False - + while True: - + if max_errors < 1: break - + try: current_pos = list(map(int, self.get_current_position())) except ValueError: max_errors -= 1 continue - + if current_pos == end_pos: break - + if max_errors < 1: return False - + return True - + def move_async(self, pos: WorldPosition) -> list[float]: """Move the robot asynchronously. Return before movement ends. - + Parameters ---------- pos: WorldPosition Position to move to - + Return ------ End position in steps - """ - + """ + return self.__move(pos) - + def check_online(self) -> bool: """Check if the device is online and working - + Return ------ True if online - """ - - global _ser + """ + + global _ser msg = _ser.read_until().decode("utf-8").strip() @@ -341,110 +375,71 @@ class SphericalParallelJoint: if msg == "Ready": return True return False - + if len(msg) > 0: - return True + return True return False - + def get_current_position(self) -> list[float]: """Get the current position of the motors in steps - + Return ------ A list in the format (stepZ, stepY, stepX) """ - + global _worker_thread - + self.__check_connected() - + self._current_pos = _worker_thread._current_pos - + # Sleep to avoid overloading the lock time.sleep(0.001) - + return self._current_pos - + def close(self): - """End communication with the device - """ - + """End communication with the device""" + global _worker_thread global _ser - + if not self._connected: return - + if _worker_thread.running: _worker_thread.stop() - + _ser.close() self._connected = False def __del__(self): - """Destructor - """ + """Destructor""" if self._connected: self.close() - - def save_lookup_table(self): - - angles = [x for x in range(-180, 181)] - - # print(comb) - lut_steps: list[tuple] = [] - lut_angles: list[tuple] = list(combinations_with_replacement(angles, 3)) - - for c in lut_angles: - pos = WorldPosition(*c) - lut_steps.append(tuple(map(int, self.compute_steps(pos)))) - - with open("spj_lut.json", "w") as file: - data = { - "angles" : lut_angles, - "steps" : lut_steps - } - - file.write(json.dumps(data, indent=4)) - # index = lut_angles.index((0,0,0)) - # print(index) - # print(lut_steps[index]) - - def load_lookup_table(self, filepath: Path) -> tuple[list[tuple],list[tuple]]: - - with open(filepath, "r") as file: - data = json.loads(file.read()) - - return (data["angles"], data["steps"]) - + + if "__main__" == __name__: import os - + os.chdir(os.path.dirname(__file__)) robot = SphericalParallelJoint() - angles, steps = robot.load_lookup_table("spj_lut.json") - - print("Searching angle") - # print(angles) - index = angles.index([0,0,0]) - print(index) - - print(steps[index]) - # robot.connect() - - # print("Move sync") - # if not robot.move_sync(robot.HOME_POS): - # print("Move failed") - - # print("Move async") - # robot.move_async(robot.STORE_POS) - # time.sleep(4) - - # print(f"Current position: {robot.get_current_position()}") - - # robot.close() - \ No newline at end of file + + robot.connect() + + print("Move sync") + if not robot.move_sync(SphericalParallelJoint.home_pos()): + print("Move failed") + + print("Move async") + robot.move_async(SphericalParallelJoint.store_pos()) + time.sleep(4) + + print(f"Current position: {robot.get_current_position()}") + + robot.close() -- GitLab From 5b52022f4a18a4b05b0e6598d17a3828628024d0 Mon Sep 17 00:00:00 2001 From: "mattia.gallacchi" Date: Thu, 14 Nov 2024 08:45:40 +0100 Subject: [PATCH 4/5] Update README Signed-off-by: mattia.gallacchi --- README.md | 4 ++++ dist/pyspj-0.1.0-py3-none-any.whl | Bin 2136 -> 0 bytes dist/pyspj-0.1.0.tar.gz | Bin 2142 -> 0 bytes 3 files changed, 4 insertions(+) delete mode 100644 dist/pyspj-0.1.0-py3-none-any.whl delete mode 100644 dist/pyspj-0.1.0.tar.gz diff --git a/README.md b/README.md index 10c53cc..4bad813 100644 --- a/README.md +++ b/README.md @@ -27,5 +27,9 @@ poetry add pyspj --source igib ## GUI application +The GUI application can be run using the following command +```bash +poetry run python frontend/main.py +``` diff --git a/dist/pyspj-0.1.0-py3-none-any.whl b/dist/pyspj-0.1.0-py3-none-any.whl deleted file mode 100644 index e7c60d407d4f6994ed0720dc87deea73bd9f4637..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2136 zcmZ`)2{hDeAO4R)LPM6KL6I0?Oc_f=jf}Aj#uyS~gt5<9VlaFn+uT7VTZpb)N|uze zTq0}ARxo2l{Go0S}}X+q15)v_+#_&Tn19xxg^P<`=-c{Mz0&ND)3qUb9Ol z>k^wy*K%R#Qi*Cm{bx_YOrK|!B!{I7Ak-jUkQZ-CT+u3-CWY{>!wQmOvrBvY$dYy` zE36y&X?$HwC23_lwCdn{7vudxy@C;u!I7&L0o^}tS>r!!HkN05;>#~XVlnwI4=mLw z9vJ@xmg2|`!bc=ArZm5HUJxN@woZtnb?_+--3WZ#;>>e#}ZOFXX0}2I#-4ZYyb54eg17B{TFcUxXlTLmbUE5olvA14R za46b*+D__QON!{_VsGe4aJBh-P7nn9csqWK z6&DDU!Pap?>5PfYG{MMafoUYhu@JittzHn1L|bE*Z+Xg{GclOpV~-4ghm<7S6`3oK z4mmzJ<3z&Fti8;@#rzaLxkjw7Np04;#%izFXhJ4wmz{_*m3aO@@{{Qx0~o@5&8PH# z>UMP(8UG9tVjW#YPu=SU*XNXvx%aE;im2~{p@iZs?X&Ljko|;Ru`$zmy9Hu4TW?=y z+8^j?$@6q2X?f`);9VsG%8(O=k%W#$A%#_w~XDA(|xEuPqZ{W_^KCvMgz@^gmua#z195WmRe+eB-H4$I0r$*ns&r+ zTKAm3YRLf6RzK2&n|ksqUy1#<^{txqzX*H zV#4n4(%o{W^1pPHQVW+x3ZgDIjF2}z9ekHSARoUoTK38R+tUS%Uq~f#aYcGiWPKt0 ze&ST1!eozfVkvF?VaA&eH6?QJaSG8UW9Dv-lkR6%L||PaqNPeq1fw7Xid#}{eWW85 zuQwU0kkCeYaTn>jb`QheFgw+$8fq7DGYZ(8epyW_{9&&pD;(4NRKHbKFVhv6C!E)9 z#st_PWe)0(_Jt7S3|8Pvb4J>S4JFr)&xWzX{4Qd<*Sd)3J7wM8f64J?*gemAN}9AB zt)@k-^jjSbiD{{RJ!YlrqjS6DCT(jM?w=jR^oVY;KY`mh9Jqk~)j`w|@MCZUoa_@E zqVD7G8K8026phA8cA*4g4vF%;Y`n9=BaYVB5M5Fz9Nzb{7+J~0Iedfi{p|i6)?oQ3O<`jim(`CEWO;QXt%?MYgsg!hvElmR^!k6MmIdrx`lmmZMg` z_4ZLxZ0rfeCv?*%)S@==|%W&Nd19mlK{#fFxP zcOSM{ovAJ$Dnrj%;d_qwek4-XVe@Vgv*N$@`a?;N@tEjhjpaT67WS>v;gn9Upu${X zWp)%6k3M6KH>UM8gvKHO;N^IL8A6_6%7EC2J@8cl-LjHHcC{s`u=h@n`0c_l8c5X3 z{)r-2YMKcN3;kb=S;?C+;x-fsJsI>9WfwEp>WQ{nr3SBC)tqft8yCz^!({iYMe|&X zlbtCU+ia;f$Yao-eVd~D*Q&NI7ANfvZCFIskCWd>ITF3XtNWCT40g|5IBI+x+_dFJ z{FuR~=J5jQJNJR1lZY#Dsoi3iS7FjX@Jr%Pm|c?9n*z#oRQ z0P#owJC91P3V*DkOTQog9Gu%M|EEO$v;%-5kP&zE{$ct53uZgf_VoUn=*5miehe?Kw^AI+@c4MMncmkc67IfM6^+lmFfYKuIJe*-n#q&S|h7kI2R1!(y=) zkc53~A3r(Ai=PRe5O(lQ$WgTK*Kz#L=34Mw$8)^{v^aPG$}^4`41e%_l6Qh4P9t*a z_Pp*%&vgUG_By>@;Cse_rhh!8%Pjq~?bxpE9H+~aC4Z7Jw|NqUUpM}PPG@iY_X798 z?F2!u>w8X5#J|_|y9db8@&C_4j(}Zxg93C1mjtDwM4e1t*L*oNs8BJorl0E{g z3CYHcro3Rkozo1Z_zKTRhTt;K$OQ2Ojgy$OBt(2p(3^Bl7#-sfonwZ>kc8-835_|r zNG=l&m^kP2gk^v?XhfKEitQQ5;qiD*(c?Pj$(*z?8{6agSp#%tP`Z@X7cZVZfAzwS zCIES8l&5A-NJ`=fiN};=&?jBOfKvbe`nD^k73Wd9lnl>yQu1xz_LcgVISnW6Y?*Nq$!WgN zDI>~AYOMA+VMLffiC*II6$~iXPw67s?FeT{3^zEX09;#tTi1VG|5x?D>jl9{$9~jx zJU=+m_5Zu8|8RW_|8~0k+VOu&{qMP+FYABciOa62|E{k84?{Fc6ULFMemFyMdT10p zRs9Z)DN7JjtloHuQxI0%A9)kEUUQ zIn*pDx&?Q+lY8UV*(>n%_F}cWkeiG3C=)V8qY;fMAC1h6gi{MiZTl$WOccvwDAvcE zFP}bpra)IP40CcS zE-Y!ABF85^aAZ2+udD|1VnC47VzRO|!d)W(eAr8(FL2^yd_#4|b;0@-`U zP2YA})*dF>=Axl>2cReW?&879+}-`N_1WMJTdQ>yM5icDxU$T6PR3WGBo1jz%vQgu z0>d;T=pv8BI=o;kVP@kcrr~O{8bx^wF2o7CrhE=Q^MQ^@qg4ed`_qhYu@qP=5)MbO zM`d(TxEF<8-$J>Ng>XCz$;x%5Bf-?-iCnmTc`iy&uIhx$JccKxWR;Z27Z%Ew@P8wn zuIvs2Iq0wq7|BrC?pVS!L|0m-0X$adA27mm77Nc;yiLSHh^ILxBY2uhvm#DoaxD@s1ZO4Z${-DC z#v!Kouo9zF$}$=EBNbdK2KOWbXFY<=ZYp^k9*!sXMR7|g%MuEoQ+96M4>)QQ!- zEACshj^2)rAn24kI^w?G(cjS#1if-cPuy3O6}}~OHC^XK7BSBe)eG!cOhbBgN};7w zG{ELUN}8UEnUt1&T>yIf0lmKk=T$-(zh%OGC=dhSk{7ukhi547^8UBCdH?Hmypy2&$nHFHyC=SW{rkPW|6PAR`v&oU+xK6m6NvY}UGTra z4z!3^sV_SN`^MkN9`}u2ZZ3uGiJ^|3LBIK58R) zVxaM?kK{9h;HZy2(jnCOg!MNe;21`V$LhexDSBrNn$n4y!!>^-zW>O!kIiNu4Vp*I z;U~kOG1xi`N2QYtqPP6^bapC_U09EO@zbi1}q$!TGfC7brmSPrXS_EJE{fb{Y*dp4#{9wxpI85T%sD^%5-hCH~zZHTy zA0ztJ&%#tCMkK;C+$Fa;GqdUek}-#WB5}pXt?iZ#Ub#JITdb8s0DpB^ZILdqX_dEy zMBOc#;=@UmwZf98cFKZH;RUXv+`uxx$&@FUM@MCkXz2=ekNkiB@1NSgyngxH1LuFI z*9+>;e?iyR`Trs472#OKV*5WTEB29RyT)tCn-HtQH;i?IbQ0rD9z~cf`{>K_qw!QG ztv;$JtD>PTv(I+(CYKo(MJi~Y%hb?ErF>vK6Q={{loA-?oFy}cqX^0z`W45s9MndA z;H!gxaOHolJdCp$|2fTgzc@x=R*PAyE>)Z&G^@33hgfm`(A<=|mhpU3N7~Q8)FkHZ zx62fki<1REhPAnxB-gQF+*M&9RqCv$Ns}f`nlx$Bq)C$|O`0@m(xgd~CQX_&Y0{)g UlO|1?H2ov!FInQ=mHk -- GitLab From 579097d7ba93526935f3b285cb2f61d805c7cbc9 Mon Sep 17 00:00:00 2001 From: "mattia.gallacchi" Date: Thu, 14 Nov 2024 08:48:56 +0100 Subject: [PATCH 5/5] Update CI Signed-off-by: mattia.gallacchi --- .gitignore | 3 ++- .gitlab-ci.yml | 4 ++-- pyproject.toml | 3 +++ 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index 5a91b9b..bb619c6 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,5 @@ build/ # python stuff __pycache__/ -*.exe \ No newline at end of file +*.exe +dist/ \ No newline at end of file diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index b579085..e8d63df 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -18,7 +18,7 @@ lint-black: allow_failure: false script: - poetry install --only lint - - poetry run black --check spj/ + - poetry run black --check pyspj/ lint-pylint: stage: lint @@ -30,7 +30,7 @@ lint-pylint: allow_failure: false script: - poetry install --only lint - - poetry run pylint --fail-under=8 spj/ + - poetry run pylint --fail-under=8 pyspj/ deploy-job: # This job runs only when the merge is accepted diff --git a/pyproject.toml b/pyproject.toml index d5c0491..f439576 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,6 +4,9 @@ version = "0.1.1" description = "This pakages is used to control the Spherical Parallel Joint Robot" authors = ["mattia.gallacchi "] readme = "README.md" +packages = [ + {include = "frontend"} +] [tool.poetry.dependencies] python = ">=3.10,<3.13" -- GitLab