fix plot coord arrow for std dh

CKAyano · Aug 10, 2022 · 5f6b222 · 5f6b222
1 parent b34b2e8
commit 5f6b222
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 7 deletions.
diff --git a/example.py b/example.py
@@ -4,6 +4,43 @@
 import sympy as sp
 
 
+def fanuc():
+    ang_90 = np.pi / 2
+
+    dh = {
+        "theta": [0, -np.pi / 2],
+        "d": [100, 0],
+        "a": [0, 260],
+        "alpha": [-ang_90, 0],
+    }
+    # dh = {
+    #     "theta": [0, -np.pi / 2, 0],
+    #     "d": [0, 100, 0],
+    #     "a": [0, 260, 20],
+    #     "alpha": [-ang_90, 0, -ang_90],
+    # }
+
+    fanuc = Robot(dh, "Fanuc", DHAngleType.RAD, DHType.STANDARD)
+    # links = fanuc.forword_kine([0, 0, 0, 0, 0, 0])
+    fanuc.plot([0, 0])
+    # print(links[-1].matrix)
+
+
+def puma_std():
+    ang_90 = np.pi / 2
+
+    dh = {
+        "theta": [0, 0, 0, 0, 0, 0],
+        "d": [0, 149.09, 0, 433.07, 0, 0],
+        "a": [0, 431.8, -20.32, 0, 0, 0],
+        "alpha": [-ang_90, 0, ang_90, -ang_90, ang_90, 0],
+    }
+
+    # 生成機械手臂物件（需代入DH參數）
+    puma = Robot(dh, "puma", dh_angle=DHAngleType.RAD, dh_type=DHType.STANDARD)
+    puma.plot([0, 0, 2.8, 0, 0, 0])
+
+
 def puma():
     ang_90 = np.pi / 2
 
@@ -69,4 +106,4 @@ def puma_symbol():
 
 
 if __name__ == "__main__":
-    puma()
+    puma_std()
diff --git a/robotck/plot.py b/robotck/plot.py
@@ -139,6 +139,12 @@ def plot_robot(
         p_y.append(np.round(t.coord[1], 4))
         p_z.append(np.round(t.coord[2], 4))
 
+        if show_coord:
+            arrow_start = t.get_coord_list()
+            _plot_arrow(ax, t, arrow_start, [arrow_length, 0, 0], "red")
+            _plot_arrow(ax, t, arrow_start, [0, arrow_length, 0], "green")
+            _plot_arrow(ax, t, arrow_start, [0, 0, arrow_length], "blue")
+
         if i == len(trans_list) - 1:
             if dh_type == DHType.STANDARD:
                 break
@@ -152,12 +158,6 @@ def plot_robot(
 
         ax.plot_surface(x_t, y_t, z_t, rstride=1, cstride=1, linewidth=0, alpha=1)
 
-        if show_coord:
-            arrow_start = t.get_coord_list()
-            _plot_arrow(ax, t, arrow_start, [arrow_length, 0, 0], "red")
-            _plot_arrow(ax, t, arrow_start, [0, arrow_length, 0], "green")
-            _plot_arrow(ax, t, arrow_start, [0, 0, arrow_length], "blue")
-
     ax.plot3D(p_x, p_y, p_z, "-r")
     ax.plot3D(p_x, p_y, p_z, ".b")
     _set_axes_equal(ax)