Skip to content

Commit 88f073d

Browse files
jhavljhavl
committed
new things
1 parent 3a40b78 commit 88f073d

File tree

679 files changed

+143746
-39
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

679 files changed

+143746
-39
lines changed

‎examples/mmc.py

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,8 @@
88
import numpy as np
99
import qpsolvers as qp
1010

11+
import time
12+
1113
# Launch the simulator Swift
1214
env = rtb.backend.Swift()
1315
env.launch()

‎examples/swifty.py

Lines changed: 101 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,21 +1,108 @@
1-
import webbrowser as wb
2-
import asyncio
3-
import datetime
4-
import random
5-
import websockets
61

7-
async def time(websocket, path):
8-
# while True:
9-
now = datetime.datetime.utcnow().isoformat() + "Z"
10-
await websocket.send(now)
11-
await asyncio.sleep(random.random() * 3)
2+
#!/usr/bin/env python
3+
"""
4+
@author Jesse Haviland
5+
"""
126

13-
start_server = websockets.serve(time, "127.0.0.1", 5678)
7+
import roboticstoolbox as rtb
8+
import spatialmath as sm
9+
import numpy as np
10+
import qpsolvers as qp
1411

15-
asyncio.get_event_loop().run_until_complete(start_server)
16-
asyncio.get_event_loop().run_forever()
12+
# Launch the simulator Swift
13+
env = rtb.backend.Swift()
14+
env.launch()
1715

16+
# Create a Panda robot object
17+
panda = rtb.models.Panda()
1818

19+
# Set joint angles to ready configuration
20+
panda.q = panda.qr
1921

22+
# Add the Panda to the simulator
23+
env.add(panda)
2024

21-
wb.open_new_tab('http://www.google.com')
25+
26+
while 1:
27+
pass
28+
29+
30+
31+
32+
33+
34+
35+
36+
37+
38+
# import webbrowser as wb
39+
# import asyncio
40+
# import datetime
41+
# import random
42+
# import websockets
43+
# import threading
44+
# import time
45+
# from queue import Queue
46+
47+
48+
# class Socket:
49+
50+
# def __init__(self):
51+
# print('Server Started')
52+
# self.loop = asyncio.new_event_loop()
53+
# asyncio.set_event_loop(self.loop)
54+
55+
# start_server = websockets.serve(self.serve, "localhost", 8997)
56+
57+
# self.loop.run_until_complete(start_server)
58+
# self.loop.run_forever()
59+
60+
# async def serve(self, websocket, path):
61+
# # while True:
62+
# # message = await self.producer()
63+
# await websocket.send('51111')
64+
# self.loop.stop()
65+
66+
# # async def producer(self):
67+
# # data = self.q.get()
68+
# # # await asyncio.sleep(1)
69+
# # # now = datetime.datetime.utcnow().isoformat() + "Z"
70+
71+
# # return data
72+
73+
74+
# # q = Queue()
75+
# # # x = threading.Thread(target=Socket, args=(q, ), daemon=True)
76+
# # # x.start()
77+
# Socket()
78+
79+
80+
# while 1:
81+
# time.sleep(1)
82+
# print('hello')
83+
# # q.put('hi')
84+
85+
86+
# wb.open_new_tab('http://www.google.com')
87+
88+
# def send(data):
89+
90+
91+
92+
# panda = rp.models.Panda()
93+
# panda.q = panda.qr
94+
95+
# Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
96+
97+
# arrived = False
98+
# # env.add(panda)
99+
100+
# dt = 0.05
101+
102+
# while not arrived:
103+
104+
# v, arrived = rp.p_servo(panda.fkine(), Tep, 1)
105+
# panda.qd = np.linalg.pinv(panda.jacobe()) @ v
106+
# time.sleep(1)
107+
108+
# env.step(50)

‎roboticstoolbox/backend/Swift/Swift.py

