update

app.py

@@ -101,7 +101,7 @@ def highlight_str(val):
 
 def warning_msg_box(msg: str):
     msg_box = QMessageBox()
-    msg_box.setWindowTitle("輸入錯誤")
+    msg_box.setWindowTitle("警告")
 
     pixmap = QStyle.SP_MessageBoxWarning
     icon = QDialog().style().standardIcon(pixmap)
@@ -113,6 +113,20 @@ def warning_msg_box(msg: str):
     msg_box.exec()
 
 
+def info_msg_box(msg: str):
+    msg_box = QMessageBox()
+    msg_box.setWindowTitle("輸入錯誤")
+
+    pixmap = QStyle.SP_MessageBoxInformation
+    icon = QDialog().style().standardIcon(pixmap)
+    msg_box.setWindowIcon(icon)
+
+    msg_box.setIcon(msg_box.Information)
+
+    msg_box.setText(msg)
+    msg_box.exec()
+
+
 def is_validated_config(json_path):
     if not os.path.exists(json_path) or os.stat(json_path).st_size == 0:
         with open(json_path, "w") as file:
@@ -203,7 +217,7 @@ def _connect_event(self):
         self.pushButton_ik_result.clicked.connect(self.on_calc_ik_result)
         self.checkBox_ik_round.clicked.connect(self.on_click_ik_checkbox_round)
 
-        self.tabWidget_main.currentChanged.connect(self.on_change_dh)
+        # self.tabWidget_main.currentChanged.connect(self.on_change_dh)
 
         self.pushButton_plot_output.clicked.connect(self.on_plot_result)
 
@@ -253,23 +267,24 @@ def on_change_dh(self):
             else:
                 dh_type_str = "Modified"
             self.label_info.setText(f"機械手臂: {robot.name}, 軸數: {joints_count}, D-H型態: {dh_type_str}")
-            if self.is_current_tab(0):
-                self.set_fk_input(joints_count)
-                self.set_fk_output(joints_count)
-            if self.is_current_tab(1):
-                self.set_ik_input(joints_count)
-                self.set_ik_output()
-                self.on_change_ik_method()
-            if self.is_current_tab(2):
-                self.set_plot_input(joints_count)
-                self.set_plot_output([0] * robot.links_count)
+            # if self.is_current_tab(0):
+            self.set_fk_input(joints_count)
+            self.set_fk_output(joints_count)
+            # if self.is_current_tab(1):
+            self.set_ik_input(joints_count)
+            self.set_ik_output()
+            self.on_change_ik_method()
+            # if self.is_current_tab(2):
+            self.set_plot_input(joints_count)
+            self.set_plot_output([0] * robot.links_count)
 
     def set_dh_default_fk_io(self):
         self.set_fk_input(2)
         self.set_fk_output(2)
         for i in self.doubleSpinBox_fk_j_list:
             i.setDisabled(True)
         self.pushButton_fk_result.setDisabled(True)
+        self.textBrowser_fk_result.clear()
 
     def set_dh_default_ik_io(self):
         self.set_ik_input(2)
@@ -281,11 +296,15 @@ def set_dh_default_ik_io(self):
         for i in self.doubleSpinBox_ik_init_list:
             i.setDisabled(True)
         self.pushButton_ik_result.setDisabled(True)
+        self.textBrowser_ik_result.clear()
 
     def set_dh_default_plot_io(self):
         self.set_plot_input(2)
         self.widget_plot_fig.axes[0].clear()
+        self.widget_plot_fig.canvas.draw_idle()
         self.pushButton_plot_output.setDisabled(True)
+        for i in self.doubleSpinBox_plot_j_list:
+            i.setDisabled(True)
 
     def set_dh_default(self):
         self.robot_instance = None
@@ -337,6 +356,7 @@ def set_fk_output(self, joints_count: int):
         if not self.checkBox_fk_round.isChecked():
             self.spinBox_fk_round.setDisabled(True)
         self.pushButton_fk_result.setDisabled(False)
