-
Notifications
You must be signed in to change notification settings - Fork 0
/
blankdrive.py
68 lines (51 loc) · 1.72 KB
/
blankdrive.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
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)
""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
# BIG IMPORTANT!!!!!!
# a road point is 0.3x0.3 coordinates big
p.resetDebugVisualizerCamera(25, 0, -89.9, [0, 0, 0])
robot = Goodbot("goodbot/goodbot.urdf", [0, 0, 0])
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)
xSlider = p.addUserDebugParameter("positionx", -15, 15, 0)
ySlider = p.addUserDebugParameter("positiony", -15, 15, 0)
tile = p.loadURDF("road/road_point.urdf", [ 0, 0, 0.1] )
count = 0
while 1:
if count == 1000:
rp_x = p.readUserDebugParameter(xSlider)
rp_y = p.readUserDebugParameter(ySlider)
p.removeBody(tile)
tile = p.loadURDF("road/road_point.urdf", [rp_x, rp_y, 0.1] )
count = 0
count += 1
kin = p.getKeyboardEvents()
if p.B3G_END in kin.keys() and kin[p.B3G_END] == p.KEY_IS_DOWN:
file.close()
break
if rarr in kin.keys() and kin[rarr] == p.KEY_IS_DOWN:
robot.turn_right()
elif larr in kin.keys() and kin[larr] == p.KEY_IS_DOWN:
robot.turn_left()
else:
robot.turn_ahead()
if uarr in kin.keys() and kin[uarr] == p.KEY_IS_DOWN:
robot.go_forward(velocity, force)
elif darr in kin.keys() and kin[darr] == p.KEY_IS_DOWN:
robot.go_forward(-velocity, force)
else:
robot.go_forward(0, 0)