75 lines
1.6 KiB
Python
75 lines
1.6 KiB
Python
import pybullet as p
|
|
import time
|
|
import math
|
|
|
|
useGui = True
|
|
|
|
if (useGui):
|
|
p.connect(p.GUI)
|
|
else:
|
|
p.connect(p.DIRECT)
|
|
|
|
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
|
|
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
|
|
|
#p.loadURDF("samurai.urdf")
|
|
p.loadURDF("r2d2.urdf", [3, 3, 1])
|
|
|
|
rayFrom = []
|
|
rayTo = []
|
|
rayIds = []
|
|
|
|
numRays = 1024
|
|
|
|
rayLen = 13
|
|
|
|
rayHitColor = [1, 0, 0]
|
|
rayMissColor = [0, 1, 0]
|
|
|
|
replaceLines = True
|
|
|
|
for i in range(numRays):
|
|
rayFrom.append([0, 0, 1])
|
|
rayTo.append([
|
|
rayLen * math.sin(2. * math.pi * float(i) / numRays),
|
|
rayLen * math.cos(2. * math.pi * float(i) / numRays), 1
|
|
])
|
|
if (replaceLines):
|
|
rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor))
|
|
else:
|
|
rayIds.append(-1)
|
|
|
|
if (not useGui):
|
|
timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "rayCastBench.json")
|
|
|
|
numSteps = 10
|
|
if (useGui):
|
|
numSteps = 327680
|
|
|
|
for i in range(numSteps):
|
|
p.stepSimulation()
|
|
for j in range(8):
|
|
results = p.rayTestBatch(rayFrom, rayTo, j + 1)
|
|
|
|
#for i in range (10):
|
|
# p.removeAllUserDebugItems()
|
|
|
|
if (useGui):
|
|
if (not replaceLines):
|
|
p.removeAllUserDebugItems()
|
|
|
|
for i in range(numRays):
|
|
hitObjectUid = results[i][0]
|
|
|
|
if (hitObjectUid < 0):
|
|
hitPosition = [0, 0, 0]
|
|
p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor, replaceItemUniqueId=rayIds[i])
|
|
else:
|
|
hitPosition = results[i][3]
|
|
p.addUserDebugLine(rayFrom[i], hitPosition, rayHitColor, replaceItemUniqueId=rayIds[i])
|
|
|
|
#time.sleep(1./240.)
|
|
|
|
if (not useGui):
|
|
p.stopStateLogging(timingLog)
|