+        self.textBrowser_fk_result.clear()
 
     def on_click_fk_checkbox_round(self):
         if self.checkBox_fk_round.isChecked():
@@ -442,6 +462,7 @@ def set_ik_output(self):
         self.pushButton_ik_result.setDisabled(False)
         if not self.checkBox_ik_round.isChecked():
             self.spinBox_ik_round.setDisabled(True)
+        self.textBrowser_ik_result.clear()
 
     def on_change_ik_method(self):
         if self.comboBox_ik_method.currentText() == "Simplex":
@@ -468,7 +489,11 @@ def get_ik(self):
                 init_angle.append(i.value())
             if self.radioButton_ik_init_deg.isChecked():
                 init_angle = deg2rad(init_angle)
-            ik = self.robot_instance.inverse_kine_simplex(coord, init_angle)
+            ik, is_warned, err = self.robot_instance.inverse_kine_simplex(coord, init_angle, save_err=True)
+            if is_warned:
+                warning_msg_box(f"座標誤差高於0.1({err:.4f}), 此座標「可能」無法到達, 或嘗試調整初始角度")
+            else:
+                info_msg_box(f"座標誤差為{err:E}")
         elif self.comboBox_ik_method.currentText() == "Pieper":
             try:
                 ik = self.robot_instance.inverse_kine_pieper_first_three(coord)
@@ -511,8 +536,9 @@ def replot_robot(self, joints_angle=None):
                 for j in range(_dh_array.shape[1]):
                     if isinstance(_dh_array[i, j], str):
                         _dh_array[i, j] = np.nan
-            longest_link = np.nanmean(np.abs(_dh_array))
-            joint_radius = longest_link * 0.1
+            longest_link = np.nanmax(np.abs(_dh_array))
+            joint_radius = longest_link * 0.05
+            # joint_radius = 10
             self.robot_instance.plot(
                 joints_angle, joint_radius=joint_radius, qt_ax=self.widget_plot_fig.axes[0], is_plot=False
             )

conf/dh.json

@@ -1 +1,49 @@
-[]
+[
+  {
+    "robot_name": "Fanuc",
+    "dh": {
+      "d": [
+        0.0,
+        0.0,
+        0.0,
+        290.0,
+        0.0,
+        0.0
+      ],
+      "theta": [
+        0.0,
+        -90.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0
+      ],
+      "a": [
+        0.0,
+        260.0,
+        20.0,
+        0.0,
+        0.0,
+        0.0
+      ],
+      "alpha": [
+        -90.0,
+        0.0,
+        -90.0,
+        90.0,
+        -90.0,
+        0.0
+      ]
+    },
+    "is_std": true,
+    "is_rad": false,
+    "is_revol": [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ]
+  }
+]

robotck/robot.py

@@ -11,6 +11,7 @@
 from robotck.nelder_mead_simplex import simplex
 import matplotlib.pyplot as plt
 import copy
+import warnings
 
 
 T = TypeVar("T", HomoMatrix, Links)
@@ -311,6 +312,7 @@ def fitness(joints):
             err = np.sqrt(np.sum(np.square(coord - coord_fit)))
             return err
 
+        is_warned = False
         if isinstance(coord, list):
             coord = np.array(coord)
         if isinstance(init_ang, list):
@@ -332,16 +334,14 @@ def fitness(joints):
         joints = res[0]
         err = res[1]
 
-        try:
-            if err >= 0.1:
-                raise Warning("Error is greater than 0.1")
-        except Warning as e:
-            print(repr(e))
+        if err >= 0.1:
+            warnings.warn("Error is greater than 0.1")
+            is_warned = True
 
         if save_err:
-            return joints, err
+            return joints, is_warned, err
 
-        return joints
+        return joints, is_warned
 
     def plot(
         self,