Update graphical robot fkine to also run set_joint_poses, and save the updated angles locally.

micah-huth · micah-huth · commit e6800ef07360 · 2021-03-31T12:36:07.000+10:00
Also fixes missing function call parenthesis (was just running equality on function itself, not result)
diff --git a/roboticstoolbox/backends/VPython/VPython.py b/roboticstoolbox/backends/VPython/VPython.py
@@ -171,11 +171,10 @@ def step(self, dt=None, id=None, q=None, fig_num=0):
         # If GraphicalRobot given
         if isinstance(id, GraphicalRobot):
             if self.canvases[fig_num].is_robot_in(id):
-                poses = id.fkine(q)
-                id.set_joint_poses(poses)
-                if self.canvases[fig_num].current_mode == UImode.TEACHPANEL:
+                id.fkine_and_set(q)
+                if self.canvases[fig_num].current_mode() == UImode.TEACHPANEL:
                     # Reload the joint sliders
-                    self.canvases[fig_num].teach_mode()
+                    self.canvases[fig_num].teach_mode(teach=True)
 
         # If DHRobot is given (or equivalent)
         else:
@@ -202,11 +201,11 @@ def step(self, dt=None, id=None, q=None, fig_num=0):
                 print("No robot found")
                 return
             # Set poses of graphical robot
-            poses = graphical_dh_robot.fkine(q)
-            graphical_dh_robot.set_joint_poses(poses)
-            if self.canvases[fig_num].current_mode == UImode.TEACHPANEL:
+            graphical_dh_robot.fkine_and_set(q)
+
+            if self.canvases[fig_num].current_mode() == UImode.TEACHPANEL:
                 # Reload the joint sliders
-                self.canvases[fig_num].teach_mode()
+                self.canvases[fig_num].teach_mode(teach=True)
 
         if dt is not None:
             sleep(dt)
diff --git a/roboticstoolbox/backends/VPython/canvas.py b/roboticstoolbox/backends/VPython/canvas.py
@@ -783,10 +783,7 @@ def __joint_slider(self, s):
             angles.append(self.__teachpanel_sliders[idx].value)
 
         # Run fkine
-        poses = self.__robots[self.__selected_robot].fkine(angles)
-
-        # Update joints
-        self.__robots[self.__selected_robot].set_joint_poses(poses)
+        self.__robots[self.__selected_robot].fkine_and_set(angles)
 
         for joint in self.__teachpanel[self.__selected_robot]:
             if joint[self.__idx_text] is None:
diff --git a/roboticstoolbox/backends/VPython/graphicalrobot.py b/roboticstoolbox/backends/VPython/graphicalrobot.py
@@ -786,7 +786,7 @@ def animate(self, frame_poses, fps):
             while t_stop - t_start < f:
                 t_stop = perf_counter()
 
-    def fkine(self, joint_angles):
+    def fkine_and_set(self, joint_angles):
         """
         Call fkine for the robot. If it is based on a seriallink object,
         run it's fkine function.
@@ -796,7 +796,13 @@ def fkine(self, joint_angles):
         """
         # If seriallink object, run it's fkine
         if self.robot is not None:
-            return self.robot.fkine_all(joint_angles)
+            poses = self.robot.fkine_all(joint_angles)
+            if joint_angles is None:
+                joint_angles = self.robot.q
+            for a_idx in range(len(joint_angles)):
+                # Ignore the base's angle (idx == 0)
+                self.angles[a_idx+1] = joint_angles[a_idx]
+            self.set_joint_poses(poses)
         # Else TODO
         else:
             pass
