Merge branch 'master' of https://github.com/petercorke/robotics-toolbox-python

petercorke · petercorke · commit 906f0e07b9a8 · 2021-04-02T14:23:27.000+10:00
diff --git a/roboticstoolbox/backends/VPython/VPython.py b/roboticstoolbox/backends/VPython/VPython.py
@@ -19,7 +19,7 @@
 close_localhost_session = None
 
 try:
-    from roboticstoolbox.backends.VPython.canvas import GraphicsCanvas2D, GraphicsCanvas3D
+    from roboticstoolbox.backends.VPython.canvas import GraphicsCanvas2D, GraphicsCanvas3D, UImode
     from roboticstoolbox.backends.VPython.graphicalrobot import GraphicalRobot
     from roboticstoolbox.backends.VPython.grid import GridType
 except ImportError:
@@ -171,16 +171,18 @@ 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)
+                id.fkine_and_set(q)
+                if self.canvases[fig_num].current_mode() == UImode.TEACHPANEL:
+                    # Reload the joint sliders
+                    self.canvases[fig_num].teach_mode(teach=True)
 
         # If DHRobot is given (or equivalent)
         else:
-            grpahical_dh_robot = None
+            graphical_dh_robot = None
             # If no ID given, and there are robots available
             if id is None and len(self.robots) > 0:
                 # Obtain the first one
-                grpahical_dh_robot = self.robots[0]
+                graphical_dh_robot = self.robots[0]
             # If no ID, and no robots available
             elif id is None:
                 print("No robot found")
@@ -191,16 +193,19 @@ def step(self, dt=None, id=None, q=None, fig_num=0):
                     if self.robots[i].robot is id and \
                             self.canvases[fig_num].is_robot_in_canvas(
                                                             self.robots[i]):
-                        grpahical_dh_robot = self.robots[i]
+                        graphical_dh_robot = self.robots[i]
                         break
 
             # If no graphical equivalent found, return
-            if grpahical_dh_robot is None:
+            if graphical_dh_robot is None:
                 print("No robot found")
                 return
             # Set poses of graphical robot
-            poses = grpahical_dh_robot.fkine(q)
-            grpahical_dh_robot.set_joint_poses(poses)
+            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(teach=True)
 
         if dt is not None:
             sleep(dt)
diff --git a/roboticstoolbox/backends/VPython/canvas.py b/roboticstoolbox/backends/VPython/canvas.py
@@ -222,8 +222,9 @@ def add_robot(self, robot):
         self.__reload_caption(new_list)
 
         # Set it as selected
-        self.__ui_controls.get('menu_robots').index = \
-            len(self.__robots) - 1
+        if self.__ui_mode == UImode.CANVASCONTROL:
+            self.__ui_controls.get('menu_robots').index = \
+                len(self.__robots) - 1
 
         # Place camera based on robots effective radius * 1.25
         if robot.robot is not None:
@@ -285,6 +286,9 @@ def set_grid_mode(self, mode):
         """
         self.__graphics_grid.set_mode(mode)
 
+    def current_mode(self):
+        return self.__ui_mode
+
     #######################################
     #  UI Management
     #######################################
@@ -605,6 +609,11 @@ def __setup_joint_sliders(self):
         if len(self.__teachpanel) == 0:
             self.scene.append_to_caption("No robots available\n")
             return
+
+        # Update the robots to their current joint angles
+        for joint_idx, joint in enumerate(self.__teachpanel[self.__selected_robot]):
+            joint[self.__idx_theta] = self.__robots[self.__selected_robot].angles[joint_idx]
+
         i = 0
         for joint in self.__teachpanel[self.__selected_robot]:
             if joint[self.__idx_qlim_min] == joint[self.__idx_qlim_max]:
@@ -774,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
@@ -789,7 +789,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.
@@ -799,7 +799,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