Lines changed: 162 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -3,14 +3,22 @@
33
@author Jesse Haviland
44
"""
55

6-
import os
76
from subprocess import call, Popen
87
from roboticstoolbox.backend.Connector import Connector
9-
import zerorpc
108
import roboticstoolbox as rp
119
import numpy as np
1210
import spatialmath as sm
1311
import time
12+
import websockets
13+
import asyncio
14+
from threading import Thread
15+
from queue import Queue, Empty
16+
import webbrowser as wb
17+
import json
18+
import http.server
19+
import socketserver
20+
from pathlib import Path
21+
import os
1422

1523

1624
class Swift(Connector): # pragma nocover
@@ -43,7 +51,10 @@ class Swift(Connector): # pragma nocover
4351
def __init__(self):
4452
super(Swift, self).__init__()
4553

46-
# Popen(['npm', 'start', '--prefix', os.environ['SIM_ROOT']])
54+
self.robots = []
55+
self.shapes = []
56+
self.outq = Queue()
57+
self.inq = Queue()
4758

4859
#
4960
# Basic methods to do with the state of the external program
@@ -60,11 +71,40 @@ def launch(self):
6071

6172
super().launch()
6273

63-
self.robots = []
64-
self.shapes = []
65-
66-
self.swift = zerorpc.Client()
67-
self.swift.connect("tcp://127.0.0.1:4242")
74+
# Start a http server
75+
self.server = Thread(
76+
target=Server, args=(self.inq, ), daemon=True)
77+
self.server.start()
78+
http_port = self.inq.get()
79+
80+
# Start our websocket server with a new clean port
81+
self.socket = Thread(
82+
target=Socket, args=(self.outq, self.inq, ), daemon=True)
83+
self.socket.start()
84+
port = self.inq.get()
85+
86+
# Launch the simulator
87+
wb.open_new_tab('http://localhost:' + str(http_port))
88+
# wb.open_new_tab('file:///home/jesse/swift/public/index.html')
89+
90+
# Let swift know which port to talk on using the common port
91+
loop = asyncio.new_event_loop()
92+
93+
async def send_port(websocket, path):
94+
await websocket.send(str(port))
95+
await websocket.wait_closed()
96+
loop.stop()
97+
98+
asyncio.set_event_loop(loop)
99+
port_ws = websockets.serve(send_port, "localhost", 8997)
100+
loop.run_until_complete(port_ws)
101+
loop.run_forever()
102+
103+
try:
104+
self.inq.get(timeout=10)
105+
except Empty:
106+
print('\nCould not connect to the Swift simulator \n')
107+
raise
68108

69109
def step(self, dt=50):
70110
"""
@@ -81,9 +121,9 @@ def step(self, dt=50):
81121
- Each robot in the scene is updated based on
82122
their control type (position, velocity, acceleration, or torque).
83123
- Upon acting, the other three of the four control types will be
84-
updated in the internal state of the robot object.
85-
- The control type is defined by the robot object, and not all robot
86-
objects support all control types.
124+
updated in the internal state of the robot object.
125+
- The control type is defined by the robot object, and not all
126+
robot objects support all control types.
87127
- Execution is blocked for the specified interval
88128
89129
"""
@@ -149,7 +189,8 @@ def add(self, ob, show_robot=True, show_collision=False):
149189
:return: object id within visualizer
150190
:rtype: int
151191
152-
``id = env.add(robot)`` adds the ``robot`` to the graphical environment.
192+
``id = env.add(robot)`` adds the ``robot`` to the graphical
193+
environment.
153194
154195
.. note::
155196
@@ -169,20 +210,21 @@ def add(self, ob, show_robot=True, show_collision=False):
169210
robot = ob.to_dict()
170211
robot['show_robot'] = show_robot
171212
robot['show_collision'] = show_collision
172-
id = self.swift.robot(robot)
213+
id = self._send_socket('robot', robot)
173214

174215
loaded = False
175216
while not loaded:
176-
loaded = self.swift.is_loaded(id)
217+
loaded = self._send_socket('is_loaded', id)
177218
time.sleep(0.1)
178219

179220
self.robots.append(ob)
180221
return id
181-
elif isinstance(ob, rp.Shape):
182-
shape = ob.to_dict()
183-
id = self.swift.shape(shape)
184-
self.shapes.append(ob)
185-
return id
222+
# elif isinstance(ob, rp.Shape):
223+
# shape = ob.to_dict()
224+
# id = self.swift.shape(shape)
225+
# id = self._send_socket('shape', shape)
226+
# self.shapes.append(ob)
227+
# return id
186228

187229
def remove(self):
188230
"""
@@ -239,13 +281,108 @@ def _draw_all(self):
239281

