-
Notifications
You must be signed in to change notification settings - Fork 0
/
parcour.py
117 lines (100 loc) · 6.21 KB
/
parcour.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
import pybullet as p
import math
def tunnel(length, width, wall_width = 0.05, position=[0., 0., 0.], orientation=[0., 0., 0., 1.]):
# create a rectangular tunnel as a building block of an obstacle parcour
half_extents = [[length / 2, width / 2 + wall_width, wall_width / 2],
[length / 2, width / 2 + wall_width, wall_width / 2],
[length / 2, wall_width / 2, width / 2],
[length / 2, wall_width / 2, width / 2]]
positions = [[0, 0., -width / 2 - wall_width / 2],
[0, 0., width / 2 + wall_width / 2],
[0, -width / 2 - wall_width / 2, 0.],
[0, width / 2 + wall_width / 2, 0.]]
visual_shape_id = p.createVisualShapeArray(shapeTypes=[p.GEOM_BOX] * 4,
halfExtents=half_extents,
visualFramePositions=positions
)
collision_shape_id = p.createCollisionShapeArray(shapeTypes=[p.GEOM_BOX] * 4,
halfExtents=half_extents,
collisionFramePositions=positions
)
mb = p.createMultiBody(baseMass=0.,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collision_shape_id,
baseVisualShapeIndex=visual_shape_id,
basePosition=position,
baseOrientation=orientation,
useMaximalCoordinates=False)
p.changeVisualShape(mb, -1, rgbaColor=[1, 1, 1, 0.5], specularColor=[0.4, 0.4, 0])
return mb
def corner(width, wall_width=0.05, position=[0., 0., 0.], orientation=[0., 0., 0., 1.]):
# create a corner (cube open at neg. x and pos. z) as a building block of an obstacle parcour
half_extents = [[width / 2, width / 2 + wall_width, wall_width / 2],
[width / 2, wall_width / 2, width / 2],
[width / 2, wall_width / 2, width / 2],
[wall_width / 2, width / 2 + wall_width, width / 2]]
positions = [[0, 0., -width / 2 - wall_width / 2],
[0, -width / 2 - wall_width / 2, 0.],
[0, width / 2 + wall_width / 2, 0.],
[width/2 + wall_width / 2, 0., 0.]]
visual_shape_id = p.createVisualShapeArray(shapeTypes=[p.GEOM_BOX] * 4,
halfExtents=half_extents,
visualFramePositions=positions
)
collision_shape_id = p.createCollisionShapeArray(shapeTypes=[p.GEOM_BOX] * 4,
halfExtents=half_extents,
collisionFramePositions=positions
)
mb = p.createMultiBody(baseMass=0.,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collision_shape_id,
baseVisualShapeIndex=visual_shape_id,
basePosition=position,
baseOrientation=orientation,
useMaximalCoordinates=False)
p.changeVisualShape(mb, -1, rgbaColor=[1, 1, 1, 0.5], specularColor=[0.4, 0.4, 0])
return mb
def cap(width, wall_width=0.05, position=[0, 0, 0,], orientation=[0, 0, 0]):
# create a cap that can be used to close corners or tunnels.
# By default it is a wall in the y-z-plane that can be rotated and translated to the desired position
# create a corner (cube open at neg. x and pos. z) as a building block of an obstacle parcour
half_extents = [wall_width / 2, width / 2 + wall_width, width / 2]
pos = [0, 0., 0.]
visual_shape_id = p.createVisualShape(shapeType=p.GEOM_BOX,
halfExtents=half_extents,
visualFramePosition=pos
)
collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_BOX,
halfExtents=half_extents,
collisionFramePosition=pos
)
mb = p.createMultiBody(baseMass=0.,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collision_shape_id,
baseVisualShapeIndex=visual_shape_id,
basePosition=position,
baseOrientation=orientation,
useMaximalCoordinates=False)
p.changeVisualShape(mb, -1, rgbaColor=[1, 1, 1, 0.5], specularColor=[0.4, 0.4, 0])
return mb
def parcour():
# create an obstacle parcour
# width of the tunnels
width = 1
# create some orientation quaternions to rotate tunnels and corners
tunnel_ornx = [0., 0., 0., 1.]
tunnel_orny = p.getQuaternionFromAxisAngle(axis=[0, 0, 1], angle=math.pi / 2)
tunnel_ornz = p.getQuaternionFromAxisAngle(axis=[0, 1, 0], angle=math.pi / 2)
corner_negxposz = [0., 0., 0., 1.]
corner_negzposy = p.getQuaternionFromEuler([math.pi, 0.0, -math.pi / 2])
corner_negyposx = p.getQuaternionFromEuler([math.pi/2, 0.0, math.pi / 2])
# create the parcour
cap(width=width, position=[-1., 0., 2.])
tunnel(length=5., width=width, position=[1.5, 0., 2.], orientation=tunnel_ornx)
corner(width=width, position=[4.5, 0., 2.], orientation=corner_negxposz)
tunnel(length=2., width=width, position=[4.5, 0.0, 3.5], orientation=tunnel_ornz)
corner(width=width, position=[4.5, 0., 5.0], orientation=corner_negzposy)
tunnel(length=4., width=width, position=[4.5, 2.5, 5], orientation=tunnel_orny)
corner(width=width, position=[4.5, 5., 5.], orientation=corner_negyposx)
tunnel(length=4., width=width, position=[7.0, 5.0, 5.], orientation=tunnel_ornx)
cap(width=width, position=[9., 5., 5.])