pybullet-parallel/multiple_processes_pool.py at main · RKJenamani/pybullet-parallel · GitHub

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
# import threading
import multiprocessing
import time
import numpy as np
from types import SimpleNamespace
from multiprocessing import Pool

def setup_exp(p):
    mu = 0.65
    object_ids = []

    local_path = "./"

    state = ([-0.0404755 , -0.00112935 , 0.183326,0.000183599, 0.0274296, -3.12269])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/024_bowl/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

    state = ([-0.031447 , 0.00245332 , 0.231471,1.11077, 0.116883, 1.88827])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/037_scissors/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

    state = ([-0.0342039 , -0.0411717 , 0.221495,-1.21418, -0.411491, 1.3281])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/011_banana/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

    state = ([-0.00109234 , 0.00217348, 0.0940739,0.603044, -1.5319, 2.60045])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/004_sugar_box/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

    state = ([0.0431414 , -0.0814738, 0.100775,-1.56183, 0.0200383, -0.0332266])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/010_potted_meat_can/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

    state = ([-0.0996265 , -0.0109982, 0.229266,1.09516, -0.0324135, 1.93206])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/037_scissors/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

    state = ([0.0391189 , -0.0793964, 0.142291,-3.03823, -0.084093, 1.51302])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/009_gelatin_box/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

    state = ([-0.039549 , -0.000712143, 0.141748,-0.00616443, 0.0036889, -1.53266])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/008_pudding_box/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

    state = ([0.0518593 , -0.113949, 0.206423,-2.99895, -0.0929695, 1.27618])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/007_tuna_fish_can/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

    state = ([0.0575085 , -0.102112, 0.171442,-3.01417, -0.111824, 1.44483])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/007_tuna_fish_can/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

    state = ([0.000732725 , 0.000258393, 0.0369799,-2.1136, -1.55321, 0.539161])
    req = SimpleNamespace(o_x=state[0],o_y=state[1],o_z=state[2],o_r = state[3], o_p = state[4], o_yaw = state[5], type = 8)
    xyz = [req.o_x, req.o_y, req.o_z]
    rpy = [req.o_r, req.o_p, req.o_yaw]
    body_id = p.loadURDF(local_path + "models/003_cracker_box/model.urdf", xyz, p.getQuaternionFromEuler(rpy))
    p.changeDynamics(body_id, -1, lateralFriction=mu)
    object_ids.append(body_id)

def make_sim():

    import pybullet
    import pybullet_data
    import pybullet_utils.bullet_client as bc
    p = bc.BulletClient(connection_mode=pybullet.DIRECT)

    # p.connect(p.DIRECT)

    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.setGravity(0,0,-10)
    plane = p.loadURDF("plane.urdf")
    p.setRealTimeSimulation(0)

    setup_exp(p)

    for _ in range(1000):
        p.stepSimulation()

    return None


def worker(inst):
    pos = make_sim()


for iter in range(1,13):

    num_parallel = iter
    start_time = time.time()

    p = Pool(num_parallel)
    p.map(worker, range(num_parallel))

    end_time = time.time()

    print("Total time taken: {0:.2f} | Parallel Envs: {1} | Time taken per simulation: {2:.2f} ".format((end_time-start_time),num_parallel,(end_time-start_time)/num_parallel))