diff --git a/.github/workflows/publish_pypi.yml b/.github/workflows/publish_pypi.yml new file mode 100644 index 0000000..0b7e1f1 --- /dev/null +++ b/.github/workflows/publish_pypi.yml @@ -0,0 +1,33 @@ +name: Publish to PyPI + +on: + workflow_dispatch: + release: + types: [published] + +jobs: + publish_package1: + runs-on: ubuntu-latest + steps: + - name: Checkout code + uses: actions/checkout@v3 + with: + token: ${{ secrets.GITHUB_TOKEN }} + + - name: Set up Python + uses: actions/setup-python@v3 + with: + python-version: '3.10' # Choose your preferred Python version + + - name: Build and Publish Package 1 + env: + TWINE_USERNAME: __token__ + TWINE_PASSWORD: ${{ secrets.PYPI_TOKEN }} + run: | + python3 -m pip install --upgrade pip + python3 -m pip install --upgrade build + python3 -m pip install --upgrade twine + python3 -m build + echo $TWINE_USERNAME + echo $TWINE_PASSWORD + python3 -m twine upload dist/* \ No newline at end of file diff --git a/setup.cfg b/setup.cfg index 024cf94..945170c 100644 --- a/setup.cfg +++ b/setup.cfg @@ -1,6 +1,6 @@ [metadata] name = sparke_kinematics -version = 0.0.2a +version = 0.1.0b0 author = Colin Fuelberth author_email = colin.fuelberth@icloud.com url = https://github.com/Infinite-Echo/sparkeKinematics @@ -14,6 +14,17 @@ classifiers = Programming Language :: Python :: 3 [options] +package_dir = + = src +packages = find: +python_requires = >=3.6 install_requires = numpy >= 1.24.0 - matplotlib >= 3.1.2 \ No newline at end of file + matplotlib >= 3.1.2 + PyQt5 + +[options.packages.find] +where = src + +[options.package_data] +sparke_kinematics_tools=*.ui \ No newline at end of file diff --git a/src/sparke_kinematics/test_plotter.py b/src/sparke_kinematics/test_plotter.py index b26f62e..ae07ef2 100644 --- a/src/sparke_kinematics/test_plotter.py +++ b/src/sparke_kinematics/test_plotter.py @@ -4,6 +4,7 @@ from sparke_leg_IK import SparkeLeg as spLegIK import base_transformations as basetf import leg_transformations as legtf +import csv def plot_points(arr1, arr2, arr3, arr4, arr5, arr6, arr7, arr8): # Create figure and 3D axis @@ -30,8 +31,15 @@ def plot_points(arr1, arr2, arr3, arr4, arr5, arr6, arr7, arr8): # Show the plot plt.show() +def append_to_csv(filepath, row): + with open(filepath, 'a', newline='') as csvfile: + writer = csv.writer(csvfile) + writer.writerow(row) + def main(): - Tm = basetf.create_base_transformation(0, 0, -0.05, 0, -np.pi/24, 0) + filepath = '/home/echo/workspaces/sparkeKinematics/temp2.csv' + + Tm = basetf.create_base_transformation(0, 0, 0, 0, 0, 0) tb0 = legtf.create_Tb0(Tm, 1, 1) t01 = legtf.create_T01(0) t12 = legtf.create_T12(np.pi/4) @@ -39,15 +47,17 @@ def main(): tb1 = np.matmul(tb0, t01) tb2 = np.matmul(tb1, t12) tb3 = np.matmul(tb2, t23) - x_ee = tb3[0,3] + 0.1 + x_ee = tb3[0,3] y_ee = tb3[1,3] z_ee = tb3[2,3] leg1 = spLegIK(1) leg1.solve_angles(Tm, x_ee, y_ee, z_ee) + print(f'xee: {x_ee}') + print(f'yee: {y_ee}') + print(f'zee: {z_ee}') print(f'theta1: {leg1.theta1}') print(f'theta2: {leg1.theta2}') print(f'theta3: {leg1.theta3}') - tb0 = legtf.create_Tb0(Tm, 1, -1) t01 = legtf.create_T01(0) @@ -230,6 +240,11 @@ def main(): points_arr8 = np.append(points_arr8, [[0,0,0], [i4_x0, i4_y0, i4_z0], [i4_x1, i4_y1, i4_z1]], axis=0) points_arr8 = np.append(points_arr8, [[i4_x2, i4_y2, i4_z2], [i4_x3, i4_y3, i4_z3]], axis=0) + append_to_csv(filepath, points_arr1) + append_to_csv(filepath, points_arr3) + append_to_csv(filepath, points_arr5) + append_to_csv(filepath, points_arr7) + plot_points(points_arr1, points_arr2, points_arr3, points_arr4, points_arr5, points_arr6, points_arr7, points_arr8) if __name__ == '__main__': diff --git a/src/sparke_kinematics_tools/__init__.py b/src/sparke_kinematics_tools/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/sparke_kinematics_tools/bezier_utils.py b/src/sparke_kinematics_tools/bezier_utils.py new file mode 100644 index 0000000..448edb7 --- /dev/null +++ b/src/sparke_kinematics_tools/bezier_utils.py @@ -0,0 +1,28 @@ +import numpy as np +from scipy.special import comb + +def bernstein_poly(i, n, t): + """ + The Bernstein polynomial of n, i as a function of t + """ + return comb(n, i) * ( t**(n-i) ) * (1 - t)**i + + +def bezier_curve(control_points, n_times=1000): + """ + Calculates the bezier curve formed by the given control points and returns n_times points evenly distrubeted across the curve + """ + n_points = len(control_points) + x_points = np.array([p[0] for p in control_points]) + y_points = np.array([p[1] for p in control_points]) + z_points = np.array([p[2] for p in control_points]) + + t = np.linspace(0.0, 1.0, n_times) + + polynomial_array = np.array([bernstein_poly(i, n_points-1, t) for i in range(n_points)]) + + x_vals = np.dot(x_points, polynomial_array) + y_vals = np.dot(y_points, polynomial_array) + z_vals = np.dot(z_points, polynomial_array) + + return np.array([x_vals, y_vals, z_vals]).T \ No newline at end of file diff --git a/src/sparke_kinematics_tools/forward_kinematics_utils.py b/src/sparke_kinematics_tools/forward_kinematics_utils.py new file mode 100644 index 0000000..cb392f8 --- /dev/null +++ b/src/sparke_kinematics_tools/forward_kinematics_utils.py @@ -0,0 +1,35 @@ +import numpy as np +from sparke_kinematics.sparke_leg_IK import SparkeLeg +import sparke_kinematics.leg_transformations as legtf + +def get_leg_positions(leg: SparkeLeg, Tm: np.matrix, angles): + leg.update_Tb0(Tm) + leg_positions = [] + leg_positions.append(get_postions_from_tf(leg.t_b0)) + leg_positions.append(get_postions_from_tf(get_fk_tb1(leg.t_b0, angles))) + leg_positions.append(get_postions_from_tf(get_fk_tb2(leg.t_b0, angles))) + leg_positions.append(get_postions_from_tf(get_fk_tb3(leg.t_b0, angles))) + return leg_positions + +def get_postions_from_tf(tf: np.matrix): + positions = [ + tf[0,3], + tf[1,3], + tf[2,3], + ] + return positions + +def get_fk_tb1(t_b0: np.matrix, angles): + t_01 = legtf.create_T01(angles[0]) + t_b1 = np.matmul(t_b0, t_01) + return t_b1 + +def get_fk_tb2(t_b0: np.matrix, angles): + t_12 = legtf.create_T12(angles[1]) + t_b2 = np.matmul(get_fk_tb1(t_b0, angles), t_12) + return t_b2 + +def get_fk_tb3(t_b0: np.matrix, angles): + t_23 = legtf.create_T23(angles[2]) + t_b3 = np.matmul(get_fk_tb2(t_b0, angles), t_23) + return t_b3 \ No newline at end of file diff --git a/src/sparke_kinematics_tools/kinematics_controller.py b/src/sparke_kinematics_tools/kinematics_controller.py new file mode 100644 index 0000000..ad79ede --- /dev/null +++ b/src/sparke_kinematics_tools/kinematics_controller.py @@ -0,0 +1,210 @@ +from sparke_kinematics import base_transformations as basetf +from sparke_kinematics import leg_transformations as legtf +# from sparke_kinematics.sparke_base_IK import SparkeBase +from sparke_kinematics_tools.matplotlib_widget import Matplotlib3DWidget +from sparke_kinematics.sparke_leg_IK import SparkeLeg +from PyQt5.QtWidgets import QTreeView, QApplication, QWidget, QVBoxLayout, QSizePolicy, QMainWindow +import numpy as np +from sparke_kinematics_tools.mainwindow import Ui_MainWindow +from sparke_kinematics_tools.kinematics_controller_utils import * +import sparke_kinematics_tools.forward_kinematics_utils as fk_utils +import time +import copy +class kinematicsController(): + HOME_POSITIONS = [ + [0.101, 0.055, 0.], #Base FL + [0.101, 0.11, 0.], #FL Shoulder + [0.01261165, 0.11, -0.08838835], #FL Leg + [0.10807107, 0.11, -0.18384776], #FL Wrist + [0.101, -0.055, 0.], #Base FR + [0.101, -0.11, 0.], #FR Shoulder + [0.01261165, -0.11, -0.08838835], #FR Leg + [0.10807107, -0.11, -0.18384776], #FR Wrist + [-0.101, 0.055, 0.], #Base BL + [-0.101, 0.11, 0.], #BL Shoulder + [-0.18938835, 0.11, -0.08838835], #BL Leg + [-0.09392893, 0.11, -0.18384776], #BL Wrist + [-0.101, -0.055, 0.], #Base BR + [-0.101, -0.11, 0.], #BR Shoulder + [-0.18938835, -0.11, -0.08838835], #BR Leg + [-0.09392893, -0.11, -0.18384776], #BR Wrist + ] + HOME_ANGLES = [ + [0, 0.7853981633974481, 1.5707963267948968], + [0, 0.7853981633974481, 1.5707963267948968], + [0, 0.7853981633974481, 1.5707963267948968], + [0, 0.7853981633974481, 1.5707963267948968], + ] + JOINT_DICT = { + 0: 'Shoulder', + 1: 'Leg', + 2: 'Wrist', + } + LEG_DICT = { + 0: 'Front Left', + 1: 'Front Right', + 2: 'Back Left', + 3: 'Back Right', + } + + def __init__(self, parent, model, plot_widget): + self.current_positions = copy.deepcopy(kinematicsController.HOME_POSITIONS) + self.current_angles = copy.deepcopy(kinematicsController.HOME_ANGLES) + self.parent = parent + self.model = model + self.plot_widget: Matplotlib3DWidget + self.plot_widget = plot_widget + self.sparke_legs = [] + for leg in range(1, 5): + self.sparke_legs.append(SparkeLeg(leg)) + self.home() + + def home(self): + self.model.blockSignals(True) + self.current_positions = copy.deepcopy(kinematicsController.HOME_POSITIONS) + self.current_angles = copy.deepcopy(kinematicsController.HOME_ANGLES) + self.Tm = basetf.create_base_transformation(0, 0, 0, 0, 0, 0) + set_positions(self.model, 'Base', [0.,0.,0.]) + set_base_rotations(self.model, [0.,0.,0.]) + for i in range(4): + self.sparke_legs[i].update_Tb0(self.Tm) + set_positions(self.model, kinematicsController.LEG_DICT[i], kinematicsController.HOME_POSITIONS[(4*i)+1], kinematicsController.JOINT_DICT[0]) + set_positions(self.model, kinematicsController.LEG_DICT[i], kinematicsController.HOME_POSITIONS[(4*i)+2], kinematicsController.JOINT_DICT[1]) + set_positions(self.model, kinematicsController.LEG_DICT[i], kinematicsController.HOME_POSITIONS[(4*i)+3], kinematicsController.JOINT_DICT[2]) + set_joint_angle(self.model, kinematicsController.LEG_DICT[i], kinematicsController.JOINT_DICT[0], kinematicsController.HOME_ANGLES[i][0]) + set_joint_angle(self.model, kinematicsController.LEG_DICT[i], kinematicsController.JOINT_DICT[1], kinematicsController.HOME_ANGLES[i][1]) + set_joint_angle(self.model, kinematicsController.LEG_DICT[i], kinematicsController.JOINT_DICT[2], kinematicsController.HOME_ANGLES[i][2]) + self.model.blockSignals(False) + self.update_plot() + + def solve_ik(self): + old_angles = copy.deepcopy(self.current_angles) + old_positions = copy.deepcopy(self.current_positions) + self.update_Tm() + try: + for i in range(4): + end_effector_positions = get_positions(self.model, kinematicsController.LEG_DICT[i], 'Wrist') + self.sparke_legs[i].solve_angles(self.Tm, end_effector_positions[0], end_effector_positions[1], end_effector_positions[2]) + self.current_angles[i][0] = self.sparke_legs[i].theta1 + self.current_angles[i][1] = self.sparke_legs[i].theta2 + self.current_angles[i][2] = self.sparke_legs[i].theta3 + for j in range(3): + set_joint_angle(self.model, kinematicsController.LEG_DICT[i], kinematicsController.JOINT_DICT[j], self.current_angles[i][j]) + self.solve_fk() + except: + print('No Possible Solution For Given End Effectors') + self.restore_old_angles(old_angles) + self.restore_old_positions(old_positions) + + def solve_fk(self): + old_positions = copy.deepcopy(self.current_positions) + old_angles = copy.deepcopy(self.current_angles) + try: + self.update_Tm() + self.current_angles = self.get_angles_from_tree() + for i in range(4): + positions = fk_utils.get_leg_positions(self.sparke_legs[i], self.Tm, self.current_angles[i]) + self.current_positions[(i*4)] = positions[0] + self.current_positions[(i*4)+1] = positions[1] + self.current_positions[(i*4)+2] = positions[2] + self.current_positions[(i*4)+3] = positions[3] + self.set_tree_to_current_vals() + except: + print('Invalid Inputs For FK, Unable to Solve') + self.restore_old_positions(old_positions) + self.restore_old_angles(old_angles) + + def data_changed(self): + self.model.blockSignals(True) + if(self.data_different()): + mode = self.parent.kinematics_tree_widget.mode + if(mode == 'fk'): + self.solve_fk() + if(mode == 'ik'): + self.solve_ik() + self.update_plot() + self.model.blockSignals(False) + + def data_different(self): + if(self.current_angles == self.get_angles_from_tree()): + if(self.current_positions == self.get_positions_from_tree()): + if(self.base_positions == get_positions(self.model, 'Base')): + if(self.base_rotations == get_base_rotations(self.model)): + return False + return True + + def update_plot(self): + # Retrieve the updated data from the model + # and perform calculations to get new x, y, and z values + self.plot_widget.clear_plot() + colors = { + 0: 'blue', + 1: 'red', + 2: 'green', + 3: 'purple', + } + base_array = [ + self.current_positions[0], + self.current_positions[4], + self.current_positions[12], + self.current_positions[8], + self.current_positions[0], + ] + self.plot_widget.update_plot(base_array, color='orange') + + for i in range(4): + array = [] + array.append(self.current_positions[(4*i)]) + array.append(self.current_positions[(4*i)+1]) + array.append(self.current_positions[(4*i)+2]) + array.append(self.current_positions[(4*i)+3]) + self.plot_widget.update_plot(array, colors[i]) + + def set_tree_to_current_vals(self): + for i in range(4): + set_positions(self.model, kinematicsController.LEG_DICT[i], self.current_positions[(4*i)+1], kinematicsController.JOINT_DICT[0]) + set_positions(self.model, kinematicsController.LEG_DICT[i], self.current_positions[(4*i)+2], kinematicsController.JOINT_DICT[1]) + set_positions(self.model, kinematicsController.LEG_DICT[i], self.current_positions[(4*i)+3], kinematicsController.JOINT_DICT[2]) + set_joint_angle(self.model, kinematicsController.LEG_DICT[i], kinematicsController.JOINT_DICT[0], self.current_angles[i][0]) + set_joint_angle(self.model, kinematicsController.LEG_DICT[i], kinematicsController.JOINT_DICT[1], self.current_angles[i][1]) + set_joint_angle(self.model, kinematicsController.LEG_DICT[i], kinematicsController.JOINT_DICT[2], self.current_angles[i][2]) + + def restore_old_positions(self, old_positions): + self.current_positions = copy.deepcopy(old_positions) + for i in range(4): + set_positions(self.model, kinematicsController.LEG_DICT[i], self.current_positions[(4*i)+1], \ + kinematicsController.JOINT_DICT[0]) + set_positions(self.model, kinematicsController.LEG_DICT[i], self.current_positions[(4*i)+2], \ + kinematicsController.JOINT_DICT[1]) + set_positions(self.model, kinematicsController.LEG_DICT[i], self.current_positions[(4*i)+3], \ + kinematicsController.JOINT_DICT[2]) + + def restore_old_angles(self, old_angles): + self.current_angles = copy.deepcopy(old_angles) + for i in range(4): + for j in range(3): + set_joint_angle(self.model, kinematicsController.LEG_DICT[i], kinematicsController.JOINT_DICT[j], self.current_angles[i][j]) + + def update_Tm(self): + self.base_positions = get_positions(self.model, 'Base') + self.base_rotations = get_base_rotations(self.model) + self.Tm = basetf.create_base_transformation(self.base_positions[0], self.base_positions[1], self.base_positions[2], \ + self.base_rotations[0], self.base_rotations[1], self.base_rotations[2]) + + def get_angles_from_tree(self): + angles = [] + for i in range(4): + joint_angles = [] + for j in range(3): + angle = get_joint_angle(self.model, kinematicsController.LEG_DICT[i], kinematicsController.JOINT_DICT[j]) + joint_angles.append(float(np.deg2rad(angle))) + angles.append(joint_angles) + return angles + + def get_positions_from_tree(self): + positions = [] + for i in range(4): + for j in range(3): + position = get_positions(self.model, kinematicsController.LEG_DICT[i], kinematicsController.JOINT_DICT[j]) + positions.append(position) + return positions \ No newline at end of file diff --git a/src/sparke_kinematics_tools/kinematics_controller_utils.py b/src/sparke_kinematics_tools/kinematics_controller_utils.py new file mode 100644 index 0000000..b809ada --- /dev/null +++ b/src/sparke_kinematics_tools/kinematics_controller_utils.py @@ -0,0 +1,89 @@ +from sparke_kinematics import base_transformations as basetf +from sparke_kinematics import leg_transformations as legtf +from sparke_kinematics.sparke_base_IK import SparkeBase +from sparke_kinematics.sparke_leg_IK import SparkeLeg +from PyQt5.QtWidgets import QTreeView, QApplication, QWidget, QVBoxLayout, QSizePolicy, QMainWindow +from PyQt5.QtCore import Qt, QModelIndex +import numpy as np +from sparke_kinematics_tools.mainwindow import Ui_MainWindow + +def get_link_index(model, link_name): + linkDict = { + 'Base': 0, + 'Front Left': 1, + 'Front Right': 2, + 'Back Left': 3, + 'Back Right': 4, + } + return model.index(linkDict[link_name], 0, QModelIndex()) + +def get_joint_index(model, link_index, joint_name): + jointDict = { + 'Shoulder': 0, + 'Leg': 1, + 'Wrist': 2, + } + return model.index(jointDict[joint_name], 0, link_index) + +def get_position_index(model, index): + position_index = model.index(0,0,index) + return position_index + +def get_angle_index(model, index): + angle_index = model.index(1,0,index) + return angle_index + +def get_positions(model, link_name, joint_name=None): + link_index = get_link_index(model, link_name) + if joint_name==None: + position_index = get_position_index(model, link_index) + else: + position_index = get_position_index(model, get_joint_index(model, link_index, joint_name)) + positions = [] + for i in range(3): + val_index = model.index(i,1,position_index) + positions.append(float(val_index.data())) + return positions + +def set_positions(model, link_name, positions, joint_name=None): + link_index = get_link_index(model, link_name) + if joint_name==None: + position_index = get_position_index(model, link_index) + else: + position_index = get_position_index(model, get_joint_index(model, link_index, joint_name)) + for i in range(3): + val_index = model.index(i,1,position_index) + val_model = val_index.model() + val_model.setData(val_index, float(positions[i])) + +def get_base_rotations(model): + link_index = get_link_index(model, 'Base') + base_rotation_index = model.index(1, 0, link_index) + rotations = [] + for i in range(3): + val_index = model.index(i,1,base_rotation_index) + rotations.append(float(val_index.data())) + return rotations + +def set_base_rotations(model, rotations): + link_index = get_link_index(model, 'Base') + base_rotation_index = model.index(1, 0, link_index) + for i in range(3): + val_index = model.index(i,1,base_rotation_index) + val_model = val_index.model() + val_model.setData(val_index, float(rotations[i])) + +def get_joint_angle(model, link_name, joint_name): + link_index = get_link_index(model, link_name) + angle_index = get_angle_index(model, get_joint_index(model, link_index, joint_name)) + val_index = model.index(0,1,angle_index) + return float(val_index.data()) + +def set_joint_angle(model, link_name, joint_name, angle): + angle = np.rad2deg(angle) + angle = float(round(angle)) + link_index = get_link_index(model, link_name) + angle_index = get_angle_index(model, get_joint_index(model, link_index, joint_name)) + val_index = model.index(0,1,angle_index) + val_model = val_index.model() + val_model.setData(val_index, float(angle)) \ No newline at end of file diff --git a/src/sparke_kinematics_tools/kinematics_gui.py b/src/sparke_kinematics_tools/kinematics_gui.py new file mode 100644 index 0000000..fd7c823 --- /dev/null +++ b/src/sparke_kinematics_tools/kinematics_gui.py @@ -0,0 +1,112 @@ +import sys +from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QVBoxLayout, QTreeWidgetItem, QFileDialog +from PyQt5.QtGui import QIcon +from PyQt5.QtCore import Qt, QModelIndex, pyqtSignal, QModelIndex +from sparke_kinematics_tools.mainwindow import Ui_MainWindow +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas +from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.pyplot as plt +from sparke_kinematics_tools.matplotlib_widget import Matplotlib3DWidget +from sparke_kinematics_tools.kinematics_tree_widget import KinematicsTree +from sparke_kinematics_tools.trajectory_tree_widget import TrajectoryTree +from sparke_kinematics_tools.kinematics_controller import kinematicsController +import csv + +class App(QMainWindow): + kinematics_data_changed_signal = pyqtSignal(QModelIndex, QModelIndex) + trajectory_data_changed_signal = pyqtSignal(QModelIndex, QModelIndex) + kinematics_header = [ + '', + 'X', + 'Y', + 'Z', + 'θ', + ] + trajectory_header = [ + 'Point', + 'X', + 'Y', + 'Z', + ] + + def __init__(self): + super().__init__() + self.setWindowTitle('Robot Dog Control') + self.setGeometry(100, 100, 800, 600) + self.setWindowIcon(QIcon('icon.png')) + + # Set up the UI from the converted Python module + self.ui = Ui_MainWindow() + self.ui.setupUi(self) + + # Get the plot widget and layout from the UI + self.plot_widget = Matplotlib3DWidget(self) + self.plot_layout = self.ui.plot_layout + self.plot_layout.addWidget(self.plot_widget) + + self.kinematics_tree_widget = KinematicsTree(self) + self.kinematics_tree_layout = self.ui.kinematics_tree_layout + self.kinematics_tree_layout.addWidget(self.kinematics_tree_widget) + kinematics_model = self.kinematics_tree_widget.tree.model() + + self.trajectory_tree_widget = TrajectoryTree(self) + self.trajectory_tree_layout = self.ui.trajectory_tree_layout + self.trajectory_tree_layout.addWidget(self.trajectory_tree_widget) + trajectory_model = self.trajectory_tree_widget.tree.model() + + self.controller = kinematicsController(self, kinematics_model, self.plot_widget) + # Connect the dataChanged signal to the controller's slot + self.kinematics_data_changed_signal.connect(self.controller.data_changed) + self.trajectory_data_changed_signal.connect(self.trajectory_tree_widget.data_changed) + self.ui.actionReset.triggered.connect(self.reset_triggered) + self.ui.actionExportKinematics.triggered.connect(self.export_kinematics) + self.ui.actionExportPoints.triggered.connect(self.export_points) + self.ui.toggle_points_button.pressed.connect(self.trajectory_tree_widget.toggle_points) + self.ui.clear_trajectory_plot_button.pressed.connect(self.plot_widget.clear_trajectory) + self.ui.plot_trajectory_button.pressed.connect(self.trajectory_tree_widget.plot_trajectory) + + # Emit the signal when the model's data changes + kinematics_model.dataChanged.connect(lambda i1, i2: self.kinematics_data_changed_signal.emit(i1, i2)) + trajectory_model.dataChanged.connect(lambda i1, i2: self.trajectory_data_changed_signal.emit(i1, i2)) + + def reset_triggered(self): + self.controller.home() + self.trajectory_tree_widget.reset_trajectory() + self.trajectory_tree_widget.reset_velocity() + + def export_kinematics(self): + options = QFileDialog.Options() + options |= QFileDialog.DontUseNativeDialog + fileName, _ = QFileDialog.getSaveFileName(self, "Export Kinematics", "", "CSV Files (*.csv);;All Files (*)", options=options) + if fileName: + if fileName.endswith('.csv'): + angles = self.controller.get_angles_from_tree() + positions = self.controller.get_positions_from_tree() + with open(fileName, 'a', newline='') as csvfile: + writer = csv.writer(csvfile, delimiter=',') + writer.writerow(self.kinematics_header) + for i in range(4): + writer.writerow([self.controller.LEG_DICT[i]]) + for j in range(3): + writer.writerow([self.controller.JOINT_DICT[j], positions[(i*3)+j][0], \ + positions[(i*3)+j][1], positions[(i*3)+j][2], angles[i][j]]) + + def export_points(self): + options = QFileDialog.Options() + options |= QFileDialog.DontUseNativeDialog + fileName, _ = QFileDialog.getSaveFileName(self, "Export Points", "", "CSV Files (*.csv);;All Files (*)", options=options) + if fileName: + if fileName.endswith('.csv'): + control_points = self.trajectory_tree_widget.generate_control_points() + with open(fileName, 'a', newline='') as csvfile: + writer = csv.writer(csvfile, delimiter=',') + writer.writerow(self.trajectory_header) + for i in range(12): + writer.writerow([i, control_points[i][0], control_points[i][1], control_points[i][2]]) + +if __name__ == '__main__': + app = QApplication(sys.argv) + ex = App() + ex.show() + sys.exit(app.exec_()) diff --git a/src/sparke_kinematics_tools/kinematics_tree_widget.py b/src/sparke_kinematics_tools/kinematics_tree_widget.py new file mode 100644 index 0000000..63e0aed --- /dev/null +++ b/src/sparke_kinematics_tools/kinematics_tree_widget.py @@ -0,0 +1,164 @@ +from PyQt5.QtCore import Qt, QModelIndex +from PyQt5.QtGui import QStandardItemModel, QStandardItem +from PyQt5.QtWidgets import QTreeView, QApplication, QWidget, QVBoxLayout, QSizePolicy, QMainWindow +from sparke_kinematics_tools.mainwindow import Ui_MainWindow +from sparke_kinematics_tools.kinematics_controller_utils import * + +class KinematicsTree(QWidget): + def __init__(self, parent: QMainWindow): + super().__init__(parent) + self.ui = parent.ui + # Initialize the model and set the headers for the tree + self.model = QStandardItemModel() + self.model.setHorizontalHeaderLabels(['Robot', '']) + self.tree = QTreeView(self) + self.tree.setModel(self.model) + self.tree.setColumnWidth(0, 150) + self.tree.setColumnWidth(1, 50) + + # Add some items to the tree, with the first column unable to be edited + self.parent_item = self.model.invisibleRootItem() + + self.parent_item.appendRow([self.generate_base_item(), self.generate_empty_item()]) + + leg_items = self.generate_leg_items() + for leg in leg_items: + self.parent_item.appendRow([leg, self.generate_empty_item()]) + + layout = QVBoxLayout(self) + layout.addWidget(self.tree) + self.ui.actionForward_Kinematics.triggered.connect(self.toggle_ik) + self.ui.actionInverse_Kinematics.triggered.connect(self.toggle_fk) + + self.mode = 'fk' + self.update_mode() + + def generate_base_item(self): + base_item = QStandardItem('Base') + base_item.setEditable(False) + base_item.appendRow([self.generate_position_item(True), self.generate_empty_item()]) + base_item.appendRow([self.generate_base_rot_item(), self.generate_empty_item()]) + return base_item + + def generate_leg_items(self): + legs = ['Front Left', 'Front Right', 'Back Left', 'Back Right'] + leg_items = [] + for leg in legs: + leg_item = QStandardItem(leg) + leg_item.setEditable(False) + joint_items = self.generate_joint_items() + for joint in joint_items: + leg_item.appendRow([joint, self.generate_empty_item()]) + leg_items.append(leg_item) + return leg_items + + def generate_joint_items(self): + joints = ['Shoulder', 'Leg', 'Wrist'] + joint_items = [] + for joint in joints: + joint_item = QStandardItem(joint) + joint_item.setEditable(False) + joint_item.appendRow([self.generate_position_item(), self.generate_empty_item()]) + joint_item.appendRow([self.generate_rotation_item(), self.generate_empty_item()]) + joint_items.append(joint_item) + return joint_items + + def generate_position_item(self, editable=False): + pos_parent_item = QStandardItem('Position') + pos_parent_item.setEditable(False) + positions = ['X', 'Y', 'Z'] + for position in positions: + position_item = QStandardItem(position) + position_item.setEditable(False) + pos_parent_item.appendRow([position_item, self.generate_empty_item(editable)]) + return pos_parent_item + + def generate_base_rot_item(self): + rot_parent_item = QStandardItem('Rotation') + rot_parent_item.setEditable(False) + rotations = ['Roll', 'Pitch', 'Yaw'] + for rotation in rotations: + rotation_item = QStandardItem(rotation) + rotation_item.setEditable(False) + rot_parent_item.appendRow([rotation_item, self.generate_empty_item(True)]) + return rot_parent_item + + def generate_rotation_item(self): + rotation_item = QStandardItem('Rotation') + rotation_item.setEditable(False) + theta_item = QStandardItem('Theta') + theta_item.setEditable(False) + rotation_item.appendRow([theta_item, self.generate_empty_item()]) + return rotation_item + + def generate_empty_item(self, editable = False): + item = QStandardItem('') + item.setEditable(editable) + return item + + def update_mode(self): + #toggle editability of joint angles (fk) / pos (ik) + if self.ui.actionForward_Kinematics.isChecked()==True and self.ui.actionInverse_Kinematics.isChecked()==False: + self.mode = 'fk' + self.activate_fk_mode() + elif self.ui.actionForward_Kinematics.isChecked()==False and self.ui.actionInverse_Kinematics.isChecked()==True: + self.mode = 'ik' + self.activate_ik_mode() + + def toggle_fk(self): + self.ui.actionForward_Kinematics.toggle() + self.update_mode() + + def toggle_ik(self): + self.ui.actionInverse_Kinematics.toggle() + self.update_mode() + + def activate_ik_mode(self): + model = self.tree.model() + model.blockSignals(True) + self.set_wrists_to_editable(True) + self.set_angles_to_editable(False) + model.blockSignals(False) + + def activate_fk_mode(self): + model = self.tree.model() + model.blockSignals(True) + self.set_wrists_to_editable(False) + self.set_angles_to_editable(True) + model.blockSignals(False) + + def set_wrists_to_editable(self, editable: bool): + model = self.tree.model() + legs = ['Front Left', + 'Front Right', + 'Back Left', + 'Back Right',] + for leg in legs: + link_index = get_link_index(model, leg) + position_index = get_position_index(model, get_joint_index(model, link_index, 'Wrist')) + for i in range(3): + val_index = model.index(i,1,position_index) + item = QStandardItemModel.itemFromIndex(self.tree.model(), val_index) + item.setEditable(editable) + + def set_angles_to_editable(self, editable: bool): + model = self.tree.model() + legs = ['Front Left', + 'Front Right', + 'Back Left', + 'Back Right',] + joints = ['Shoulder', + 'Leg', + 'Wrist',] + for leg in legs: + link_index = get_link_index(model, leg) + for joint in joints: + angle_index = get_angle_index(model, get_joint_index(model, link_index, joint)) + val_index = model.index(0,1,angle_index) + item = QStandardItemModel.itemFromIndex(self.tree.model(), val_index) + item.setEditable(editable) + +if __name__ == '__main__': + app = QApplication([]) + window = KinematicsTree() + app.exec_() diff --git a/src/sparke_kinematics_tools/mainwindow.py b/src/sparke_kinematics_tools/mainwindow.py new file mode 100644 index 0000000..e13187d --- /dev/null +++ b/src/sparke_kinematics_tools/mainwindow.py @@ -0,0 +1,209 @@ +# -*- coding: utf-8 -*- + +# Form implementation generated from reading ui file 'mainwindow.ui' +# +# Created by: PyQt5 UI code generator 5.14.1 +# +# WARNING! All changes made in this file will be lost! + + +from PyQt5 import QtCore, QtGui, QtWidgets + + +class Ui_MainWindow(object): + def setupUi(self, MainWindow): + MainWindow.setObjectName("MainWindow") + MainWindow.setWindowModality(QtCore.Qt.NonModal) + MainWindow.resize(991, 629) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(MainWindow.sizePolicy().hasHeightForWidth()) + MainWindow.setSizePolicy(sizePolicy) + MainWindow.setMinimumSize(QtCore.QSize(903, 594)) + MainWindow.setAcceptDrops(False) + MainWindow.setWindowFilePath("") + self.centralwidget = QtWidgets.QWidget(MainWindow) + self.centralwidget.setMaximumSize(QtCore.QSize(16777215, 480)) + self.centralwidget.setObjectName("centralwidget") + self.gridLayout_5 = QtWidgets.QGridLayout(self.centralwidget) + self.gridLayout_5.setObjectName("gridLayout_5") + self.plot_dock_widget = QtWidgets.QDockWidget(self.centralwidget) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.plot_dock_widget.sizePolicy().hasHeightForWidth()) + self.plot_dock_widget.setSizePolicy(sizePolicy) + self.plot_dock_widget.setMinimumSize(QtCore.QSize(480, 240)) + self.plot_dock_widget.setMaximumSize(QtCore.QSize(799, 524287)) + self.plot_dock_widget.setFeatures(QtWidgets.QDockWidget.NoDockWidgetFeatures) + self.plot_dock_widget.setAllowedAreas(QtCore.Qt.NoDockWidgetArea) + self.plot_dock_widget.setObjectName("plot_dock_widget") + self.dockWidgetContents_3 = QtWidgets.QWidget() + self.dockWidgetContents_3.setObjectName("dockWidgetContents_3") + self.gridLayout_6 = QtWidgets.QGridLayout(self.dockWidgetContents_3) + self.gridLayout_6.setObjectName("gridLayout_6") + self.plot_layout = QtWidgets.QVBoxLayout() + self.plot_layout.setObjectName("plot_layout") + self.gridLayout_6.addLayout(self.plot_layout, 0, 0, 1, 1) + self.plot_dock_widget.setWidget(self.dockWidgetContents_3) + self.gridLayout_5.addWidget(self.plot_dock_widget, 0, 1, 1, 1) + self.frame_2 = QtWidgets.QFrame(self.centralwidget) + self.frame_2.setFrameShape(QtWidgets.QFrame.VLine) + self.frame_2.setFrameShadow(QtWidgets.QFrame.Plain) + self.frame_2.setLineWidth(2) + self.frame_2.setObjectName("frame_2") + self.gridLayout_5.addWidget(self.frame_2, 0, 2, 2, 1) + self.frame_3 = QtWidgets.QFrame(self.centralwidget) + self.frame_3.setFrameShape(QtWidgets.QFrame.VLine) + self.frame_3.setFrameShadow(QtWidgets.QFrame.Plain) + self.frame_3.setLineWidth(2) + self.frame_3.setObjectName("frame_3") + self.gridLayout_5.addWidget(self.frame_3, 0, 0, 2, 1) + MainWindow.setCentralWidget(self.centralwidget) + self.menubar = QtWidgets.QMenuBar(MainWindow) + self.menubar.setGeometry(QtCore.QRect(0, 0, 991, 22)) + self.menubar.setObjectName("menubar") + self.menuFile = QtWidgets.QMenu(self.menubar) + self.menuFile.setObjectName("menuFile") + self.menuExport = QtWidgets.QMenu(self.menuFile) + self.menuExport.setObjectName("menuExport") + self.menuOptions = QtWidgets.QMenu(self.menubar) + self.menuOptions.setObjectName("menuOptions") + self.menuMode = QtWidgets.QMenu(self.menuOptions) + self.menuMode.setObjectName("menuMode") + MainWindow.setMenuBar(self.menubar) + self.statusbar = QtWidgets.QStatusBar(MainWindow) + self.statusbar.setObjectName("statusbar") + MainWindow.setStatusBar(self.statusbar) + self.tree_dock_widget = QtWidgets.QDockWidget(MainWindow) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.tree_dock_widget.sizePolicy().hasHeightForWidth()) + self.tree_dock_widget.setSizePolicy(sizePolicy) + self.tree_dock_widget.setAutoFillBackground(False) + self.tree_dock_widget.setFeatures(QtWidgets.QDockWidget.NoDockWidgetFeatures) + self.tree_dock_widget.setAllowedAreas(QtCore.Qt.LeftDockWidgetArea|QtCore.Qt.TopDockWidgetArea) + self.tree_dock_widget.setObjectName("tree_dock_widget") + self.dockWidgetContents = QtWidgets.QWidget() + self.dockWidgetContents.setObjectName("dockWidgetContents") + self.gridLayout_2 = QtWidgets.QGridLayout(self.dockWidgetContents) + self.gridLayout_2.setContentsMargins(0, 0, 0, 0) + self.gridLayout_2.setSpacing(0) + self.gridLayout_2.setObjectName("gridLayout_2") + self.kinematics_tree_layout = QtWidgets.QGridLayout() + self.kinematics_tree_layout.setObjectName("kinematics_tree_layout") + self.gridLayout_2.addLayout(self.kinematics_tree_layout, 0, 0, 1, 1) + self.tree_dock_widget.setWidget(self.dockWidgetContents) + MainWindow.addDockWidget(QtCore.Qt.DockWidgetArea(1), self.tree_dock_widget) + self.trajectory_dock_widget = QtWidgets.QDockWidget(MainWindow) + self.trajectory_dock_widget.setEnabled(True) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.trajectory_dock_widget.sizePolicy().hasHeightForWidth()) + self.trajectory_dock_widget.setSizePolicy(sizePolicy) + self.trajectory_dock_widget.setMinimumSize(QtCore.QSize(127, 380)) + self.trajectory_dock_widget.setMaximumSize(QtCore.QSize(300, 600)) + self.trajectory_dock_widget.setFeatures(QtWidgets.QDockWidget.NoDockWidgetFeatures) + self.trajectory_dock_widget.setAllowedAreas(QtCore.Qt.BottomDockWidgetArea|QtCore.Qt.RightDockWidgetArea|QtCore.Qt.TopDockWidgetArea) + self.trajectory_dock_widget.setObjectName("trajectory_dock_widget") + self.dockWidgetContents_2 = QtWidgets.QWidget() + self.dockWidgetContents_2.setObjectName("dockWidgetContents_2") + self.gridLayout_3 = QtWidgets.QGridLayout(self.dockWidgetContents_2) + self.gridLayout_3.setContentsMargins(0, 0, 0, 0) + self.gridLayout_3.setSpacing(0) + self.gridLayout_3.setObjectName("gridLayout_3") + self.trajectory_tree_layout = QtWidgets.QGridLayout() + self.trajectory_tree_layout.setSpacing(0) + self.trajectory_tree_layout.setObjectName("trajectory_tree_layout") + self.gridLayout_3.addLayout(self.trajectory_tree_layout, 0, 0, 1, 1) + self.trajectory_dock_widget.setWidget(self.dockWidgetContents_2) + MainWindow.addDockWidget(QtCore.Qt.DockWidgetArea(2), self.trajectory_dock_widget) + self.control_dock_widget = QtWidgets.QDockWidget(MainWindow) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.control_dock_widget.sizePolicy().hasHeightForWidth()) + self.control_dock_widget.setSizePolicy(sizePolicy) + self.control_dock_widget.setFeatures(QtWidgets.QDockWidget.NoDockWidgetFeatures) + self.control_dock_widget.setObjectName("control_dock_widget") + self.dockWidgetContents_4 = QtWidgets.QWidget() + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.dockWidgetContents_4.sizePolicy().hasHeightForWidth()) + self.dockWidgetContents_4.setSizePolicy(sizePolicy) + self.dockWidgetContents_4.setObjectName("dockWidgetContents_4") + self.gridLayout = QtWidgets.QGridLayout(self.dockWidgetContents_4) + self.gridLayout.setObjectName("gridLayout") + self.plot_trajectory_button = QtWidgets.QPushButton(self.dockWidgetContents_4) + self.plot_trajectory_button.setObjectName("plot_trajectory_button") + self.gridLayout.addWidget(self.plot_trajectory_button, 4, 1, 1, 1) + self.toggle_points_button = QtWidgets.QPushButton(self.dockWidgetContents_4) + self.toggle_points_button.setObjectName("toggle_points_button") + self.gridLayout.addWidget(self.toggle_points_button, 4, 0, 1, 1) + self.clear_trajectory_plot_button = QtWidgets.QPushButton(self.dockWidgetContents_4) + self.clear_trajectory_plot_button.setObjectName("clear_trajectory_plot_button") + self.gridLayout.addWidget(self.clear_trajectory_plot_button, 4, 2, 1, 1) + self.control_dock_widget.setWidget(self.dockWidgetContents_4) + MainWindow.addDockWidget(QtCore.Qt.DockWidgetArea(8), self.control_dock_widget) + self.actionReset = QtWidgets.QAction(MainWindow) + self.actionReset.setObjectName("actionReset") + self.actionForward_Kinematics = QtWidgets.QAction(MainWindow) + self.actionForward_Kinematics.setCheckable(True) + self.actionForward_Kinematics.setChecked(True) + self.actionForward_Kinematics.setObjectName("actionForward_Kinematics") + self.actionInverse_Kinematics = QtWidgets.QAction(MainWindow) + self.actionInverse_Kinematics.setCheckable(True) + self.actionInverse_Kinematics.setObjectName("actionInverse_Kinematics") + self.actionHome_Position = QtWidgets.QAction(MainWindow) + self.actionHome_Position.setCheckable(True) + self.actionHome_Position.setObjectName("actionHome_Position") + self.actionKinematics = QtWidgets.QAction(MainWindow) + self.actionKinematics.setCheckable(True) + self.actionKinematics.setObjectName("actionKinematics") + self.actionExit = QtWidgets.QAction(MainWindow) + self.actionExit.setObjectName("actionExit") + self.actionExportKinematics = QtWidgets.QAction(MainWindow) + self.actionExportKinematics.setObjectName("actionExportKinematics") + self.actionExportPoints = QtWidgets.QAction(MainWindow) + self.actionExportPoints.setObjectName("actionExportPoints") + self.menuExport.addAction(self.actionExportKinematics) + self.menuExport.addAction(self.actionExportPoints) + self.menuFile.addAction(self.actionReset) + self.menuFile.addAction(self.menuExport.menuAction()) + self.menuFile.addAction(self.actionExit) + self.menuMode.addAction(self.actionForward_Kinematics) + self.menuMode.addAction(self.actionInverse_Kinematics) + self.menuOptions.addAction(self.menuMode.menuAction()) + self.menubar.addAction(self.menuFile.menuAction()) + self.menubar.addAction(self.menuOptions.menuAction()) + + self.retranslateUi(MainWindow) + self.actionExit.triggered.connect(MainWindow.close) + QtCore.QMetaObject.connectSlotsByName(MainWindow) + + def retranslateUi(self, MainWindow): + _translate = QtCore.QCoreApplication.translate + MainWindow.setWindowTitle(_translate("MainWindow", "Sparke Kinematics Previewer")) + self.plot_dock_widget.setWindowTitle(_translate("MainWindow", "3D Plot")) + self.menuFile.setTitle(_translate("MainWindow", "File")) + self.menuExport.setTitle(_translate("MainWindow", "Export")) + self.menuOptions.setTitle(_translate("MainWindow", "Options")) + self.menuMode.setTitle(_translate("MainWindow", "Mode")) + self.tree_dock_widget.setWindowTitle(_translate("MainWindow", "Robot Tree")) + self.trajectory_dock_widget.setWindowTitle(_translate("MainWindow", "Trajectory")) + self.control_dock_widget.setWindowTitle(_translate("MainWindow", "Control")) + self.plot_trajectory_button.setText(_translate("MainWindow", "Plot Trajectory")) + self.toggle_points_button.setText(_translate("MainWindow", "Toggle Points")) + self.clear_trajectory_plot_button.setText(_translate("MainWindow", "Clear Trajectory Plot")) + self.actionReset.setText(_translate("MainWindow", "Reset")) + self.actionForward_Kinematics.setText(_translate("MainWindow", "Forward Kinematics")) + self.actionInverse_Kinematics.setText(_translate("MainWindow", "Inverse Kinematics")) + self.actionHome_Position.setText(_translate("MainWindow", "Home Position")) + self.actionKinematics.setText(_translate("MainWindow", "Kinematics")) + self.actionExit.setText(_translate("MainWindow", "Exit")) + self.actionExportKinematics.setText(_translate("MainWindow", "Kinematics")) + self.actionExportPoints.setText(_translate("MainWindow", "Control Points")) diff --git a/src/sparke_kinematics_tools/mainwindow.ui b/src/sparke_kinematics_tools/mainwindow.ui new file mode 100644 index 0000000..f7a2500 --- /dev/null +++ b/src/sparke_kinematics_tools/mainwindow.ui @@ -0,0 +1,381 @@ + + + MainWindow + + + Qt::NonModal + + + + 0 + 0 + 991 + 629 + + + + + 0 + 0 + + + + + 903 + 594 + + + + false + + + Sparke Kinematics Previewer + + + + + + + + 16777215 + 480 + + + + + + + + 0 + 0 + + + + + 480 + 240 + + + + + 799 + 524287 + + + + QDockWidget::NoDockWidgetFeatures + + + Qt::NoDockWidgetArea + + + 3D Plot + + + + + + + + + + + + + + QFrame::VLine + + + QFrame::Plain + + + 2 + + + + + + + QFrame::VLine + + + QFrame::Plain + + + 2 + + + + + + + + + 0 + 0 + 991 + 22 + + + + + File + + + + Export + + + + + + + + + + + Options + + + + Mode + + + + + + + + + + + + + + 0 + 0 + + + + false + + + QDockWidget::NoDockWidgetFeatures + + + Qt::LeftDockWidgetArea|Qt::TopDockWidgetArea + + + Robot Tree + + + 1 + + + + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + + + + + true + + + + 0 + 0 + + + + + 127 + 380 + + + + + 300 + 600 + + + + QDockWidget::NoDockWidgetFeatures + + + Qt::BottomDockWidgetArea|Qt::RightDockWidgetArea|Qt::TopDockWidgetArea + + + Trajectory + + + 2 + + + + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + + + + + 0 + + + + + + + + + + 0 + 0 + + + + QDockWidget::NoDockWidgetFeatures + + + Control + + + 8 + + + + + 0 + 0 + + + + + + + Plot Trajectory + + + + + + + Toggle Points + + + + + + + Clear Trajectory Plot + + + + + + + + + Reset + + + + + true + + + true + + + Forward Kinematics + + + + + true + + + Inverse Kinematics + + + + + true + + + Home Position + + + + + true + + + Kinematics + + + + + Exit + + + + + Kinematics + + + + + Control Points + + + + + + + actionExit + triggered() + MainWindow + close() + + + -1 + -1 + + + 495 + 314 + + + + + diff --git a/src/sparke_kinematics_tools/matplotlib_widget.py b/src/sparke_kinematics_tools/matplotlib_widget.py new file mode 100644 index 0000000..e4cd1cf --- /dev/null +++ b/src/sparke_kinematics_tools/matplotlib_widget.py @@ -0,0 +1,107 @@ +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from PyQt5.QtCore import Qt +from PyQt5.QtWidgets import QWidget, QVBoxLayout, QSizePolicy +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas +from sparke_kinematics_tools.bezier_utils import * + +class Matplotlib3DWidget(QWidget): + def __init__(self, parent=None): + super().__init__(parent) + self.fig = plt.Figure() + self.ax = self.fig.add_subplot(111, projection='3d') + self.canvas = FigureCanvas(self.fig) + self.canvas.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self.canvas.updateGeometry() + layout = QVBoxLayout(self) + layout.addWidget(self.canvas) + self.button_pressed = False + self.start_x = None + self.start_y = None + self.start_rotation = None + self.trajectory = None + self.points = None + self.canvas.mpl_connect('button_press_event', self.on_button_press) + self.canvas.mpl_connect('motion_notify_event', self.on_motion_notify) + self.canvas.mpl_connect('button_release_event', self.on_button_release) + + def on_button_press(self, event): + if event.button == Qt.LeftButton: + self.button_pressed = True + self.start_x = event.x + self.start_y = event.y + self.start_rotation = self.ax.azim, self.ax.elev + + def on_motion_notify(self, event): + if self.button_pressed: + dx = event.x - self.start_x + dy = -(event.y - self.start_y) # invert the y direction + rotation = self.start_rotation[1] + dy/2, self.start_rotation[0] - dx/2 + self.ax.view_init(*rotation) + self.canvas.draw() + + def on_button_release(self, event): + if event.button == Qt.LeftButton: + self.button_pressed = False + + def update_plot(self, positions, color='blue'): + """ + Update the plot with an array of 3D positions, connected by a line with the specified color. + :param positions: list of 3D positions, each of the form [x, y, z] + :param color: optional color string for the line + """ + xs, ys, zs = zip(*positions) + self.ax.plot(xs, ys, zs, color=color) + + self.ax.set_xlim(-0.3, 0.3) + self.ax.set_ylim(-0.2, 0.2) + self.ax.set_zlim(-0.18384776, 0.18384776) + + self.canvas.draw() + + def plot_points(self, positions): + """ + Plot each point in the list of positions as an independent point. + :param positions: list of 3D positions, each of the form [x, y, z] + """ + self.clear_points() + xs, ys, zs = zip(*positions) + + self.points = self.ax.scatter(xs, ys, zs, color='cyan') + + self.ax.set_xlim(-0.3, 0.3) + self.ax.set_ylim(-0.2, 0.2) + self.ax.set_zlim(-0.18384776, 0.18384776) + + self.canvas.draw() + + def plot_trajectory(self, control_points): + self.clear_trajectory() + control_points = np.array(control_points) + curve = bezier_curve(control_points) + xs = curve[:, 0] + ys = curve[:, 1] + zs = curve[:, 2] + self.trajectory, = self.ax.plot(xs, ys, zs, color='orange') + self.canvas.draw() + + def clear_trajectory(self): + if self.trajectory is not None: + try: + self.trajectory.remove() + except: + pass + self.canvas.draw() + + def clear_points(self): + if self.points is not None: + try: + self.points.remove() + except: + pass + self.canvas.draw() + + def clear_plot(self): + self.ax.clear() + self.canvas.draw() \ No newline at end of file diff --git a/src/sparke_kinematics_tools/trajectory_tree_widget.py b/src/sparke_kinematics_tools/trajectory_tree_widget.py new file mode 100644 index 0000000..5a47f2e --- /dev/null +++ b/src/sparke_kinematics_tools/trajectory_tree_widget.py @@ -0,0 +1,155 @@ +from PyQt5.QtCore import Qt, QModelIndex +from PyQt5.QtGui import QStandardItemModel, QStandardItem +from PyQt5.QtWidgets import QTreeView, QApplication, QWidget, QVBoxLayout, QSizePolicy, QMainWindow +from sparke_kinematics_tools.trajectory_utils import * +class TrajectoryTree(QWidget): + DEFAULT_VEL = [ + 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, + ] + END_POINT = [0.10807106781186548, 0.11, -0.18384776310850237] + DEFAULT_CONTROL_POINTS = [ + [END_POINT[0] - DEFAULT_VEL[0], END_POINT[1], END_POINT[2]], #0 + [END_POINT[0] - DEFAULT_VEL[0] - 0.01, END_POINT[1], END_POINT[2]], #1 + [END_POINT[0] - DEFAULT_VEL[0] - 0.02, END_POINT[1], END_POINT[2]+0.03], #2 + [END_POINT[0] - DEFAULT_VEL[0] - 0.02, END_POINT[1], END_POINT[2]+0.03], #3 + [END_POINT[0] - DEFAULT_VEL[0] - 0.02, END_POINT[1], END_POINT[2]+0.03], #4 + [END_POINT[0] - (DEFAULT_VEL[0]/2), END_POINT[1], END_POINT[2]+0.03], #5 + [END_POINT[0] - (DEFAULT_VEL[0]/2), END_POINT[1], END_POINT[2]+0.03], #6 + [END_POINT[0] - (DEFAULT_VEL[0]/2), END_POINT[1], END_POINT[2]+0.05], #7 + [END_POINT[0]+0.02, END_POINT[1], END_POINT[2]+0.05], #8 + [END_POINT[0]+0.02, END_POINT[1], END_POINT[2]+0.05], #9 + [END_POINT[0]+0.01, END_POINT[1], END_POINT[2]], #10 + END_POINT, + ] + + def __init__(self, parent: QMainWindow): + super().__init__(parent) + self.__parent = parent + self.ui = parent.ui + # Initialize the model and set the headers for the tree + self.model = QStandardItemModel() + self.model.setHorizontalHeaderLabels(['Control Points', '']) + self.tree = QTreeView(self) + self.tree.setModel(self.model) + self.tree.setColumnWidth(0, 125) + self.tree.setColumnWidth(1, 50) + self.tree_model = self.tree.model() + + self.points_enabled = False + + # Add some items to the tree, with the first column unable to be edited + self.parent_item = self.model.invisibleRootItem() + + self.parent_item.appendRow([self.generate_velocity_item(), self.generate_empty_item()]) + + point_items = self.generate_point_items() + for point in point_items: + self.parent_item.appendRow([point, self.generate_empty_item()]) + + set_velocity(self.tree_model, self.DEFAULT_VEL) + set_points(self.tree_model, self.DEFAULT_CONTROL_POINTS) + + self.__parent.plot_widget.plot_points(self.DEFAULT_CONTROL_POINTS) + + layout = QVBoxLayout(self) + layout.addWidget(self.tree) + + def generate_velocity_item(self): + vel_item = QStandardItem('Velocity') + vel_item.setEditable(False) + vel_item.appendRow([self.generate_linear_vel_item(), self.generate_empty_item()]) + vel_item.appendRow([self.generate_angular_vel_item(), self.generate_empty_item()]) + return vel_item + + def generate_linear_vel_item(self): + linear_vel_parent_item = QStandardItem('Linear') + linear_vel_parent_item.setEditable(False) + linear_vels = ['X', 'Y', 'Z'] + for vel in linear_vels: + vel_item = QStandardItem(vel) + vel_item.setEditable(False) + if vel == 'Z': + editing = False + else: + editing = True + linear_vel_parent_item.appendRow([vel_item, self.generate_empty_item(editing)]) + return linear_vel_parent_item + + def generate_angular_vel_item(self): + angular_vel_parent_item = QStandardItem('Angular') + angular_vel_parent_item.setEditable(False) + angular_vels = ['X\'', 'Y\'', 'Z\''] + for vel in angular_vels: + vel_item = QStandardItem(vel) + vel_item.setEditable(False) + if vel == 'Z\'': + editing = True + else: + editing = False + angular_vel_parent_item.appendRow([vel_item, self.generate_empty_item(editing)]) + return angular_vel_parent_item + + def generate_point_items(self): + points = [] + for i in range(12): + points.append(f'P{i+1}') + + positions = ['X', 'Y', 'Z'] + + point_items = [] + for i in range(len(points)): + pos_parent_item = QStandardItem(points[i]) + pos_parent_item.setEditable(False) + if i in range(1,11): + editing = True + else: + editing = False + for position in positions: + position_item = QStandardItem(position) + position_item.setEditable(False) + pos_parent_item.appendRow([position_item, self.generate_empty_item(editing)]) + point_items.append(pos_parent_item) + return point_items + + def generate_empty_item(self, editable = False): + item = QStandardItem('') + item.setEditable(editable) + return item + + def toggle_points(self): + self.points_enabled = not self.points_enabled + if self.points_enabled: + control_points = self.generate_control_points() + self.__parent.plot_widget.plot_points(control_points) + else: + self.__parent.plot_widget.clear_points() + + def reset_trajectory(self): + set_points(self.tree_model, self.DEFAULT_CONTROL_POINTS) + + def reset_velocity(self): + set_velocity(self.tree_model, self.DEFAULT_VEL) + + def plot_trajectory(self): + control_points = self.generate_control_points() + self.__parent.plot_widget.plot_trajectory(control_points) + + def generate_control_points(self): + model = self.tree.model() + vel = get_velocity(self.tree_model) + points = get_points(self.tree_model) + + points[0][0] = self.END_POINT[0] - vel[0] + points[0][1] = self.END_POINT[1] - vel[1] + points[0][2] = self.END_POINT[2] - vel[2] + points[11] = self.END_POINT + + model.blockSignals(True) + set_points(self.tree_model, points) + model.blockSignals(False) + return points + + def data_changed(self): + if self.points_enabled: + control_points = self.generate_control_points() + self.__parent.plot_widget.plot_points(control_points) \ No newline at end of file diff --git a/src/sparke_kinematics_tools/trajectory_utils.py b/src/sparke_kinematics_tools/trajectory_utils.py new file mode 100644 index 0000000..f6849c6 --- /dev/null +++ b/src/sparke_kinematics_tools/trajectory_utils.py @@ -0,0 +1,60 @@ +from PyQt5.QtCore import QModelIndex + +def get_velocity(model): + lin_vel_index = get_lin_vel_index(model) + ang_vel_index = get_ang_vel_index(model) + vel = [] + for i in range(3): + val_index = model.index(i, 1, lin_vel_index) + vel.append(float(val_index.data())) + + for j in range(3): + val_index = model.index(j, 1, ang_vel_index) + vel.append(float(val_index.data())) + return vel + +def set_velocity(model, cmd_vel): + lin_vel_index = get_lin_vel_index(model) + ang_vel_index = get_ang_vel_index(model) + vel = [] + for i in range(3): + val_index = model.index(i, 1, lin_vel_index) + val_model = val_index.model() + val_model.setData(val_index, float(cmd_vel[i])) + val_index = model.index(i, 1, ang_vel_index) + val_model = val_index.model() + val_model.setData(val_index, float(cmd_vel[i+3])) + +def get_points(model): + points = [] + for i in range(1, 13): + position = [] + point_index = get_point_index(model, i) + for j in range(3): + val_index = model.index(j, 1, point_index) + position.append(float(val_index.data())) + points.append(position) + return points + +def set_points(model, points): + for i in range(1, 13): + position = points[i-1] + point_index = get_point_index(model, i) + for j in range(3): + val_index = model.index(j, 1, point_index) + val_model = val_index.model() + val_model.setData(val_index, float(position[j])) + +def get_vel_index(model): + return model.index(0,0, QModelIndex()) + +def get_lin_vel_index(model): + vel_index = get_vel_index(model) + return model.index(0,0, vel_index) + +def get_ang_vel_index(model): + vel_index = get_vel_index(model) + return model.index(1,0, vel_index) + +def get_point_index(model, point_num): + return model.index(point_num, 0, QModelIndex()) \ No newline at end of file