Updated to draw correct STLs for each joint, and apply fkine when sliders change.

micah-huth · micah-huth · commit 0d4e0628b84d · 2020-08-24T09:55:53.000+10:00
diff --git a/graphics/graphics_canvas.py b/graphics/graphics_canvas.py
@@ -453,6 +453,9 @@ def __setup_joint_sliders(self):
         """
         i = 1
         for joint in self.__teachpanel[self.__selected_robot]:
+            if joint[self.__idx_qlim_min] == joint[self.__idx_qlim_max]:
+                # If a slider with (effectively) no values, skip it
+                continue
             # Add a title
             self.scene.append_to_caption('Joint {0}:\t'.format(i))
             i += 1
@@ -597,12 +600,11 @@ def __joint_slider(self, s):
 
         # Get all angles for the robot
         angles = []
-        for joint_slider in self.__teachpanel_sliders:
-            angles.append(joint_slider.value)
+        for idx in range(len(self.__teachpanel_sliders)):
+            angles.append(self.__teachpanel_sliders[idx].value)
 
         # Run fkine
         poses = self.__robots[self.__selected_robot].fkine(angles)
-        poses = poses[1:len(poses)]  # Ignore the first item
 
         # Update joints
         self.__robots[self.__selected_robot].set_joint_poses(poses)
diff --git a/graphics/graphics_robot.py b/graphics/graphics_robot.py
@@ -580,12 +580,14 @@ def __init__(self, graphics_canvas, name, seriallink=None):
             # Get initial poses
             zero_angles = [0] * len(self.seriallink.links)
             all_poses = self.seriallink.fkine(zero_angles, alltout=True)
+            # Create the base
+            self.append_link("s", all_poses[0], str(seriallink.basemesh), [0, 0], 0)
             # Create the joints
             i = 0
             for link in self.seriallink.links:
                 # Get info
                 j_type = link.jointtype  # Type of
-                pose = all_poses[i]  # Pose
+                pose = all_poses[i+1]  # Pose
                 if link.mesh is None:
                     x1, x2 = all_poses[i].t[0], all_poses[i + 1].t[0]
                     y1, y2 = all_poses[i].t[1], all_poses[i + 1].t[1]
@@ -725,9 +727,9 @@ def set_joint_poses(self, all_poses):
         if self.num_joints == 0:
             raise UserWarning("Robot has 0 joints. Create some using append_link()")
 
-        # if self.num_joints != len(all_poses):
-        #     err = "Number of given poses {0} does not equal number of joints {1}"
-        #     raise UserWarning(err.format(len(all_poses), self.num_joints))
+        if self.num_joints != len(all_poses):
+            err = "Number of given poses {0} does not equal number of joints {1}"
+            raise UserWarning(err.format(len(all_poses), self.num_joints))
 
         # Update the joints
         for idx in range(self.num_joints):
diff --git a/roboticstoolbox/robot/serial_link.py b/roboticstoolbox/robot/serial_link.py
@@ -185,17 +185,14 @@ def plot(self, jointconfig, unit='rad'):
 
         if self.roplot is None:
             # No current plot, create robot plot
+            self.g_canvas = gph.GraphicsCanvas3D()
+            print("canvas created")
 
-                self.g_canvas = gph.GraphicsCanvas3D()
-                print("canvas created")
+            self.roplot = gph.GraphicalRobot(self.g_canvas, self.name, self)
 
-                self.roplot = gph.GraphicalRobot(self.g_canvas, self.name, self)
-
-                return
-        else:
-            # Move existing plot
-            self.roplot.set_joint_poses(poses)
-            return
+        # Move existing plot
+        self.roplot.set_joint_poses(poses)
+        return
 
     def animate(self, q1, q2, unit='rad', frames=10, fps=5):
         """
diff --git a/tests/_test_teachpanel.py b/tests/_test_teachpanel.py
@@ -1,22 +1,28 @@
 import graphics as gph
 from roboticstoolbox.robot.serial_link import SerialLink, Link
+from roboticstoolbox.models.Puma560 import Puma560
 
 if __name__ == "__main__":
-    canvas = gph.GraphicsCanvas3D()
 
-    L = []
-    L2 = []
+    puma = Puma560()
 
-    L.append(Link(a=1, jointtype='R'))
-    L.append(Link(a=1, jointtype='R'))
-    L.append(Link(a=1, jointtype='R'))
+    puma.plot(puma.qz)
 
-    L2.append(Link(a=1, jointtype='R'))
-    L2.append(Link(a=1, jointtype='R'))
-    L2.append(Link(a=1, jointtype='R'))
-
-    tl = SerialLink(L, name='R1')
-    tl2 = SerialLink(L, name='R2')
-
-    robot = gph.GraphicalRobot(canvas, '', seriallink=tl)
-    robot2 = gph.GraphicalRobot(canvas, '', seriallink=tl2)
+    # canvas = gph.GraphicsCanvas3D()
+    #
+    # L = []
+    # L2 = []
+    #
+    # L.append(Link(a=1, jointtype='R'))
+    # L.append(Link(a=1, jointtype='R'))
+    # L.append(Link(a=1, jointtype='R'))
+    #
+    # L2.append(Link(a=1, jointtype='R'))
+    # L2.append(Link(a=1, jointtype='R'))
+    # L2.append(Link(a=1, jointtype='R'))
+    #
+    # tl = SerialLink(L, name='R1')
+    # tl2 = SerialLink(L, name='R2')
+    #
+    # robot = gph.GraphicalRobot(canvas, '', seriallink=tl)
+    # robot2 = gph.GraphicalRobot(canvas, '', seriallink=tl2)
