|
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