NickNgHK
diff --git a/‎examples/RRMC_swift.py
Lines changed: 27 additions & 0 deletions b/‎examples/RRMC_swift.py
Lines changed: 27 additions & 0 deletions
diff --git a/‎roboticstoolbox/backends/PyPlot/PyPlot.py
Lines changed: 18 additions & 17 deletions b/‎roboticstoolbox/backends/PyPlot/PyPlot.py
Lines changed: 18 additions & 17 deletions
@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+"""
+@author Jesse Haviland
+"""
+
+import roboticstoolbox as rp
+import spatialmath as sm
+import numpy as np
+
+env = rp.backends.Swift()
+env.launch()
+
+panda = rp.models.Panda()
+panda.q = panda.qr
+
+Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
+
+arrived = False
+env.add(panda)
+
+dt = 0.05
+
+while not arrived:
+
+    v, arrived = rp.p_servo(panda.fkine(), Tep, 1)
+    panda.qd = np.linalg.pinv(panda.jacobe()) @ v
+    env.step(dt)
@@ -8,7 +8,6 @@
 from roboticstoolbox.backends.Connector import Connector
 import matplotlib
 import matplotlib.pyplot as plt
-import signal
 from roboticstoolbox.backends.PyPlot.RobotPlot import RobotPlot
 from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot
 from spatialmath.base.argcheck import getvector
@@ -30,7 +29,7 @@ class PyPlot(Connector):
     Graphical backend using matplotlib
 
     matplotlib is a common and highly portable graphics library for Python,
-    but has relatively limited 3D capability.  
+    but has relatively limited 3D capability.
 
     Example:
 
@@ -48,7 +47,7 @@ class PyPlot(Connector):
 
     .. note::  PyPlot is the default backend, and ``robot.plot(q)`` effectively
         performs lines 7-8 above.
-        
+
     """
 
     def __init__(self):
@@ -75,9 +74,9 @@ def launch(self, name=None, limits=None):
         labels = ['X', 'Y', 'Z']
 
         if name is not None:
-            self.fig = plt.figure()
+            self.fig = plt.figure(name)
         else:
-            self.fig = plt.figure()
+            self.fig = plt.figure('Robotics Toolbox for Python')
 
         self.fig.subplots_adjust(left=-0.09, bottom=0, top=1, right=0.99)
 
@@ -113,18 +112,18 @@ def step(self, dt=50):
 
         :param dt: time step in milliseconds, defaults to 50
         :type dt: int, optional
- 
+
         ``env.step(args)`` triggers an update of the 3D scene in the matplotlib
         window referenced by ``env``.
 
-        .. note:: 
+        .. note::
 
             - Each robot in the scene is updated based on
               their control type (position, velocity, acceleration, or torque).
             - Upon acting, the other three of the four control types will be
-              updated in the internal state of the robot object. 
-            - The control type is defined by the robot object, and not all robot
-              objects support all control types.
+              updated in the internal state of the robot object.
+            - The control type is defined by the robot object, and not all
+              robot objects support all control types.
             - Execution is blocked for the specified interval
 
         """
@@ -160,8 +159,8 @@ def restart(self):
         Restart the graphics display
 
         ``env.restart()`` triggers a restart of the matplotlib view referenced
-        by ``env``. It is closed and relaunched to the original state defined by
-        ``launch()``.
+        by ``env``. It is closed and relaunched to the original state defined
+        by ``launch()``.
 
         """
         # TODO what does this actually do for matplotlib??
@@ -205,11 +204,12 @@ def add(
         :param name: [description], defaults to True
         :type name: bool, optional
 
-        ``id = env.add(robot)`` adds the ``robot`` to the graphical environment.
+        ``id = env.add(robot)`` adds the ``robot`` to the graphical
+            environment.
 
         .. note::
 
-            - ``robot`` must be of an appropriate class. 
+            - ``robot`` must be of an appropriate class.
             - Adds the robot object to a list of robots which will be updated
               when the ``step()`` method is called.
 
@@ -239,18 +239,19 @@ def remove(self):
         """
         Remove a robot to the graphical scene
 
-        :param id: The id of the robot to remove. Can be either the DHLink or 
+        :param id: The id of the robot to remove. Can be either the DHLink or
             GraphicalRobot
         :type id: class:`~roboticstoolbox.robot.DHRobot.DHRobot`,
                   class:`roboticstoolbox.backends.VPython.graphics_robot.GraphicalRobot`
         :param fig_num: The canvas index to delete the robot from, defaults to
              the initial one
         :type fig_num: int, optional
-        :raises ValueError: Figure number must be between 0 and total number 
+        :raises ValueError: Figure number must be between 0 and total number
             of canvases
         :raises TypeError: Input must be a DHLink or GraphicalRobot
 
-        ``env.remove(robot)`` removes the ``robot`` from the graphical environment.
+        ``env.remove(robot)`` removes the ``robot`` from the graphical
+            environment.
         """
         # TODO should be an id to remove?