def testAgent(self):
physics = None
viewer = None
try:
scene = Scene()
physics = Panda3dBulletPhysics(scene, debug=True)
agent = scene.agents[0].find('**/+BulletRigidBodyNode')
agent.setPos(LVector3f(0, 0, 1.0))
agent.node().setLinearVelocity(LVector3f(1, 0, 0))
agent.node().setAngularVelocity(LVector3f(0, 0, 1))
agent.node().setActive(True)
viewer = Viewer(scene, interactive=False)
viewer.disableMouse()
mat = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, -10, 0, 1])
mat = LMatrix4f(*mat.ravel())
viewer.cam.setMat(mat)
for _ in range(50):
viewer.step()
time.sleep(1.0)
finally:
self.hulkSmash(None, physics, viewer)
def testDebugHouseWithRender(self):
renderer = None
physics = None
viewer = None
try:
scene = SunCgSceneLoader.loadHouseFromJson(
"0004d52d1aeeb8ae6de39d6bd993e992", TEST_SUNCG_DATA_DIR)
renderer = Panda3dRenderer(scene,
shadowing=False,
mode='offscreen')
renderer.showRoomLayout(showCeilings=False)
physics = Panda3dBulletPhysics(
scene, suncgDatasetRoot=TEST_SUNCG_DATA_DIR, debug=True)
viewer = Viewer(scene, interactive=False)
viewer.disableMouse()
mat = np.array([
0.999992, 0.00394238, 0, 0, -0.00295702, 0.750104, -0.661314,
0, -0.00260737, 0.661308, 0.75011, 0, 43.621, -55.7499,
12.9722, 1
])
mat = LMatrix4f(*mat.ravel())
viewer.cam.setMat(mat)
for _ in range(20):
viewer.step()
time.sleep(1.0)
finally:
self.hulkSmash(renderer, physics, viewer)
def testDebugHouseWithViewer(self):
try:
scene = SunCgSceneLoader.loadHouseFromJson(
"0004d52d1aeeb8ae6de39d6bd993e992", TEST_SUNCG_DATA_DIR)
physics = Panda3dBulletPhysics(
scene, suncgDatasetRoot=TEST_SUNCG_DATA_DIR, debug=True)
viewer = Viewer(scene, interactive=False)
viewer.disableMouse()
mat = np.array([
0.999992, 0.00394238, 0, 0, -0.00295702, 0.750104, -0.661314,
0, -0.00260737, 0.661308, 0.75011, 0, 43.621, -55.7499,
12.9722, 1
])
mat = LMatrix4f(*mat.ravel())
viewer.cam.setMat(mat)
for _ in range(20):
viewer.step()
time.sleep(1.0)
finally:
physics.destroy()
viewer.destroy()
viewer.graphicsEngine.removeAllWindows()
def __init__(self, houseId, suncgDatasetRoot=None, size=(256, 256), debug=False, depth=False, realtime=False, dt=0.1, cameraTransform=None):
self.__dict__.update(houseId=houseId, suncgDatasetRoot=suncgDatasetRoot, size=size,
debug=debug, depth=depth, realtime=realtime, dt=dt, cameraTransform=cameraTransform)
self.scene = SunCgSceneLoader.loadHouseFromJson(houseId, suncgDatasetRoot)
agentRadius = 0.1
agentHeight = 1.6
if self.cameraTransform is None:
self.cameraTransform = TransformState.makePos(LVector3f(0.0, 0.0, agentHeight/2.0 - agentRadius))
self.renderWorld = Panda3dRenderer(self.scene, size, shadowing=False, depth=depth, cameraTransform=self.cameraTransform)
self.physicWorld = Panda3dBulletPhysics(self.scene, suncgDatasetRoot, debug=debug, objectMode='box',
agentRadius=agentRadius, agentHeight=agentHeight, agentMass=60.0, agentMode='capsule')
self.clock = ClockObject.getGlobalClock()
self.worlds = {
"physics": self.physicWorld,
"render": self.renderWorld,
}
self.agent = self.scene.agents[0]
self.agentRbNp = self.agent.find('**/+BulletRigidBodyNode')
self.labeledNavMap = None
self.occupancyMapCoord = None
def testStep(self):
scene = SunCgSceneLoader.loadHouseFromJson(
"0004d52d1aeeb8ae6de39d6bd993e992", TEST_SUNCG_DATA_DIR)
physics = Panda3dBulletPhysics(scene)
for _ in range(10):
physics.step(dt=0.1)
physics.destroy()
def testDebugObjectWithRender(self):
scene = Scene()
modelId = '83'
modelFilename = os.path.join(TEST_SUNCG_DATA_DIR, "object",
str(modelId),
str(modelId) + ".egg")
assert os.path.exists(modelFilename)
model = loadModel(modelFilename)
model.setName('model-' + str(modelId))
model.hide(BitMask32.allOn())
objectsNp = scene.scene.attachNewNode('objects')
objNp = objectsNp.attachNewNode('object-' + str(modelId))
model.reparentTo(objNp)
# Calculate the center of this object
minBounds, maxBounds = model.getTightBounds()
centerPos = minBounds + (maxBounds - minBounds) / 2.0
# Add offset transform to make position relative to the center
model.setTransform(TransformState.makePos(-centerPos))
renderer = None
physics = None
viewer = None
try:
renderer = Panda3dRenderer(scene, shadowing=False)
physics = Panda3dBulletPhysics(scene, debug=True)
viewer = Viewer(scene, interactive=False)
viewer.disableMouse()
viewer.cam.setTransform(
TransformState.makePos(LVecBase3(5.0, 0.0, 0.0)))
viewer.cam.lookAt(model)
for _ in range(20):
viewer.step()
time.sleep(1.0)
finally:
self.hulkSmash(renderer, physics, viewer)
def testCalculate2dNavigationMap(self):
scene = SunCgSceneLoader.loadHouseFromJson(
"0004d52d1aeeb8ae6de39d6bd993e992", TEST_SUNCG_DATA_DIR)
physics = Panda3dBulletPhysics(scene)
navMap, _ = physics.calculate2dNavigationMap(scene.agents[0],
z=1.0,
precision=0.1)
self.assertTrue(np.max(navMap) >= 1.0)
fig = plt.figure()
plt.axis("off")
ax = plt.subplot(111)
ax.imshow(navMap, cmap='gray')
plt.show(block=False)
time.sleep(1.0)
plt.close(fig)
physics.destroy()
def main():
# Create scene and remove any default agents
scene = SunCgSceneLoader.loadHouseFromJson(
houseId="0004d52d1aeeb8ae6de39d6bd993e992", datasetRoot=SUNCG_DATA_DIR)
scene.scene.find('**/agents').node().removeAllChildren()
scene.agents = []
# Create multiple agents
agents = []
for i in range(3):
agentRadius = 0.15
agent = Agent(scene, 'agent-%d' % (i), agentRadius)
agents.append(agent)
# NOTE: specify to move the camera slightly outside the model (not to render the interior of the model)
cameraTransform = TransformState.makePos(
LVector3f(0.0, 1.1 * agentRadius, 0.0))
# Initialize rendering and physics
renderer = RgbRenderer(scene,
size=(128, 128),
fov=70.0,
cameraTransform=cameraTransform)
renderer.showRoomLayout(showCeilings=False,
showWalls=True,
showFloors=True)
physics = Panda3dBulletPhysics(scene,
SUNCG_DATA_DIR,
objectMode='box',
agentRadius=agentRadius,
agentMode='sphere')
# Configure the camera
viewer = Viewer(scene,
interactive=False,
showPosition=False,
cameraMask=renderer.cameraMask)
transform = TransformState.makePosHpr(LVecBase3f(44.01, -43.95, 22.97),
LVecBase3f(0.0, -81.04, 0.0))
viewer.cam.setTransform(transform)
# Initialize the agent
agents[0].setPosition((45, -42.5, 1.6))
agents[1].setPosition((42.5, -39, 1.6))
agents[2].setPosition((42.5, -41.5, 1.6))
# Initialize figure that will show the point-of-view of each agent
plt.ion()
fig = plt.figure(figsize=(12, 4), facecolor='white')
ims = []
for i in range(len(agents)):
ax = fig.add_subplot(1, len(agents), i + 1)
ax.set_title(agents[i].agentId)
ax.axis('off')
rgbImage = np.zeros(renderer.size + (3, ), dtype=np.uint8)
im = ax.imshow(rgbImage)
ims.append(im)
plt.tight_layout()
plt.show()
# Main loop
clock = ClockObject.getGlobalClock()
try:
while True:
# Update physics
dt = clock.getDt()
physics.step(dt)
# Update viewer
viewer.step()
for i, agent in enumerate(agents):
# Get the current RGB
rgbImage = renderer.getRgbImage(agent.agentId,
channelOrder="RGB")
# Get the current observation for the agent
observation = {
"position": agent.getPosition(),
"orientation": agent.getOrientation(),
"rgb-image": rgbImage
}
agent.step(observation)
ims[i].set_data(rgbImage)
fig.canvas.draw()
plt.pause(0.0001)
except KeyboardInterrupt:
pass
viewer.destroy()
renderer.destroy()
return 0
