def _initAgents(self): # Load agents for agent in self.scene.scene.findAllMatches('**/agents/agent*'): transform = TransformState.makeIdentity() if self.agentMode == 'capsule': shape = BulletCapsuleShape( self.agentRadius, self.agentHeight - 2 * self.agentRadius) elif self.agentMode == 'sphere': shape = BulletCapsuleShape(self.agentRadius, 2 * self.agentRadius) # XXX: use BulletCharacterControllerNode class, which already handles local transform? node = BulletRigidBodyNode('physics') node.setMass(self.agentMass) node.setStatic(False) node.setFriction(self.defaultMaterialFriction) node.setRestitution(self.defaultMaterialRestitution) node.addShape(shape) self.bulletWorld.attach(node) # Constrain the agent to have fixed position on the Z-axis node.setLinearFactor(Vec3(1.0, 1.0, 0.0)) # Constrain the agent not to be affected by rotations node.setAngularFactor(Vec3(0.0, 0.0, 0.0)) node.setIntoCollideMask(BitMask32.allOn()) node.setDeactivationEnabled(True) # Enable continuous collision detection (CCD) node.setCcdMotionThreshold(1e-7) node.setCcdSweptSphereRadius(0.50) if node.isStatic(): agent.setTag('physics-mode', 'static') else: agent.setTag('physics-mode', 'dynamic') # Attach the physic-related node to the scene graph physicsNp = NodePath(node) physicsNp.setTransform(transform) # Reparent all child nodes below the new physic node for child in agent.getChildren(): child.reparentTo(physicsNp) physicsNp.reparentTo(agent) # NOTE: we need this to update the transform state of the internal bullet node physicsNp.node().setTransformDirty() # Validation assert np.allclose( mat4ToNumpyArray(physicsNp.getNetTransform().getMat()), mat4ToNumpyArray(agent.getNetTransform().getMat()), atol=1e-6)
def _initLayoutModels(self): # Load layout objects as meshes for model in self.scene.scene.findAllMatches('**/layouts/object*/model*'): # NOTE: ignore models that have no geometry defined if model.getTightBounds() is None: logger.warning('Object %s has no geometry defined and will be ignored for physics!' % (str(model))) continue shape, transform = getCollisionShapeFromModel( model, mode='mesh', defaultCentered=False) node = BulletRigidBodyNode('physics') node.setMass(0.0) node.setFriction(self.defaultMaterialFriction) node.setRestitution(self.defaultMaterialRestitution) node.setStatic(True) node.addShape(shape) node.setDeactivationEnabled(True) node.setIntoCollideMask(BitMask32.allOn()) self.bulletWorld.attach(node) # Attach the physic-related node to the scene graph physicsNp = model.getParent().attachNewNode(node) physicsNp.setTransform(transform) if node.isStatic(): model.getParent().setTag('physics-mode', 'static') else: model.getParent().setTag('physics-mode', 'dynamic') # Reparent render and acoustics nodes (if any) below the new physic node # XXX: should be less error prone to just reparent all children # (except the hidden model) renderNp = model.getParent().find('**/render') if not renderNp.isEmpty(): renderNp.reparentTo(physicsNp) acousticsNp = model.getParent().find('**/acoustics') if not acousticsNp.isEmpty(): acousticsNp.reparentTo(physicsNp) semanticsNp = model.getParent().find('**/semantics') if not semanticsNp.isEmpty(): semanticsNp.reparentTo(physicsNp) # NOTE: we need this to update the transform state of the internal # bullet node physicsNp.node().setTransformDirty() # Validation assert np.allclose(mat4ToNumpyArray(physicsNp.getNetTransform().getMat()), mat4ToNumpyArray(model.getNetTransform().getMat()), atol=1e-6)
def testMat4ToNumpyArray(self): xr = np.random.random((4, 4)) mat = LMatrix4(*xr.ravel()) x = mat4ToNumpyArray(mat) self.assertTrue(np.allclose(x, xr, atol=1e-6))
def _initObjects(self): # Load objects for model in self.scene.scene.findAllMatches( '**/objects/object*/model*'): modelId = model.getParent().getTag('model-id') # XXX: we could create BulletGhostNode instance for non-collidable objects, but we would need to filter out the collisions later on if not modelId in self.openedDoorModelIds: shape, transform = getCollisionShapeFromModel( model, self.objectMode, defaultCentered=True) node = BulletRigidBodyNode('physics') node.addShape(shape) node.setFriction(self.defaultMaterialFriction) node.setRestitution(self.defaultMaterialRestitution) node.setIntoCollideMask(BitMask32.allOn()) node.setDeactivationEnabled(True) if self.suncgDatasetRoot is not None: # Check if it is a movable object category = self.modelCatMapping.getCoarseGrainedCategoryForModelId( modelId) if category in self.movableObjectCategories: # Estimate mass of object based on volumetric data and default material density objVoxFilename = os.path.join(self.suncgDatasetRoot, 'object_vox', 'object_vox_data', modelId, modelId + '.binvox') voxelData = ObjectVoxelData.fromFile(objVoxFilename) mass = Panda3dBulletPhysics.defaultDensity * voxelData.getFilledVolume( ) node.setMass(mass) else: node.setMass(0.0) node.setStatic(True) else: node.setMass(0.0) node.setStatic(True) if node.isStatic(): model.getParent().setTag('physics-mode', 'static') else: model.getParent().setTag('physics-mode', 'dynamic') # Attach the physic-related node to the scene graph physicsNp = model.getParent().attachNewNode(node) physicsNp.setTransform(transform) # Reparent render and acoustics nodes (if any) below the new physic node #XXX: should be less error prone to just reparent all children (except the hidden model) renderNp = model.getParent().find('**/render') if not renderNp.isEmpty(): renderNp.reparentTo(physicsNp) acousticsNp = model.getParent().find('**/acoustics') if not acousticsNp.isEmpty(): acousticsNp.reparentTo(physicsNp) # NOTE: we need this to update the transform state of the internal bullet node node.setTransformDirty() # NOTE: we need to add the node to the bullet engine only after setting all attributes self.bulletWorld.attach(node) # Validation assert np.allclose( mat4ToNumpyArray(physicsNp.getNetTransform().getMat()), mat4ToNumpyArray( model.getParent().getNetTransform().getMat()), atol=1e-6) else: logger.debug('Object %s ignored from physics' % (modelId))
def loadHouseFromJson(houseId, datasetRoot): filename = SunCgSceneLoader.getHouseJsonPath(datasetRoot, houseId) with open(filename) as f: data = json.load(f) assert houseId == data['id'] houseId = str(data['id']) # Create new node for house instance houseNp = NodePath('house-' + str(houseId)) objectIds = {} for levelId, level in enumerate(data['levels']): logger.debug('Loading Level %s to scene' % (str(levelId))) # Create new node for level instance levelNp = houseNp.attachNewNode('level-' + str(levelId)) roomNpByNodeIndex = {} for nodeIndex, node in enumerate(level['nodes']): if not node['valid'] == 1: continue modelId = str(node['modelId']) if node['type'] == 'Room': logger.debug('Loading Room %s to scene' % (modelId)) # Create new nodes for room instance roomNp = levelNp.attachNewNode('room-' + str(modelId)) roomLayoutsNp = roomNp.attachNewNode('layouts') roomObjectsNp = roomNp.attachNewNode('objects') # Load models defined for this room for roomObjFilename in reglob( os.path.join(datasetRoot, 'room', houseId), modelId + '[a-z].obj'): # Convert extension from OBJ + MTL to EGG format f, _ = os.path.splitext(roomObjFilename) modelFilename = f + ".egg" if not os.path.exists(modelFilename): raise Exception( 'The SUNCG dataset object models need to be convert to Panda3D EGG format!' ) # Create new node for object instance objectNp = NodePath('object-' + str(modelId) + '-0') objectNp.reparentTo(roomLayoutsNp) model = loadModel(modelFilename) model.setName('model-' + os.path.basename(f)) model.reparentTo(objectNp) model.hide() if 'nodeIndices' in node: for childNodeIndex in node['nodeIndices']: roomNpByNodeIndex[childNodeIndex] = roomObjectsNp elif node['type'] == 'Object': logger.debug('Loading Object %s to scene' % (modelId)) # Instance identification if modelId in objectIds: objectIds[modelId] = objectIds[modelId] + 1 else: objectIds[modelId] = 0 # Create new node for object instance objectNp = NodePath('object-' + str(modelId) + '-' + str(objectIds[modelId])) #TODO: loading the BAM format would be much more efficient # Convert extension from OBJ + MTL to EGG format objFilename = os.path.join(datasetRoot, 'object', node['modelId'], node['modelId'] + '.obj') assert os.path.exists(objFilename) f, _ = os.path.splitext(objFilename) modelFilename = f + ".egg" if not os.path.exists(modelFilename): raise Exception( 'The SUNCG dataset object models need to be convert to Panda3D EGG format!' ) model = loadModel(modelFilename) model.setName('model-' + os.path.basename(f)) model.reparentTo(objectNp) model.hide() # 4x4 column-major transformation matrix from object coordinates to scene coordinates transform = np.array(node['transform']).reshape((4, 4)) # Transform from Y-UP to Z-UP coordinate systems #TODO: use Mat4.convertMat(CS_zup_right, CS_yup_right) yupTransform = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]]) zupTransform = np.array([[1, 0, 0, 0], [0, 0, 1, 0], [0, -1, 0, 0], [0, 0, 0, 1]]) transform = np.dot(np.dot(yupTransform, transform), zupTransform) transform = TransformState.makeMat( LMatrix4f(*transform.ravel())) # 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 objectNp.setTransform( transform.compose(TransformState.makePos(centerPos))) model.setTransform(TransformState.makePos(-centerPos)) # Get the parent nodepath for the object (room or level) if nodeIndex in roomNpByNodeIndex: objectNp.reparentTo(roomNpByNodeIndex[nodeIndex]) else: objectNp.reparentTo(levelNp) # Validation assert np.allclose(mat4ToNumpyArray( model.getNetTransform().getMat()), mat4ToNumpyArray(transform.getMat()), atol=1e-6) objectNp.setTag('model-id', str(modelId)) objectNp.setTag('level-id', str(levelId)) objectNp.setTag('house-id', str(houseId)) elif node['type'] == 'Ground': logger.debug('Loading Ground %s to scene' % (modelId)) # Create new nodes for ground instance groundNp = levelNp.attachNewNode('ground-' + str(modelId)) groundLayoutsNp = groundNp.attachNewNode('layouts') # Load model defined for this ground for groundObjFilename in reglob( os.path.join(datasetRoot, 'room', houseId), modelId + '[a-z].obj'): # Convert extension from OBJ + MTL to EGG format f, _ = os.path.splitext(groundObjFilename) modelFilename = f + ".egg" if not os.path.exists(modelFilename): raise Exception( 'The SUNCG dataset object models need to be convert to Panda3D EGG format!' ) objectNp = NodePath('object-' + str(modelId) + '-0') objectNp.reparentTo(groundLayoutsNp) model = loadModel(modelFilename) model.setName('model-' + os.path.basename(f)) model.reparentTo(objectNp) model.hide() else: raise Exception('Unsupported node type: %s' % (node['type'])) scene = Scene() houseNp.reparentTo(scene.scene) # Recenter objects in rooms for room in scene.scene.findAllMatches('**/room*'): # Calculate the center of this room minBounds, maxBounds = room.getTightBounds() centerPos = minBounds + (maxBounds - minBounds) / 2.0 # Add offset transform to room node room.setTransform(TransformState.makePos(centerPos)) # Add recentering transform to all children nodes for childNp in room.getChildren(): childNp.setTransform(TransformState.makePos(-centerPos)) # Recenter objects in grounds for ground in scene.scene.findAllMatches('**/ground*'): # Calculate the center of this ground minBounds, maxBounds = ground.getTightBounds() centerPos = minBounds + (maxBounds - minBounds) / 2.0 # Add offset transform to ground node ground.setTransform(TransformState.makePos(centerPos)) # Add recentering transform to all children nodes for childNp in ground.getChildren(): childNp.setTransform(TransformState.makePos(-centerPos)) return scene