/
tumble_tower.py
154 lines (129 loc) · 4.79 KB
/
tumble_tower.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
from yarok import component, interface
@component(
tag='tumble-tower',
defaults={
'rows': 8
},
# language=xml
template="""
<mujoco>
<asset>
<material name="wood" rgba="0.5 0.4 0.3 1" specular="0.95"/>
</asset>
<default>
<default class='tt-block'>
<geom type='box'
material='wood'
mass='0.1'
size="0.04 0.12 0.028"/>
</default>
</default>
<worldbody>
<for each='range(rows)' as='z'>
<for each='range(3)' as='x'>
<body pos="
${0.5 + 0.082*x if z % 2 == 0 else 0.58}
${0.48 if z % 2 == 0 else 0.4 + 0.082*x}
${0.061*z}"
euler="0 0 ${0 if z % 2 == 0 else 1.57}">
<freejoint/>
<geom class='tt-block'/>
</body>
</for>
</for>
<!--
<body>
<geom class='tt-block-left'/>
<geom class='tt-block-center'/>
<geom class='tt-block-right'/>
</body>
<body euler='0 0 1.57' pos='0 0 0.061'>
<geom class='tt-block-left'/>
<geom class='tt-block-center'/>
<geom class='tt-block-right'/>
</body>
<body pos='0 0 0.121'>
<geom class='tt-block-left'/>
<geom class='tt-block-center'/>
<geom class='tt-block-right'/>
</body>
<body euler='0 0 1.57' pos='0 0 0.181'>
<geom class='tt-block-left'/>
<geom class='tt-block-center'/>
<geom class='tt-block-right'/>
</body>
<body pos='0 0 0.241'>
<geom class='tt-block-left'/>
<geom class='tt-block-center'/>
<geom class='tt-block-right'/>
</body>
<body euler='0 0 1.57' pos='0 0 0.301'>
<geom class='tt-block-left'/>
<geom class='tt-block-center'/>
<geom class='tt-block-right'/>
</body>
<body pos='0 0 0.361'>
<geom class='tt-block-left'/>
<geom class='tt-block-center'/>
<geom class='tt-block-right'/>
</body>
<body euler='0 0 1.57' pos='0 0 0.421'>
<geom class='tt-block-left'/>
<geom class='tt-block-center'/>
<geom class='tt-block-right'/>
</body> -->
</worldbody>
</mujoco>
"""
)
class TumbleTower:
pass
if __name__ == '__main__':
from yarok import Platform, Injector
from yarok.comm.worlds.empty_world import EmptyWorld
from yarok.comm.components.ur5e.ur5e import UR5e
from yarok.comm.components.robotiq_2f85.robotiq_2f85 import Robotiq2f85
from math import pi
@component(
extends=EmptyWorld,
components=[
TumbleTower,
UR5e,
Robotiq2f85
],
template="""
<mujoco>
<worldbody>
<tumble-tower name="tower"/>
<body euler="0 0 1.57">
<ur5e name='arm'>
<robotiq-2f85 name="gripper" parent="ee_link">
</robotiq-2f85>
</ur5e>
</body>
</worldbody>
</mujoco>
"""
)
class BlocksTowerTestWorld:
pass
class TumbleTheTowerBehaviour:
def __init__(self, injector: Injector):
self.arm: UR5e = injector.get('arm')
self.pl: Platform = injector.get(Platform)
def on_update(self):
self.pl.wait_seconds(10)
self.pl.wait(
self.arm.move_xyz(xyz=[0.8, 0.1, 0.12], xyz_angles=[3.11, -1.6e-7, -pi / 2])
)
self.pl.wait(
self.arm.move_xyz(xyz=[0.1, 0.8, 0.12], xyz_angles=[3.11, -1.6e-7, -pi / 2])
)
self.pl.wait_seconds(1000)
Platform.create({
'world': BlocksTowerTestWorld,
'behaviour': TumbleTheTowerBehaviour,
'defaults': {
'plugins': []
}
}).run()