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