8000 Added example · Global19/robotics-toolbox-python@8ea1816 · GitHub
[go: up one dir, main page]
Skip to content

Commit 8ea1816

Browse files
jhavljhavl
committed
Added example
1 parent 7ba50d4 commit 8ea1816

File tree

3 files changed

+89
-32
lines changed

3 files changed

+89
-32
lines changed

‎examples/teach_swift.py

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
# #!/usr/bin/env python
2+
# """
3+
# @author Jesse Haviland
4+
# """
5+
6+
import roboticstoolbox as rtb
7+
import numpy as np
8+
9+
# Launch the simulator Swift
10+
env = rtb.backends.Swift()
11+
env.launch()
12+
13+
# Make a Panda robot and add it to Swift
14+
panda = rtb.models.Panda()
15+
panda.q = panda.qr
16+
env.add(panda)
17+
18+
19+
# This is our callback funciton from the sliders in Swift which set
20+
# the joint angles of our robot to the value of the sliders
21+
def set_joint(j, value):
22+
panda.q[j] = np.deg2rad(float(value))
23+
24+
25+
# Loop through each link in the Panda and if it is a variable joint,
26+
# add a slider to Swift to control it
27+
j = 0
28+
for link in panda.links:
29+
if link.isjoint:
30+
31+
# We use a lambda as the callback function from Swift
32+
# j=j is used to set the value of j rather than the variable j
33+
# We use the HTML unicode format for the degree sign in the unit arg
34+
env.add_slider(
35+
lambda x, j=j: set_joint(j, x),
36+
min=np.round(np.rad2deg(link.qlim[0]), 2),
37+
max=np.round(np.rad2deg(link.qlim[1]), 2),
38+
step=1,
39+
value=np.round(np.rad2deg(panda.q[j]), 2),
40+
desc='Panda Joint ' + str(j),
41+
unit='°')
42+
43+
j += 1
44+
45+
46+
while(True):
47+
# Process the event queue from Swift, this invokes the callback functions
48+
# from the sliders if the slider value was changed
49+
env.process_events()
50+
51+
# Update the environment with the new robot pose
52+
env.step(0)

‎examples/test.py

Lines changed: 31 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -15,55 +15,54 @@
1515
env = rtb.backends.Swift()
1616
env.launch()
1717

18-
b1 = rtb.Box(
19-
scale=[0.60, 1.1, 0.02],
20-
base=sm.SE3(1.95, 0, 0.20))
21-
22-
b2 = rtb.Box(
23-
scale=[0.60, 1.1, 0.02],
24-
base=sm.SE3(1.95, 0, 0.60))
25-
26-
b3 = rtb.Box(
27-
scale=[0.60, 1.1, 0.02],
28-
base=sm.SE3(1.95, 0, 1.00))
29-
30-
b4 = rtb.Box(
31-
scale=[0.60, 1.1, 0.02],
32-
base=sm.SE3(1.95, 0, 1.40))
33-
34-
b5 = rtb.Box(
35-
scale=[0.60, 0.02, 1.40],
36-
base=sm.SE3(1.95, 0.55, 0.7))
37-
38-
b6 = rtb.Box(
39-
scale=[0.60, 0.02, 1.40],
40-
base=sm.SE3(1.95, -0.55, 0.7))
41-
42-
collisions = [b1, b2, b3, b4, b5, b6]
43-
4418
path = pathlib.Path(path) / 'roboticstoolbox' / 'data'
4519

4620
g1 = rtb.Mesh(
4721
filename=str(path / 'gimbal-ring1.stl'),
48-
base=sm.SE3(0, 0, 2.0)
22+
base=sm.SE3(0, 0, 1.3)
4923
)
50-
g1.v = [0, 0, 0, 0.4, 0, 0]
24+
# g1.v = [0, 0, 0, 0.4, 0, 0]
5125

5226
g2 = rtb.Mesh(
5327
filename=str(path / 'gimbal-ring2.stl'),
54-
base=sm.SE3(0, 0, 2.0)
28+
base=sm.SE3(0, 0, 1.3) * sm.SE3.Rz(np.pi/2)
5529
)
56-
g2.v = [0, 0, 0, 0, 0.4, 0]
30+
g2.v = [0, 0, 0, 0.4, 0.0, 0]
5731

5832
g3 = rtb.Mesh(
5933
filename=str(path / 'gimbal-ring3.stl'),
60-
base=sm.SE3(0, 0, 2.0)
34+
base=sm.SE3(0, 0, 1.3)
6135
)
62-
g3.v = [0, 0, 0, 0, 0, 0.4]
36+
g3.v = [0, 0, 0, 0.4, 0, 0]
6337

6438
env.add(g1)
6539
env.add(g2)
6640
env.add(g3)
6741

42+
43+
panda = rtb.models.Panda()
44+
panda.q = panda.qr
45+
env.add(panda)
46+
47+
j = 0
48+
49+
50+
def set_joint(j, x):
51+
print(j)
52+
panda.q[j] = x
53+
54+
55+
for link in panda.links:
56+
if link.isjoint:
57+
58+
env.add_slider(
59+
lambda x, j=j: set_joint(j, x),
60+
min=link.qlim[0], max=link.qlim[1],
61+
step=0.01, value=panda.q[j],
62+
desc='Panda Joint ' + str(j))
63+
64+
j += 1
65+
6866
while(True):
67+
env.process_events()
6968
env.step(0.05)

‎roboticstoolbox/backends/Swift/Swift.py

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -424,6 +424,12 @@ def add_slider(
424424
['slider', id, min, max, step, value, desc, unit])
425425

426426
def process_events(self):
427+
"""
428+
Process the event queue from Swift, this invokes the callback functions
429+
from custom elements added to the page. If using custom elements
430+
(for example `add_slider`), use this function in your event loop to
431+
process updates from Swift.
432+
"""
427433
events = self._send_socket('check_elements')
428434
events = json.loads(events)
429435

0 commit comments

Comments
 (0)
0