-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtraining_grounds.py
102 lines (79 loc) · 2.42 KB
/
training_grounds.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
import pybullet as p
import pybullet_data
from goodbot import Goodbot
from threading import Timer
from road import Road
""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
phyCl = p.connect(p.GUI) # SETTING UP THE ENVIRONMENT
p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane = p.loadURDF("plane.urdf") # load the plane
p.setGravity(0, 0, -10) # set gravity
p.setRealTimeSimulation(1)
""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
#road_file = open('blender_road.txt', 'r')
#file_coords = road_file.readlines()
#idx = index.Index()
#for i in file_coords:
# coords = i.split(' ')
# p.loadURDF("road_point.urdf", [float(coords[0]), float(coords[1]), 0.1] )
""" a road point is 0.3x0.3 coordinates big"""
#p.loadURDF("road_point.urdf", [0, 0, 0.1] )
#p.loadURDF("road_point.urdf", [0.3, 0.3, 0.1] )
p.resetDebugVisualizerCamera(25, 0, -89.9, [0, 0, 0])
road = Road('road/blender_road.txt')
robot = Goodbot("goodbot/goodbot.urdf", road.fetchStart() + [0.3])
p.setRealTimeSimulation(1)
rarr = p.B3G_RIGHT_ARROW
larr = p.B3G_LEFT_ARROW
uarr = p.B3G_UP_ARROW
darr = p.B3G_DOWN_ARROW
velocity = 30
force = 100
p.setGravity(0, 0, -30)
""" output variables """
save_length = 0
outputs_size = 22
write_list = []
file = open('data/training_data.txt', 'a')
""""""""""""""""""""""""
iteration = 0
WRITE_FREQ = 1000
running = False
while 1:
""" outputs """
is_right = 0
is_left = 0
is_forward = 0
is_backward = 0
""""""""""""""""""
kin = p.getKeyboardEvents()
if ord('s') in kin.keys() and kin[ord('s')] == p.KEY_IS_DOWN:
running = True
if 120 in kin.keys() and kin[120] == p.KEY_IS_DOWN:
file.close()
break
if rarr in kin.keys() and kin[rarr] == p.KEY_IS_DOWN:
robot.turn_right()
is_right = 1
elif larr in kin.keys() and kin[larr] == p.KEY_IS_DOWN:
robot.turn_left()
is_left = 1
else:
robot.turn_ahead()
if uarr in kin.keys() and kin[uarr] == p.KEY_IS_DOWN:
robot.go_forward(velocity, force)
is_forward = 1
elif darr in kin.keys() and kin[darr] == p.KEY_IS_DOWN:
robot.go_forward(-velocity, force)
is_backward = 1
else:
robot.go_forward(0, 0)
iteration += 1
if iteration % WRITE_FREQ == 0 and running:
write_list = road.sensors_output(robot) + robot.sensors_pos_string() + [is_right, is_left]
if is_forward == 0:
continue
print(*write_list, sep=' ', file=file)
print('write #{}'.format(iteration/1000))
print(write_list)
print('done')