Added example

jhavl · jhavl · commit 8ea18160b753 · 2021-02-15T14:37:40.000+10:00
diff --git a/examples/teach_swift.py b/examples/teach_swift.py
@@ -0,0 +1,52 @@
+# #!/usr/bin/env python
+# """
+# @author Jesse Haviland
+# """
+
+import roboticstoolbox as rtb
+import numpy as np
+
+# Launch the simulator Swift
+env = rtb.backends.Swift()
+env.launch()
+
+# Make a Panda robot and add it to Swift
+panda = rtb.models.Panda()
+panda.q = panda.qr
+env.add(panda)
+
+
+# This is our callback funciton from the sliders in Swift which set
+# the joint angles of our robot to the value of the sliders
+def set_joint(j, value):
+    panda.q[j] = np.deg2rad(float(value))
+
+
+# Loop through each link in the Panda and if it is a variable joint,
+# add a slider to Swift to control it
+j = 0
+for link in panda.links:
+    if link.isjoint:
+
+        # We use a lambda as the callback function from Swift
+        # j=j is used to set the value of j rather than the variable j
+        # We use the HTML unicode format for the degree sign in the unit arg
+        env.add_slider(
+            lambda x, j=j: set_joint(j, x),
+            min=np.round(np.rad2deg(link.qlim[0]), 2),
+            max=np.round(np.rad2deg(link.qlim[1]), 2),
+            step=1,
+            value=np.round(np.rad2deg(panda.q[j]), 2),
+            desc='Panda Joint ' + str(j),
+            unit='&#176;')
+
+        j += 1
+
+
+while(True):
+    # Process the event queue from Swift, this invokes the callback functions
+    # from the sliders if the slider value was changed
+    env.process_events()
+
+    # Update the environment with the new robot pose
+    env.step(0)
diff --git a/examples/test.py b/examples/test.py
@@ -15,55 +15,54 @@
 env = rtb.backends.Swift()
 env.launch()
 
-b1 = rtb.Box(
-    scale=[0.60, 1.1, 0.02],
-    base=sm.SE3(1.95, 0, 0.20))
-
-b2 = rtb.Box(
-    scale=[0.60, 1.1, 0.02],
-    base=sm.SE3(1.95, 0, 0.60))
-
-b3 = rtb.Box(
-    scale=[0.60, 1.1, 0.02],
-    base=sm.SE3(1.95, 0, 1.00))
-
-b4 = rtb.Box(
-    scale=[0.60, 1.1, 0.02],
-    base=sm.SE3(1.95, 0, 1.40))
-
-b5 = rtb.Box(
-    scale=[0.60, 0.02, 1.40],
-    base=sm.SE3(1.95, 0.55, 0.7))
-
-b6 = rtb.Box(
-    scale=[0.60, 0.02, 1.40],
-    base=sm.SE3(1.95, -0.55, 0.7))
-
-collisions = [b1, b2, b3, b4, b5, b6]
-
 path = pathlib.Path(path) / 'roboticstoolbox' / 'data'
 
 g1 = rtb.Mesh(
     filename=str(path / 'gimbal-ring1.stl'),
-    base=sm.SE3(0, 0, 2.0)
+    base=sm.SE3(0, 0, 1.3)
 )
-g1.v = [0, 0, 0, 0.4, 0, 0]
+# g1.v = [0, 0, 0, 0.4, 0, 0]
 
 g2 = rtb.Mesh(
     filename=str(path / 'gimbal-ring2.stl'),
-    base=sm.SE3(0, 0, 2.0)
+    base=sm.SE3(0, 0, 1.3) * sm.SE3.Rz(np.pi/2)
 )
-g2.v = [0, 0, 0, 0, 0.4, 0]
+g2.v = [0, 0, 0, 0.4, 0.0, 0]
 
 g3 = rtb.Mesh(
     filename=str(path / 'gimbal-ring3.stl'),
-    base=sm.SE3(0, 0, 2.0)
+    base=sm.SE3(0, 0, 1.3)
 )
-g3.v = [0, 0, 0, 0, 0, 0.4]
+g3.v = [0, 0, 0, 0.4, 0, 0]
 
 env.add(g1)
 env.add(g2)
 env.add(g3)
 
+
+panda = rtb.models.Panda()
+panda.q = panda.qr
+env.add(panda)
+
+j = 0
+
+
+def set_joint(j, x):
+    print(j)
+    panda.q[j] = x
+
+
+for link in panda.links:
+    if link.isjoint:
+
+        env.add_slider(
+            lambda x, j=j: set_joint(j, x),
+            min=link.qlim[0], max=link.qlim[1],
+            step=0.01, value=panda.q[j],
+            desc='Panda Joint ' + str(j))
+        
+        j += 1
+
 while(True):
+    env.process_events()
     env.step(0.05)
diff --git a/roboticstoolbox/backends/Swift/Swift.py b/roboticstoolbox/backends/Swift/Swift.py
@@ -424,6 +424,12 @@ def add_slider(
             ['slider', id, min, max, step, value, desc, unit])
 
     def process_events(self):
+        """
+        Process the event queue from Swift, this invokes the callback functions
+        from custom elements added to the page. If using custom elements
+        (for example `add_slider`), use this function in your event loop to
+        process updates from Swift.
+        """
         events = self._send_socket('check_elements')
         events = json.loads(events)
 
