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 createPlane(self, pos): rb = BulletRigidBodyNode("plane") rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1)) rb.setFriction(1.0) rb.setAnisotropicFriction(1.0) rb.setRestitution(1.0) np = self.render.attachNewNode(rb) np.setPos(pos) np.setCollideMask(BitMask32.bit(0)) self.world.attachRigidBody(rb) return np
def makeBall(self, num, pos = (0, 0, 0)): shape = BulletSphereShape(0.5) node = BulletRigidBodyNode('ball_' + str(num)) node.setMass(1.0) node.setRestitution(.9) node.setDeactivationEnabled(False) node.addShape(shape) physics = render.attachNewNode(node) physics.setPos(*pos) self.world.attachRigidBody(node) model = loader.loadModel('models/ball') color = self.makeColor() model.setColor(color) self.ballColors['ball_' + str(num)] = color model.reparentTo(physics)
def createPlane(self, pos): rb = BulletRigidBodyNode("GroundPlane") rb.addShape(BulletPlaneShape(Vec3(0, 0, 1), 1)) rb.setFriction(1.0) rb.setAnisotropicFriction(1.0) rb.setRestitution(1.0) np = self.render.attachNewNode(rb) np.setPos(pos) np.setCollideMask(BitMask32.bit(0)) plane = self.loader.loadModel("cube") plane.setScale(Vec3(100, 100, 1)) plane.setPos(Vec3(0, 0, -0.5)) plane.setColor(VBase4(0.8, 0.8, 0.8, 1.0)) plane.reparentTo(np) self.world.attachRigidBody(rb) return np
def createBox(self, mass, pos, scale, color): rb = BulletRigidBodyNode('box') rb.addShape(BulletBoxShape(scale)) rb.setMass(mass) rb.setLinearDamping(0.2) rb.setAngularDamping(0.9) rb.setFriction(1.0) rb.setAnisotropicFriction(1.0) rb.setRestitution(0.0) self.world.attachRigidBody(rb) np = self.render.attachNewNode(rb) np.setPos(pos) np.setCollideMask(BitMask32.bit(1)) cube = self.loader.loadModel('cube') cube.setScale(scale) cube.setColor(color) cube.reparentTo(np) return np
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 setupObstacleThree(self, pos, scale, turn): # Box A shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)) bodyA = BulletRigidBodyNode('Box A') bodyA.setRestitution(1.0) bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) #Box B shape = BulletBoxShape(Vec3(0.1,0.1,0.1)) bodyB = BulletRigidBodyNode('Box B') bodyB.setRestitution(1.0) bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1,0.1,0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Slider frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True) slider.setDebugDrawSize(2.0) slider.setLowerLinearLimit(0) slider.setUpperLinearLimit(12) slider.setLowerAngularLimit(-90) slider.setUpperAngularLimit(-85) self.world.attachConstraint(slider) # Box C shape = BulletBoxShape(Vec3(1, 3, 0.1)) bodyC = BulletRigidBodyNode('Box C') bodyC.setRestitution(1.0) bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setMass(0.1) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(Vec3(pos.getX() + 3, pos.getY() - 4, pos.getZ())) bodyNP.setHpr(turn) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(1, 3, 0.1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyC) bodyNP.node().setLinearVelocity(-100) # Slider frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0)) slider = BulletSliderConstraint(bodyA, bodyC, frameA, frameB, True) slider.setDebugDrawSize(2.0) slider.setLowerLinearLimit(2) slider.setUpperLinearLimit(6) slider.setLowerAngularLimit(-90) slider.setUpperAngularLimit(-85) self.world.attachConstraint(slider)