Merge branch 'swift'

jhavl · jhavl · commit 88a1211d9768 · 2020-10-29T08:45:08.000+10:00
diff --git a/examples/mmc.py b/examples/mmc.py
@@ -8,6 +8,8 @@
 import numpy as np
 import qpsolvers as qp
 
+import time
+
 # Launch the simulator Swift
 env = rtb.backend.Swift()
 env.launch()
diff --git a/examples/swifty.py b/examples/swifty.py
@@ -0,0 +1,108 @@
+
+#!/usr/bin/env python
+"""
+@author Jesse Haviland
+"""
+
+import roboticstoolbox as rtb
+import spatialmath as sm
+import numpy as np
+import qpsolvers as qp
+
+# Launch the simulator Swift
+env = rtb.backend.Swift()
+env.launch()
+
+# Create a Panda robot object
+panda = rtb.models.Panda()
+
+# Set joint angles to ready configuration
+panda.q = panda.qr
+
+# Add the Panda to the simulator
+env.add(panda)
+
+
+while 1:
+    pass
+
+
+
+
+
+
+
+
+
+
+# import webbrowser as wb
+# import asyncio
+# import datetime
+# import random
+# import websockets
+# import threading
+# import time
+# from queue import Queue
+
+
+# class Socket:
+
+#     def __init__(self):
+#         print('Server Started')
+#         self.loop = asyncio.new_event_loop()
+#         asyncio.set_event_loop(self.loop)
+
+#         start_server = websockets.serve(self.serve, "localhost", 8997)
+
+#         self.loop.run_until_complete(start_server)
+#         self.loop.run_forever()
+
+#     async def serve(self, websocket, path):
+#         # while True:
+#             # message = await self.producer()
+#         await websocket.send('51111')
+#         self.loop.stop()
+
+#     # async def producer(self):
+#     #     data = self.q.get()
+#     #     # await asyncio.sleep(1)
+#     #     # now = datetime.datetime.utcnow().isoformat() + "Z"
+
+#     #     return data
+
+
+# # q = Queue()
+# # # x = threading.Thread(target=Socket, args=(q, ), daemon=True)
+# # # x.start()
+# Socket()
+
+
+# while 1:
+#     time.sleep(1)
+#     print('hello')
+#     # q.put('hi')
+
+
+# wb.open_new_tab('http://www.google.com')
+
+# def send(data):
+
+
+
+# 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
+#     time.sleep(1)
+
+    # env.step(50)
diff --git a/roboticstoolbox/backend/Swift/Swift.py b/roboticstoolbox/backend/Swift/Swift.py
@@ -3,14 +3,13 @@
 @author Jesse Haviland
 """
 
-import os
-from subprocess import call, Popen
 from roboticstoolbox.backend.Connector import Connector
-import zerorpc
 import roboticstoolbox as rp
 import numpy as np
 import spatialmath as sm
 import time
+from queue import Queue
+from swift import start_servers
 
 
 class Swift(Connector):  # pragma nocover
@@ -43,7 +42,10 @@ class Swift(Connector):  # pragma nocover
     def __init__(self):
         super(Swift, self).__init__()
 
-        # Popen(['npm', 'start', '--prefix', os.environ['SIM_ROOT']])
+        self.robots = []
+        self.shapes = []
+        self.outq = Queue()
+        self.inq = Queue()
 
     #
     #  Basic methods to do with the state of the external program
@@ -60,11 +62,7 @@ def launch(self):
 
         super().launch()
 
-        self.robots = []
-        self.shapes = []
-
-        self.swift = zerorpc.Client()
-        self.swift.connect("tcp://127.0.0.1:4242")
+        start_servers(self.outq, self.inq)
 
     def step(self, dt=50):
         """
@@ -81,9 +79,9 @@ def step(self, dt=50):
             - 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
 
         """
@@ -149,17 +147,18 @@ def add(self, ob, show_robot=True, show_collision=False):
         :return: object id within visualizer
         :rtype: int
 
-        ``id = env.add(robot)`` adds the ``robot`` to the graphical environment.
+        ``id = env.add(robot)`` adds the ``robot`` to the graphical
+            environment.
 
         .. note::
 
             - Adds the robot object to a list of robots which will be updated
               when the ``step()`` method is called.
 
         """
-        # id = add(robot) adds the robot to the external environment. robot must
-        # be of an appropriate class. This adds a robot object to a list of
-        # robots which will act upon the step() method being called.
+        # id = add(robot) adds the robot to the external environment. robot
+        # must be of an appropriate class. This adds a robot object to a
+        # list of robots which will act upon the step() method being called.
 
         # TODO can add more than a robot right?
 
@@ -169,26 +168,28 @@ def add(self, ob, show_robot=True, show_collision=False):
             robot = ob.to_dict()
             robot['show_robot'] = show_robot
             robot['show_collision'] = show_collision
-            id = self.swift.robot(robot)
+            id = self._send_socket('robot', robot)
 
-            loaded = False
-            while not loaded:
-                loaded = self.swift.is_loaded(id)
+            loaded = 0
+            while loaded == 0:
+                loaded = int(self._send_socket('is_loaded', id))
                 time.sleep(0.1)
 
             self.robots.append(ob)
             return id
-        elif isinstance(ob, rp.Shape):
-            shape = ob.to_dict()
-            id = self.swift.shape(shape)
-            self.shapes.append(ob)
-            return id
+        # elif isinstance(ob, rp.Shape):
+        #     shape = ob.to_dict()
+        #     id = self.swift.shape(shape)
+            # id = self._send_socket('shape', shape)
+        #     self.shapes.append(ob)
+        #     return id
 
     def remove(self):
         """
         Remove a robot to the graphical scene
 
-        ``env.remove(robot)`` removes the ``robot`` from the graphical environment.
+        ``env.remove(robot)`` removes the ``robot`` from the graphical
+            environment.
         """
 
         # TODO - shouldn't this have an id argument? which robot does it remove
@@ -239,13 +240,21 @@ def _draw_all(self):
 
         for i in range(len(self.robots)):
             self.robots[i].fkine_all()
-            self.swift.robot_poses([i, self.robots[i].fk_dict()])
+            # self.swift.robot_poses([i, self.robots[i].fk_dict()])
+            self._send_socket('robot_poses', [i, self.robots[i].fk_dict()])
 
         for i in range(len(self.shapes)):
-            self.swift.shape_poses([i, self.shapes[i].fk_dict()])
+            # self.swift.shape_poses([i, self.shapes[i].fk_dict()])
+            self._send_socket('shape_poses', [i, self.shapes[i].fk_dict()])
+
+    # def record_start(self, file):
+    #     self.swift.record_start(file)
+
+    # def record_stop(self):
+    #     self.swift.record_stop(1)
 
-    def record_start(self, file):
-        self.swift.record_start(file)
+    def _send_socket(self, code, data):
+        msg = [code, data]
 
-    def record_stop(self):
-        self.swift.record_stop(1)
+        self.outq.put(msg)
+        return self.inq.get()
diff --git a/roboticstoolbox/backend/__init__.py b/roboticstoolbox/backend/__init__.py
@@ -1,10 +1,10 @@
 from roboticstoolbox.backend.PyPlot import *
 from roboticstoolbox.backend.urdf import *
 
-try:
-    from roboticstoolbox.backend.Swift import *
-except ImportError:
-    pass
+# try:
+from roboticstoolbox.backend.Swift import *
+# except ImportError:
+#     pass
 
 try:
     from roboticstoolbox.backend.VPython import *
