Add check that when using non-stl seriallink, that the base pose is ignored when using poses from fkine

micah-huth · micah-huth · commit ff34211ad2ad · 2020-08-24T10:30:42.000+10:00
diff --git a/graphics/graphics_robot.py b/graphics/graphics_robot.py
@@ -3,6 +3,7 @@
 from graphics.common_functions import *
 from graphics.graphics_stl import set_stl_origin, import_object_from_numpy_stl
 from time import perf_counter
+from spatialmath import SE3
 
 
 class DefaultJoint:
@@ -581,7 +582,9 @@ def __init__(self, graphics_canvas, name, seriallink=None):
             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)
+            if seriallink.basemesh is not None:
+                self.append_link("s", all_poses[0], str(seriallink.basemesh), [0, 0], 0)
+            # else: assume no base joint
             # Create the joints
             i = 0
             for link in self.seriallink.links:
@@ -727,6 +730,14 @@ def set_joint_poses(self, all_poses):
         if self.num_joints == 0:
             raise UserWarning("Robot has 0 joints. Create some using append_link()")
 
+        # If given a base pose, when there isn't one
+        if self.num_joints == len(all_poses) - 1 and all_poses[0] == SE3():
+            # Update the joints
+            for idx in range(self.num_joints):
+                self.joints[idx].update_pose(all_poses[idx+1])
+            return
+
+        # If incorrect number of 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))
