|
3 | 3 | # @author Jesse Haviland
|
4 | 4 | # """
|
5 | 5 |
|
| 6 | +from math import pi |
6 | 7 | import roboticstoolbox as rtb
|
7 |
| -import spatialmath as sm |
| 8 | +from spatialmath import SO3, SE3 |
8 | 9 | import numpy as np
|
9 | 10 | import pathlib
|
10 | 11 | import os
|
| 12 | +import time |
11 | 13 |
|
12 | 14 | path = os.path.realpath('.')
|
13 | 15 |
|
14 | 16 | # Launch the simulator Swift
|
15 | 17 | env = rtb.backends.Swift()
|
16 | 18 | env.launch()
|
17 | 19 |
|
18 |
| -path = pathlib.Path(path) / 'roboticstoolbox' / 'data' |
| 20 | +path = pathlib.Path(path) / 'rtb-data' / 'rtbdata' / 'data' / 'test-mesh' |
19 | 21 |
|
20 |
| -g1 = rtb.Mesh( |
21 |
| - filename=str(path / 'gimbal-ring1.stl'), |
22 |
| - base=sm.SE3(0, 0, 1.3) |
23 |
| -) |
24 |
| -# g1.v = [0, 0, 0, 0.4, 0, 0] |
25 | 22 |
|
26 |
| -g2 = rtb.Mesh( |
27 |
| - filename=str(path / 'gimbal-ring2.stl'), |
28 |
| - base=sm.SE3(0, 0, 1.3) * sm.SE3.Rz(np.pi/2) |
29 |
| -) |
30 |
| -g2.v = [0, 0, 0, 0.4, 0.0, 0] |
| 23 | +# obj_bug = rtb.Mesh( |
| 24 | +# filename=str(path / 'scene.gltf'), |
| 25 | +# scale=[0.001, 0.001, 0.001], |
| 26 | +# base=SE3(0, 0, 0.45) * SE3.Rx(np.pi/2) * SE3.Ry(np.pi/2) |
| 27 | +# ) |
31 | 28 |
|
32 |
| -g3 = rtb.Mesh( |
33 |
| - filename=str(path / 'gimbal-ring3.stl'), |
34 |
| - base=sm.SE3(0, 0, 1.3) |
| 29 | +dae = rtb.Mesh( |
| 30 | + filename=str(path / 'walle.dae'), |
| 31 | + base=SE3(0, -1.5, 0) |
35 | 32 | )
|
36 |
| -g3.v = [0, 0, 0, 0.4, 0, 0] |
37 |
| - |
38 |
| -env.add(g1) |
39 |
| -env.add(g2) |
40 |
| -env.add(g3) |
41 | 33 |
|
| 34 | +obj = rtb.Mesh( |
| 35 | + filename=str(path / 'walle.obj'), |
| 36 | + base=SE3(0, -0.5, 0) |
| 37 | +) |
42 | 38 |
|
43 |
| -panda = rtb.models.Panda() |
44 |
| -panda.q = panda.qr |
45 |
| -env.add(panda) |
| 39 | +glb = rtb.Mesh( |
| 40 | + filename=str(path / 'walle.glb'), |
| 41 | + base=SE3(0, 0.5, 0) * SE3.Rz(-np.pi/2) |
| 42 | +) |
46 | 43 |
|
47 |
| -j = 0 |
| 44 | +ply = rtb.Mesh( |
| 45 | + filename=str(path / 'walle.ply'), |
| 46 | + base=SE3(0, 1.5, 0) |
| 47 | +) |
48 | 48 |
|
| 49 | +# wrl = rtb.Mesh( |
| 50 | +# filename=str(path / 'walle.wrl'), |
| 51 | +# base=SE3(0, 2.0, 0) |
| 52 | +# ) |
49 | 53 |
|
50 |
| -def set_joint(j, x): |
51 |
| - print(j) |
52 |
| - panda.q[j] = x |
| 54 | +pcd = rtb.Mesh( |
| 55 | + filename=str(path / 'pcd.pcd'), |
| 56 | + base=SE3(0, 0, 1.5) * SE3.Rx(np.pi/2) * SE3.Ry(np.pi/2) |
| 57 | +) |
53 | 58 |
|
54 | 59 |
|
55 |
| -for link in panda.links: |
56 |
| - if link.isjoint: |
| 60 | +env.add(dae) |
| 61 | +env.add(obj) |
| 62 | +env.add(glb) |
| 63 | +env.add(ply) |
| 64 | +env.add(pcd) |
57 | 65 |
|
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 |
| 66 | +time.sleep(2) |
65 | 67 |
|
66 | 68 | while(True):
|
67 |
| - env.process_events() |
68 |
| - env.step(0.05) |
| 69 | + # env.process_events() |
| 70 | + env.step(0) |
0 commit comments