add options setting mechanism via passed dicts

handle prismatic joints properly

Author: petercorke

roboticstoolbox/backends/PyPlot/PyPlot.py

@@ -123,8 +123,10 @@ def launch(self, name=None, fig=None, limits=None, **kwargs):
             self.ax.set_ylim3d([limits[2], limits[3]])
             self.ax.set_zlim3d([limits[4], limits[5]])
 
+        self.limits = limits
+
         # disable the display of value under cursor
-        self.ax.format_coord = lambda x, y: ''
+        # self.ax.format_coord = lambda x, y: ''
 
         # add time display in top-right corner
         self.timer = plt.figtext(0.85, 0.95, '')
@@ -235,7 +237,7 @@ def close(self):
 
     def add(
             self, ob, readonly=False, display=True,
-            jointaxes=True, jointlabels=False, eeframe=True, shadow=True, name=True):
+            jointaxes=True, jointlabels=False, eeframe=True, shadow=True, name=True, options=None):
         """
         Add a robot to the graphical scene
 
@@ -274,8 +276,8 @@ def add(
         if isinstance(ob, rp.DHRobot) or isinstance(ob, rp.ERobot):
             self.robots.append(
                 RobotPlot(
-                    ob, self.ax, readonly, display,
-                    jointaxes, jointlabels, eeframe, shadow, name))
+                    ob, self, readonly, display,
+                    jointaxes, jointlabels, eeframe, shadow, name, options))
             self.robots[len(self.robots) - 1].draw()
             id = len(self.robots)
 
@@ -454,8 +456,11 @@ def text_trans(text, q):  # pragma: no cover
 
         # Update the self state in mpl and the text
         def update(val, text, robot):  # pragma: no cover
-            for i in range(robot.n):
-                robot.q[i] = self.sjoint[i].val * np.pi/180
+            for j in range(robot.n):
+                if robot.isrevolute(j):
+                    robot.q[j] = np.radians(self.sjoint[j].val)
+                else:
+                    robot.q[j] = self.sjoint[j].val
             text_trans(text, robot.q)
 
         fig.subplots_adjust(left=0.25)
@@ -469,10 +474,7 @@ def update(val, text, robot):  # pragma: no cover
         self.axjoint = []
         self.sjoint = []
 
-        qlim = np.copy(robot.qlim) * 180/np.pi
-
-        qlim[0, :] = np.where(np.isnan(qlim[0, :]), -180, qlim[0, :])
-        qlim[1, :] = np.where(np.isnan(qlim[1, :]), 180, qlim[1, :])
+        qlim = robot.todegrees(robot.qlim)
 
         # if np.all(qlim == 0):     # pragma: no cover
         #     qlim[0, :] = -180
@@ -508,18 +510,24 @@ def update(val, text, robot):  # pragma: no cover
             0.02,  1 - ym + 0.06, "Joint angles",
             fontsize=9, weight="bold", color="#4f4f4f")
 
-        for i in range(robot.n):
-            ymin = (1 - ym) - i * yh
+        for j in range(robot.n):
+            ymin = (1 - ym) - j * yh
             ax = fig.add_axes([x1, ymin, x2, 0.03], facecolor='#dbdbdb')
             self.axjoint.append(ax)
 
-            self.sjoint.append(
-                Slider(
-                    ax, 'q' + str(i),
-                    qlim[0, i], qlim[1, i], robot.q[i] * 180/np.pi))
-
-            self.sjoint[i].on_changed(lambda x: update(x, text, robot))
-
+            if robot.isrevolute(j):
+                slider = Slider(
+                    ax, 'q' + str(j),
+                    qlim[0, j], qlim[1, j], np.degrees(q[j]), "% .1f°")
+            else:
+                slider = Slider(
+                    ax, 'q' + str(j),
+                    qlim[0, j], qlim[1, j], robot.q[j], "% .1f")
+
+            slider.on_changed(lambda x: update(x, text, robot))
+            self.sjoint.append(slider)
+        robot.q = q
+        self.step()
 
 def _isnotebook():
     """

roboticstoolbox/backends/PyPlot/PyPlot2.py