240282
for i in range(len(self.robots)):
241283
self.robots[i].fkine_all()
242-
self.swift.robot_poses([i, self.robots[i].fk_dict()])
284+
# self.swift.robot_poses([i, self.robots[i].fk_dict()])
285+
self._send_socket('robot_poses', [i, self.robots[i].fk_dict()])
243286

244287
for i in range(len(self.shapes)):
245-
self.swift.shape_poses([i, self.shapes[i].fk_dict()])
288+
# self.swift.shape_poses([i, self.shapes[i].fk_dict()])
289+
self._send_socket('shape_poses', [i, self.shapes[i].fk_dict()])
290+
291+
# def record_start(self, file):
292+
# self.swift.record_start(file)
293+
294+
# def record_stop(self):
295+
# self.swift.record_stop(1)
296+
297+
def _send_socket(self, code, data):
298+
msg = [code, data]
299+
300+
self.outq.put(msg)
301+
return self.inq.get()
302+
303+
304+
class Socket:
305+
306+
def __init__(self, outq, inq):
307+
self.outq = outq
308+
self.inq = inq
309+
self.USERS = set()
310+
loop = asyncio.new_event_loop()
311+
asyncio.set_event_loop(loop)
312+
313+
started = False
314+
port = 51478
315+
316+
while not started and port < 62000:
317+
try:
318+
port += 1
319+
start_server = websockets.serve(self.serve, "localhost", port)
320+
loop.run_until_complete(start_server)
321+
started = True
322+
except OSError:
323+
pass
324+
325+
self.inq.put(port)
326+
loop.run_forever()
327+
328+
async def register(self, websocket):
329+
self.USERS.add(websocket)
330+
331+
async def serve(self, websocket, path):
332+
333+
# Initial connection handshake
334+
await(self.register(websocket))
335+
recieved = await websocket.recv()
336+
self.inq.put(recieved)
337+
338+
# Now onto send, recieve cycle
339+
while True:
340+
message = await self.producer()
341+
await websocket.send(json.dumps(message))
342+
343+
recieved = await websocket.recv()
344+
self.inq.put(recieved)
345+
print(recieved)
346+
347+
async def producer(self):
348+
data = self.outq.get()
349+
return data
350+
351+
352+
class Server:
353+
354+
def __init__(self, inq):
355+
356+
PORT = 52000
357+
self.inq = inq
358+
359+
root_dir = Path(rp.__file__).parent / 'public'
360+
os.chdir(Path.home())
361+
362+
class MyHttpRequestHandler(http.server.SimpleHTTPRequestHandler):
363+
def do_GET(self):
364+
365+
home = str(Path.home())
366+
367+
if self.path == '/':
368+
self.path = str(root_dir / 'index.html')
369+
elif self.path.endswith('css') or self.path.endswith('js'):
370+
self.path = str(root_dir) + self.path
371+
372+
if self.path.startswith(home):
373+
self.path = self.path[len(home):]
374+
375+
return http.server.SimpleHTTPRequestHandler.do_GET(self)
376+
377+
Handler = MyHttpRequestHandler
246378

247-
def record_start(self, file):
248-
self.swift.record_start(file)
379+
connected = False
249380

250-
def record_stop(self):
251-
self.swift.record_stop(1)
381+
while not connected and PORT < 62000:
382+
try:
383+
with socketserver.TCPServer(("", PORT), Handler) as httpd:
384+
self.inq.put(PORT)
385+
connected = True
386+
httpd.serve_forever()
387+
except OSError:
388+
PORT += 1

0 commit comments

Comments
 (0)