Merge pull request #3 from Infinite-Echo/gui_release

GUI tool release

.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/*

setup.cfg

@@ -1,6 +1,6 @@
 [metadata]
 name = sparke_kinematics
-version = 0.0.2a
+version = 0.1.0b0
 author = Colin Fuelberth
 author_email = [email protected]
 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
+    matplotlib >= 3.1.2
+    PyQt5
+
+[options.packages.find]
+where = src
+
+[options.package_data]
+sparke_kinematics_tools=*.ui

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,24 +31,33 @@ 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)
     t23 = legtf.create_T23(np.pi/2)
     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__':

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

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

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