-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathcurve_map.py
137 lines (123 loc) · 5.29 KB
/
curve_map.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
import pybullet as p
import time
import pybullet_data
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
# Create a floor and walls
# floor = p.createCollisionShape(p.GEOM_PLANE)
# walls = p.createCollisionShape(p.GEOM_BOX, halfExtents=[2, 2, 0.1])
# p.createMultiBody(baseCollisionShapeIndex=walls, basePosition=[0, 0, 0.5])
# Create a box object
# startPos = [0,0,1]
startOrientation = p.getQuaternionFromEuler([0,0,0])
# Middle Right Line
box = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 1, 0.2])
box_body_1 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box,
basePosition=[1, 1, 0.2])
# Middle Left Line
box_body_1 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box,
basePosition=[0, 0, 0.2])
# Right Up curve
box_curve1 = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.1, 0.1, 0.2])
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[0.8, 2, 0.2])
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[0.6, 2.1, 0.2])
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[0.4, 2.2, 0.2])
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[0.2, 2.3, 0.2])
# Left Right Curve
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[-0.2, 1, 0.2])
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[-0.4, 1.1, 0.2])
# Up Left and Right Straight Lines
box_horizontal = p.createCollisionShape(p.GEOM_BOX, halfExtents=[1, 0.1, 0.2])
box_body_1 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_horizontal,
basePosition=[-1.5, 1.1, 0.2])
box_body_1 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_horizontal,
basePosition=[-0.9, 2.3, 0.2])
# Right down curve
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[1.1, -0.1, 0.2])
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[1.2, -0.3, 0.2])
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[1.3, -0.5, 0.2])
# Left down curve
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[0.1, -1.1, 0.2])
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[0.2, -1.3, 0.2])
box_body_2 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_curve1,
basePosition=[0.3, -1.5, 0.2])
# Down Left and Right Straight Lines
box_horizontal = p.createCollisionShape(p.GEOM_BOX, halfExtents=[1, 0.1, 0.2])
box_horizontal_add = p.createCollisionShape(p.GEOM_BOX, halfExtents=[1.5, 0.1, 0.2])
box_body_1 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_horizontal_add,
basePosition=[1.9, -1.5, 0.2])
box_body_1 = p.createMultiBody(
baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=box_horizontal,
basePosition=[2.4, -0.5, 0.2])
# Run the simulation
for i in range(10000):
p.stepSimulation()
time.sleep(1./240.)
# Disconnect from the simulation environment
p.disconnect()