1import os 2import inspect 3currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) 4parentdir = os.path.dirname(os.path.dirname(currentdir)) 5os.sys.path.insert(0, parentdir) 6 7from pybullet_utils import bullet_client 8import panda_sim_grasp as panda_sim 9 10 11import time 12 13useGUI = True#False 14timeStep = 1./240. 15 16# Importing the libraries 17import os 18import time 19import multiprocessing as mp 20from multiprocessing import Process, Pipe 21 22pandaEndEffectorIndex = 11 #8 23pandaNumDofs = 7 24 25 26_RESET = 1 27_CLOSE = 2 28_EXPLORE = 3 29 30 31def ExploreWorker(rank, num_processes, childPipe, args): 32 print("hi:",rank, " out of ", num_processes) 33 import pybullet as op1 34 import pybullet_data as pd 35 logName="" 36 p1=0 37 n = 0 38 space = 2 39 simulations=[] 40 sims_per_worker = 10 41 42 offsetY = rank*space 43 while True: 44 n += 1 45 try: 46 # Only block for short times to have keyboard exceptions be raised. 47 if not childPipe.poll(0.0001): 48 continue 49 message, payload = childPipe.recv() 50 except (EOFError, KeyboardInterrupt): 51 break 52 if message == _RESET: 53 if (useGUI): 54 p1 = bullet_client.BulletClient(op1.GUI) 55 else: 56 p1 = bullet_client.BulletClient(op1.DIRECT) 57 p1.setTimeStep(timeStep) 58 59 p1.setPhysicsEngineParameter(numSolverIterations=8) 60 p1.setPhysicsEngineParameter(minimumSolverIslandSize=100) 61 p1.configureDebugVisualizer(p1.COV_ENABLE_Y_AXIS_UP,1) 62 p1.configureDebugVisualizer(p1.COV_ENABLE_RENDERING,0) 63 p1.setAdditionalSearchPath(pd.getDataPath()) 64 p1.setGravity(0,-9.8,0) 65 logName = str("batchsim")+str(rank) 66 for j in range (3): 67 offsetX = 0#-sims_per_worker/2.0*space 68 for i in range (sims_per_worker): 69 offset=[offsetX,0, offsetY] 70 sim = panda_sim.PandaSimAuto(p1, offset) 71 simulations.append(sim) 72 offsetX += space 73 offsetY += space 74 childPipe.send(["reset ok"]) 75 p1.configureDebugVisualizer(p1.COV_ENABLE_RENDERING,1) 76 for i in range (100): 77 p1.stepSimulation() 78 79 logId = p1.startStateLogging(op1.STATE_LOGGING_PROFILE_TIMINGS,logName) 80 continue 81 if message == _EXPLORE: 82 sum_rewards=rank 83 84 if useGUI: 85 numSteps = int(20000) 86 else: 87 numSteps = int(5) 88 for i in range (numSteps): 89 for s in simulations: 90 s.step() 91 p1.stepSimulation() 92 #print("logId=",logId) 93 #print("numSteps=",numSteps) 94 95 childPipe.send([sum_rewards]) 96 continue 97 if message == _CLOSE: 98 p1.stopStateLogging(logId) 99 childPipe.send(["close ok"]) 100 break 101 childPipe.close() 102 103 104if __name__ == "__main__": 105 mp.freeze_support() 106 if useGUI: 107 num_processes = 1 108 else: 109 num_processes = 12 110 processes = [] 111 args=[0]*num_processes 112 113 childPipes = [] 114 parentPipes = [] 115 116 for pr in range(num_processes): 117 parentPipe, childPipe = Pipe() 118 parentPipes.append(parentPipe) 119 childPipes.append(childPipe) 120 121 for rank in range(num_processes): 122 p = mp.Process(target=ExploreWorker, args=(rank, num_processes, childPipes[rank], args)) 123 p.start() 124 processes.append(p) 125 126 127 for parentPipe in parentPipes: 128 parentPipe.send([_RESET, "blaat"]) 129 130 positive_rewards = [0]*num_processes 131 for k in range(num_processes): 132 #print("reset msg=",parentPipes[k].recv()[0]) 133 msg = parentPipes[k].recv()[0] 134 for parentPipe in parentPipes: 135 parentPipe.send([_EXPLORE, "blaat"]) 136 137 positive_rewards = [0]*num_processes 138 for k in range(num_processes): 139 positive_rewards[k] = parentPipes[k].recv()[0] 140 #print("positive_rewards=",positive_rewards[k]) 141 142 for parentPipe in parentPipes: 143 parentPipe.send([_EXPLORE, "blaat"]) 144 positive_rewards = [0]*num_processes 145 for k in range(num_processes): 146 positive_rewards[k] = parentPipes[k].recv()[0] 147 #print("positive_rewards=",positive_rewards[k]) 148 msg = positive_rewards[k] 149 150 for parentPipe in parentPipes: 151 parentPipe.send([_EXPLORE, "blaat"]) 152 153 positive_rewards = [0]*num_processes 154 for k in range(num_processes): 155 positive_rewards[k] = parentPipes[k].recv()[0] 156 #print("positive_rewards=",positive_rewards[k]) 157 158 159 for parentPipe in parentPipes: 160 parentPipe.send([_CLOSE, "pay2"]) 161 162 for p in processes: 163 p.join() 164 165 #now we merge the separate json files into a single one 166 fnameout = 'batchsim.json' 167 count = 0 168 outfile = open(fnameout, "w+") 169 outfile.writelines(["{\"traceEvents\":[\n"]) 170 numFiles = num_processes 171 for num in range(numFiles): 172 print("num=",num) 173 fname = 'batchsim%d_0.json' % (num) 174 with open(fname) as infile: 175 for line in infile: 176 if "pid" in line: 177 line = line.replace('\"pid\":1', '\"pid\":'+str(num)) 178 if num < (numFiles-1) and not "{}}," in line: 179 line = line.replace('{}}', '{}},') 180 print("line[",count,"]=",line) 181 outfile.write(line) 182 count += 1 183 print ("count=",count) 184 outfile.writelines(["],\n"]) 185 outfile.writelines(["\"displayTimeUnit\": \"ns\"}\n"]) 186 outfile.close() 187