@@ -171,7 +171,7 @@ def add(
         if isinstance(ob, rp.ERobot2):
             self.robots.append(
                 RobotPlot2(
-                    ob, self.ax, readonly, display,
+                    ob, self, readonly, display,
                     eeframe, name))
             self.robots[len(self.robots) - 1].draw()
 
@@ -187,12 +187,23 @@ def remove(self):
 
         '''
 
-        super().remove()
+        super().remove() # ???
 
     def hold(self):           # pragma: no cover
+        '''
+        hold() keeps the plot open i.e. stops the plot from closing once
+        the main script has finished.
+
+        '''
+
         # signal.setitimer(signal.ITIMER_REAL, 0)
         plt.ioff()
-        plt.show()
+
+        # keep stepping the environment while figure is open
+        while True:
+            if not plt.fignum_exists(self.fig.number):
+                break
+            self.step()
 
     #
     #  Private methods
@@ -261,15 +272,11 @@ def text_trans(text, q):  # pragma: no cover
         def update(val, text, robot):  # pragma: no cover
             for j in range(robot.n):
                 if robot.isrevolute(j):
-                    robot.q[j] = self.sjoint[j].val * np.pi / 180
+                    robot.q[j] = np.radians(self.sjoint[j].val)
                 else:
                     robot.q[j] = self.sjoint[j].val
-
             text_trans(text, robot.q)
 
-            # Step the environment
-            self.step(0)
-
         fig.subplots_adjust(left=0.38)
         text = []
 
@@ -321,16 +328,14 @@ def update(val, text, robot):  # pragma: no cover
             if robot.isrevolute(j):
                 slider = Slider(
                     self.axjoint[j], 'q' + str(j),
-                    qlim[0, j], qlim[1, j], q[j] * 180/np.pi, "% .1f°")
+                    qlim[0, j], qlim[1, j], np.degrees(q[j]), "% .1f°")
             else:
                 slider = Slider(
                     self.axjoint[j], 'q' + str(j),
                     qlim[0, j], qlim[1, j], q[j], "% .1f")
 
             slider.on_changed(lambda x: update(x, text, robot))
             self.sjoint.append(slider)
-
-        
         robot.q = q
         self.step()
 

roboticstoolbox/backends/PyPlot/RobotPlot.py

@@ -12,8 +12,9 @@
 class RobotPlot():
 
     def __init__(
-            self, robot, ax, readonly, display=True,
-            jointaxes=True, jointlabels=False, eeframe=True, shadow=True, name=True):
+            self, robot, env, readonly, display=True,
+            jointaxes=True, jointlabels=False, eeframe=True, shadow=True,
+            name=True, options=None):
 
         super(RobotPlot, self).__init__()
 
@@ -25,7 +26,8 @@ def __init__(
         self.display = display
 
         self.robot = robot
-        self.ax = ax
+        self.env = env
+        self.ax = env.ax
 
         # Line plot of robot links
         self.links = None
@@ -52,6 +54,23 @@ def __init__(
         self.shadow = shadow
         self.showname = name
 
+        defaults = {
+            'robot': {'color': '#E16F6D', 'linewidth': 5},
+            'shadow': {'color': 'lightgrey', 'linewidth': 3},
+            'jointaxes': {'color': '#8FC1E2', 'linewidth': 2},
+            'jointlabels': {},
+            'jointaxislength': 0.2,
+            'eex': {'color': '#F84752', 'linewidth': 2}, # '#EE9494'
+            'eey': {'color': '#BADA55', 'linewidth': 2}, # '#93E7B0'
+            'eez': {'color': '#54AEFF', 'linewidth': 2},
+            'eelength': 0.06,
+        }
+
+        if options is not None:
+            for key, value in options.items():
+                defaults[key] = {**defaults[key], **options[key]}
+        self.options = defaults
+
     def draw(self):
         if not self.display:
             return
@@ -96,13 +115,10 @@ def draw(self):
 
         if self.eeframe:
             # Axes arrow transforms
-            Tjx = SE3([0.06, 0, 0])
-            Tjy = SE3([0, 0.06, 0])
-            Tjz = SE3([0, 0, 0.06])
-
-            red = '#F84752'  # '#EE9494'
-            green = '#BADA55'  # '#93E7B0'
-            blue = '#54AEFF'
+            len = self.options['eelength']
+            Tjx = SE3([len, 0, 0])
+            Tjy = SE3([0, len, 0])
+            Tjz = SE3([0, 0, len])
 
             # add new ee coordinate frame
             for link in self.robot.ee_links:
@@ -113,15 +129,15 @@ def draw(self):
                 Tey = Te * Tjy
                 Tez = Te * Tjz
 
-                xaxis = self._plot_quiver(Te.t, Tex.t, red, 2)
-                yaxis = self._plot_quiver(Te.t, Tey.t, green, 2)
-                zaxis = self._plot_quiver(Te.t, Tez.t, blue, 2)
+                xaxis = self._plot_quiver(Te.t, Tex.t, self.options['eex'])
+                yaxis = self._plot_quiver(Te.t, Tey.t, self.options['eey'])
+                zaxis = self._plot_quiver(Te.t, Tez.t, self.options['eez'])
 
                 self.eeframes.extend([xaxis, yaxis, zaxis])
 
         ## Joint axes
 
-        # remove oldjoint z coordinates
+        # remove old joint z axes
         if self.joints:
             for joint in self.joints:
                 joint.remove()
@@ -151,18 +167,19 @@ def draw(self):
                         direction = R[:, 0]  #  direction
 
                 if direction is not None:
-                    arrow = self._plot_quiver2(Tj.t, direction, 0.2, link.jindex, '#8FC1E2', 2)
+                    arrow = self._plot_quiver2(Tj.t, direction, link.jindex)
                     self.joints.extend(arrow)
 
 
     def init(self):
 
         self.drawn = True
 
-        limits = np.r_[-1, 1, -1, 1, -1, 1] * self.robot.reach * 1.5
-        self.ax.set_xlim3d([limits[0], limits[1]])
-        self.ax.set_ylim3d([limits[2], limits[3]])
-        self.ax.set_zlim3d([limits[4], limits[5]])
+        if self.env.limits is None:
+            limits = np.r_[-1, 1, -1, 1, -1, 1] * self.robot.reach * 1.5
+            self.ax.set_xlim3d([limits[0], limits[1]])
+            self.ax.set_ylim3d([limits[2], limits[3]])
+            self.ax.set_zlim3d([limits[4], limits[5]])
 
         self.segments = self.robot.segments()
 
@@ -179,45 +196,47 @@ def init(self):
         self.links = []
         self.sh_links = []
         for i in range(len(self.segments)):
-            line,  = self.ax.plot(
-                0, 0, 0, linewidth=5, color='#E16F6D')
-            self.links.append(line)
+
 
             # Plot the shadow of the robot links, draw first so robot is always
             # in front
             if self.shadow:
                 shadow, = self.ax.plot(
                     0, 0,
-                    linewidth=3, color='lightgrey')
+                    zorder=1, 
+                    **self.options['shadow'])
                 self.sh_links.append(shadow)
 
+            line,  = self.ax.plot(
+                0, 0, 0, **self.options['robot'])
+            self.links.append(line)
+
         self.eeframes = []
         self.joints = []
 
-    def _plot_quiver(self, p0, p1, col, width):
+    def _plot_quiver(self, p0, p1, options):
         qv = self.ax.quiver(
             p0[0], p0[1], p0[2],
             p1[0] - p0[0],
             p1[1] - p0[1],
             p1[2] - p0[2],
-            linewidth=width,
-            color=col
+            **options
         )
         return qv
 
-    def _plot_quiver2(self, p0, dir, len, j, col, width):
-        vec = dir * len
+    def _plot_quiver2(self, p0, dir, j):
+        vec = dir * self.options['jointaxislength']
         start = p0 - vec / 2
         qv = self.ax.quiver(
             start[0], start[1], start[2],
             vec[0], vec[1], vec[2],
-            linewidth=width,
-            color=col
+            zorder=5,
+            **self.options['jointaxes']
         )
 
         if self.jointlabels:
-            pl = p0 + vec * 0.6
-            label = self.ax.text(pl[0], pl[1], pl[2], f'q{j}')
+            pl = p0 - vec * 0.6
+            label = self.ax.text(pl[0], pl[1], pl[2], f'$q_{j}$', **self.options['jointlabels'] )
             return [qv, label]
         else:
             return [qv]