def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(False) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Box A shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) bodyA = BulletRigidBodyNode('Box A') bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(-3, 0, 4) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(0, 0, 0) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Slider frameA = TransformState.makePosHpr(Point3(2, 0, 0), Vec3(0, 0, 45)) frameB = TransformState.makePosHpr(Point3(0, -3, 0), Vec3(0, 0, 0)) slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True) slider.setDebugDrawSize(2.0) slider.setLowerLinearLimit(0) slider.setUpperLinearLimit(6) slider.setLowerAngularLimit(-60) slider.setUpperAngularLimit(60) self.world.attachConstraint(slider)
def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(False) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Box A shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) bodyA = BulletRigidBodyNode('Box A') bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(-2, 0, 4) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(0, 0, 0) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Cone frameA = TransformState.makePosHpr(Point3(0, 0, -2), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(-5, 0, 0), Vec3(0, 0, 0)) cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB) cone.setDebugDrawSize(2.0) cone.setLimit(30, 45, 170, softness=1.0, bias=0.3, relaxation=8.0) self.world.attachConstraint(cone)
def _create_joint_constraint(self, joint): parent_bone = joint.parent_bone child_bone = joint.child_bone parent_frame_pos = Vec3(parent_bone.length + joint.gap_radius, 0, 0) child_frame_pos = Vec3(-child_bone.length - joint.gap_radius, 0, 0) parent_frame = TransformState.makePosHpr(parent_frame_pos, Vec3(*joint.parent_start_hpr)) child_frame = TransformState.makePosHpr(child_frame_pos, Vec3(*joint.child_start_hpr)) constraint = panda3d.bullet.BulletHingeConstraint( self.bones_to_nodes[parent_bone], self.bones_to_nodes[child_bone], parent_frame, child_frame) constraint.setLimit(*joint.angle_range) constraint.enableFeedback(False) self.world.attachConstraint(constraint) self.constraints.append(constraint)
def getPhysBody(self): bsph = BulletSphereShape(0.6) bcy = BulletCylinderShape(0.25, 0.35, ZUp) body = BulletRigidBodyNode('tntBody') body.addShape( bsph, TransformState.makePosHpr((0.05, 0, 0.43), (86.597, 24.5539, 98.1435))) body.addShape( bcy, TransformState.makePosHpr((0.05, 0.655, 0.35), (86.597, 24.5539, 98.1435))) body.setKinematic(True) body.setCcdMotionThreshold(1e-7) body.setCcdSweptSphereRadius(0.6) return body
def testInit(self): houseId = "0004d52d1aeeb8ae6de39d6bd993e992" scene = SunCgSceneLoader.loadHouseFromJson(houseId, SUNCG_DATA_DIR) agent = scene.agents[0] # Configure the agent transform = TransformState.makePosHpr(pos=LVector3f(38.42, -39.10, 1.70), hpr=LVector3f(-77.88, -13.93, 0.0)) agent.setTransform(transform) renderer = DepthRenderer(scene, size=(512, 512), fov=75.0) agentId = agent.getTag('agent-id') image = renderer.getDepthImage(agentId) fig = plt.figure(figsize=(8, 8)) plt.ion() plt.show() plt.axis("off") plt.imshow(image) plt.draw() plt.pause(1.0) plt.close(fig)
def set_bones_pos_hpr(self, positions, orientations): # position - n x 3 array for index, node in enumerate(self._get_ordered_bone_nodes()): transform = TransformState.makePosHpr(Vec3(*positions[index]), Vec3(*orientations[index])) node.setTransform(transform) node.setLinearVelocity(Vec3(0, 0, 0)) node.setAngularVelocity(Vec3(0, 0, 0))
def rect_region_detection(engine: EngineCore, position: Tuple, heading: float, heading_direction_length: float, side_direction_width: float, detection_group: int, height=10, in_static_world=False): """ ---------------------------------- | * | --->>> ---------------------------------- * position --->>> heading direction ------ longitude length | lateral width **CAUTION**: position is the middle point of longitude edge :param engine: BaseEngine class :param position: position in PGDrive :param heading: heading in PGDrive [degree] :param heading_direction_length: rect length in heading direction :param side_direction_width: rect width in side direction :param detection_group: which group to detect :param height: the detect will be executed from this height to 0 :param in_static_world: execute detection in static world :return: detection result """ region_detect_start = panda_position(position, z=height) region_detect_end = panda_position(position, z=-1) tsFrom = TransformState.makePosHpr(region_detect_start, Vec3(panda_heading(heading), 0, 0)) tsTo = TransformState.makePosHpr(region_detect_end, Vec3(panda_heading(heading), 0, 0)) shape = BulletBoxShape( Vec3(heading_direction_length / 2, side_direction_width / 2, 1)) penetration = 0.0 physics_world = engine.physics_world.dynamic_world if not in_static_world else engine.physics_world.static_world result = physics_world.sweep_test_closest(shape, tsFrom, tsTo, detection_group, penetration) return result
def createJoints(self): for jointDesc in self.joints: limbA = self.limbs[jointDesc.limbA] limbB = self.limbs[jointDesc.limbB] a = self.limbs[jointDesc.limbA].bodyNode b = self.limbs[jointDesc.limbB].bodyNode jointA = self.actorJoints[jointDesc.limbA].eNp jointB = self.actorJoints[jointDesc.limbB].eNp frame0 = TransformState.makePosHpr(jointDesc.axis0[0], jointDesc.axis0[1]) frame1 = TransformState.makePosHpr(jointDesc.axis1[0], jointDesc.axis1[1]) constraint = BulletConeTwistConstraint(a.node(), b.node(), frame0, frame1) constraint.setLimit(float(jointDesc.swing[0]), float(jointDesc.swing[1]), float(jointDesc.twist)) constraint.setEnabled(True) constraint.setDebugDrawSize(1.5) base.physicsWorld.attachConstraint(constraint)
def create_colliders(root, pose_rig, joints_config): for node, parent in pose_rig.exposed_joints: if node.getName() not in joints_config: continue joint_config = joints_config[node.getName()] if "joints" not in joint_config: continue joints = joint_config["joints"] if len(joints) == 0: continue mass = joint_config["mass"] if "mass" in joint_config else 1 box_rb = BulletRigidBodyNode(node.getName()) box_rb.setMass(1.0) # box_rb.setLinearDamping(0.2) # box_rb.setAngularDamping(0.9) # box_rb.setFriction(1.0) # box_rb.setAnisotropicFriction(1.0) # box_rb.setRestitution(0.0) for joint in joints: child_node, child_parent = next( (child_node, child_parent) for child_node, child_parent in pose_rig.exposed_joints if child_node.getName() == joint ) pos = child_node.getPos(child_parent) width = pos.length() / 2.0 scale = Vec3(3, width, 3) shape = BulletBoxShape(scale) quat = Quat() lookAt(quat, child_node.getPos(child_parent), Vec3.up()) if len(joints) > 1: transform = TransformState.makePosHpr(child_node.getPos(child_parent) / 2.0, quat.getHpr()) else: transform = TransformState.makeHpr(quat.getHpr()) box_rb.addShape(shape, transform) box_np = root.attachNewNode(box_rb) if len(joints) > 1: pos = node.getPos(pose_rig.actor) hpr = node.getHpr(pose_rig.actor) box_np.setPosHpr(root, pos, hpr) else: box_np.setPos(child_parent, child_node.getPos(child_parent) / 2.0) box_np.lookAt(child_parent, child_node.getPos(child_parent)) yield box_np
def criaPopulacao(self): vPos = 0 for n in range(0, self.POPULACAO_TREINAMENTO): indiv = Individuo() # TRONCO shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) tronco = self.worldNP.attachNewNode(BulletRigidBodyNode('Box')) tronco.node().setMass(1.0) tronco.node().addShape(shape) tronco.setPos(vPos + 2, 0.5, 3) tronco.setScale(2, 0.6, 4) tronco.setCollideMask(BitMask32.allOn()) #self.boxNP.node().setDeactivationEnabled(False) indiv.tronco = tronco self.world.attachRigidBody(tronco.node()) visualNP = loader.loadModel('models/box.egg') visualNP.clearModelNodes() visualNP.reparentTo(tronco) frameA = TransformState.makePosHpr(Point3(0, 0, -2), Vec3(0, 0, 90)) # PÉS shape = BulletBoxShape(Vec3(0.5, 1, 0.2)) posicaoPes = [1, 2] for i in range(2): for j in range(1): x = i + posicaoPes[i] + vPos y = 0.0 z = j pe = self.criaPe(shape, x, y, z) if i == 1: indiv.pe1 = pe else: indiv.pe2 = pe # Cone # frameB = TransformState.makePosHpr(Point3(-5, 0, 0), Vec3(0, 0, 0)) # cone = BulletConeTwistConstraint(tronco, pe, frameA, frameB) # cone.setDebugDrawSize(2.0) # cone.setLimit(30, 45, 170, softness=1.0, bias=0.3, relaxation=8.0) # self.world.attachConstraint(cone) self.individuos.append(indiv) vPos = vPos + 4
def _create_bone_node(self, bone): box_shape = panda3d.bullet.BulletBoxShape( Vec3(bone.length, bone.height, bone.width)) # ts = TransformState.makePos(Point3(bone.length, bone.height, bone.width)) ts = TransformState.makePos(Point3(0, 0, 0)) bone_node = panda3d.bullet.BulletRigidBodyNode(bone.name) bone_node.setMass(bone.mass) bone_node.setFriction(bone.friction) bone_node.addShape(box_shape, ts) bone_node.setTransform( TransformState.makePosHpr(Vec3(*bone.start_pos), Vec3(*bone.start_hpr))) self.world.attachRigidBody(bone_node) self.bones_to_nodes[bone] = bone_node
def createLimbs(self): # For each limbs desc, create a bullet capsule for limb in self.limbs.values(): body = BulletRigidBodyNode("ragdoll-limb-" + limb.jointName) body.setMass(limb.mass) for i in range(len(limb.shapes)): shapeDesc = limb.shapes[i] capsule = BulletCapsuleShape(shapeDesc.radius, shapeDesc.length / 2.0, ZUp) body.addShape(capsule, TransformState.makePosHpr(shapeDesc.localPos, shapeDesc.localHpr)) jointDesc = self.actorJoints[limb.jointName] eNp = jointDesc.eNp jointDesc.limb = limb bodyNp = NodePath(body) bodyNp.reparentTo(render) bodyNp.setTransform(render, eNp.getTransform(render)) bodyNp.node().setTransformDirty() base.physicsWorld.attachRigidBody(body) limb.bodyNode = bodyNp
def insert(self, world, render, i, pos): # Important numbers head_radius = 0.5 head_elevation = 1.5 torso_x = 0.3 torso_y = 0.5 torso_z = 0.75 arm_radius = 0.15 shoulder_space = 0.05 shoulder_elevation = head_elevation - head_radius - 0.1 - arm_radius torso_elevation = head_elevation - head_radius - torso_z x, y = pos # measurements below are in degrees neck_yaw_limit = 90 neck_pitch_limit = 45 shoulder_twist_limit = 90 # limit for twisting arm along the bicep axis shoulder_in_limit = 175 # maximum declination from T-pose towards torso shoulder_out_limit = 90 # maximum elevation from T-pose away from torso shoulder_forward_limit = 175 # maximum angle from down by side to pointing forward shoulder_backward_limit = 90 # maximum angle from down by side to pointing backward # Create a head head_node = BulletRigidBodyNode('Head') head_node.addShape(BulletSphereShape(head_radius)) head_node.setMass(1.0) head_pointer = render.attachNewNode(head_node) head_pointer.setPos(x, y, head_elevation) world.attachRigidBody(head_node) # Create a torso torso_node = BulletRigidBodyNode('Torso') torso_node.addShape(BulletBoxShape(Vec3(torso_x, torso_y, torso_z))) torso_node.setMass(0.0) # remain in place torso_pointer = render.attachNewNode(torso_node) torso_pointer.setPos(x, y, torso_elevation) world.attachRigidBody(torso_node) # Attach the head to the torso head_frame = TransformState.makePosHpr(Point3(0, 0, -head_radius), Vec3(0, 0, -90)) torso_frame = TransformState.makePosHpr(Point3(0, 0, torso_z), Vec3(0, 0, -90)) neck = BulletConeTwistConstraint(head_node, torso_node, head_frame, torso_frame) neck.setDebugDrawSize(0.5) neck.setLimit(neck_pitch_limit, neck_pitch_limit, neck_yaw_limit) world.attachConstraint(neck) # Create arms shoulder_pos_l = Point3( x, y - i * (torso_y + shoulder_space + arm_radius), shoulder_elevation) shoulder_pos_r = Point3( x, y + i * (torso_y + shoulder_space + arm_radius), shoulder_elevation) limits = (shoulder_in_limit, shoulder_out_limit, shoulder_forward_limit, shoulder_backward_limit, shoulder_twist_limit) arm_l = Arm(world, render, shoulder_pos_l, -i, LEFT, torso_pointer, limits) arm_r = Arm(world, render, shoulder_pos_r, i, RIGHT, torso_pointer, limits) self.head = head_pointer self.torso = torso_pointer self.arm_l, self.arm_r = arm_l, arm_r
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
def setup(self): #self.targetAlt = r.randrange(100,300) self.Valves = np.array([0.15, 0.2, 0.15]) self.EngObs = self.vulcain.predict_data_point( np.array(self.Valves).reshape(1, -1)) if self.VISUALIZE is True: self.worldNP = self.render.attachNewNode('World') else: self.root = NodePath(PandaNode("world root")) self.worldNP = self.root.attachNewNode('World') self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.80665)) # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(True) self.debugNP.show() self.world.setDebugNode(self.debugNP.node()) # self.debugNP.showTightBounds() # self.debugNP.showBounds() # Ground (static) shape = BulletPlaneShape(Vec3(0, 0, 1), 1) self.groundNP = self.worldNP.attachNewNode( BulletRigidBodyNode('Ground')) self.groundNP.node().addShape(shape) self.groundNP.setPos(0, 0, 0) self.groundNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(self.groundNP.node()) # Rocket shape = BulletCylinderShape(self.radius, self.length, ZUp) self.rocketNP = self.worldNP.attachNewNode( BulletRigidBodyNode('Cylinder')) self.rocketNP.node().setMass(self.drymass + self.fuelAmount_LH2 + self.fuelAmount_LOX) self.rocketNP.node().addShape(shape) #self.rocketNP.setPos(20,20,250) self.rocketNP.setPos(r.randrange(-200, 200), 20, 350) #r.randrange(300, 500)) #self.rocketNP.setPos(r.randrange(-self.lateralError, self.lateralError, 1), r.randrange(-self.lateralError, self.lateralError, 1), self.height) # self.rocketNP.setPos(0, 0, self.length*10) self.rocketNP.setCollideMask(BitMask32.allOn()) # self.rocketNP.node().setCollisionResponse(0) self.rocketNP.node().notifyCollisions(True) self.world.attachRigidBody(self.rocketNP.node()) for i in range(4): leg = BulletCylinderShape(0.1 * self.radius, 0.5 * self.length, XUp) self.rocketNP.node().addShape( leg, TransformState.makePosHpr( Vec3(6 * self.radius * math.cos(i * math.pi / 2), 6 * self.radius * math.sin(i * math.pi / 2), -0.6 * self.length), Vec3(i * 90, 0, 30))) shape = BulletConeShape(0.75 * self.radius, 0.5 * self.radius, ZUp) self.rocketNP.node().addShape( shape, TransformState.makePosHpr(Vec3(0, 0, -1 / 2 * self.length), Vec3(0, 0, 0))) self.npThrustForce = LineNodePath(self.rocketNP, 'Thrust', thickness=4, colorVec=Vec4(1, 0.5, 0, 1)) self.npDragForce = LineNodePath(self.rocketNP, 'Drag', thickness=4, colorVec=Vec4(1, 0, 0, 1)) self.npLiftForce = LineNodePath(self.rocketNP, 'Lift', thickness=4, colorVec=Vec4(0, 0, 1, 1)) self.rocketCSLon = self.radius**2 * math.pi self.rocketCSLat = self.length * 2 * self.radius if self.VISUALIZE is True: self.terrain = loader.loadModel("../LZGrid2.egg") self.terrain.setScale(10) self.terrain.reparentTo(self.render) self.terrain.setColor(Vec4(0.1, 0.2, 0.1, 1)) self.toggleTexture()
def __init__(self,info): # animation base setup self.character_info_ = info size = Vec3(info.width, AnimationActor.DEFAULT_WIDTH , info.height) AnimatableObject.__init__(self,info.name,size,info.mass) self.animation_root_np_.setPos(Vec3(0,0,0)) # constraints self.left_constraint_ = None self.right_constraint_ = None self.selected_constraint_ = None # removing default shapes from rigid body shapes = self.node().getShapes() for shape in shapes: self.node().removeShape(shape) # adding capsule shape radius = 0.5*size.getX() height = size.getZ() - 2.0*radius # height of cylindrical part only height = height if height >= 0 else 0.0 bullet_shape = BulletCapsuleShape(radius, height, ZUp) bullet_shape.setMargin(GameObject.DEFAULT_COLLISION_MARGIN) self.node().addShape(bullet_shape,TransformState.makePos(Vec3(0,0,0.5*size.getZ()))) # box bottom center coincident with the origin self.node().setMass(self.character_info_.mass) self.node().setLinearFactor((1,1,1)) self.node().setAngularFactor((0,0,0)) self.setCollideMask(CollisionMasks.NO_COLLISION) # setting bounding volume min = LPoint3(-0.5*size.getX(),-0.5*size.getY(),0) max = LPoint3(0.5*size.getX(),0.5*size.getY(),size.getZ()) self.node().setBoundsType(BoundingVolume.BT_box) self.node().setBounds(BoundingBox(min,max)) # setting origin ghost nodes rel_pos = Vec3(-GameObject.ORIGIN_XOFFSET,0,info.height/2) self.left_origin_gn_ = self.attachNewNode(BulletGhostNode(self.getName() + '-left-origin')) self.left_origin_gn_.node().addShape(BulletSphereShape(GameObject.ORIGIN_SPHERE_RADIUS),TransformState.makePosHpr(rel_pos,Vec3.zero())) self.left_origin_gn_.node().setIntoCollideMask(CollisionMasks.ACTION_TRIGGER_0 if not self.isFacingRight() else CollisionMasks.NO_COLLISION) rel_pos = Vec3(GameObject.ORIGIN_XOFFSET,0,info.height/2) self.right_origin_gn_ = self.attachNewNode(BulletGhostNode(self.getName() + '-right-origin')) self.right_origin_gn_.node().addShape(BulletSphereShape(GameObject.ORIGIN_SPHERE_RADIUS),TransformState.makePosHpr(rel_pos,Vec3.zero())) self.right_origin_gn_.node().setIntoCollideMask(CollisionMasks.ACTION_TRIGGER_0 if self.isFacingRight() else CollisionMasks.NO_COLLISION) # character status self.state_data_ = CharacterStateData() # state machine self.sm_ = StateMachine() # motion commander self.motion_commander_ = MotionCommander(self) # set id self.setObjectID(self.getName())
def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(True) #self.debugNP.showTightBounds() #self.debugNP.showBounds() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Ground (static) shape = BulletPlaneShape(Vec3(0, 0, 1), 1) self.groundNP = self.worldNP.attachNewNode( BulletRigidBodyNode('Ground')) self.groundNP.node().addShape(shape) self.groundNP.setPos(0, 0, -2) self.groundNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(self.groundNP.node()) #Rocket shape = BulletCylinderShape(0.2 * self.scale, 2 * self.scale, ZUp) self.rocketNP = self.worldNP.attachNewNode( BulletRigidBodyNode('Cylinder')) self.rocketNP.node().setMass(3.0) self.rocketNP.node().addShape(shape) self.rocketNP.setPos(0, 0, 2 * self.scale) self.rocketNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(self.rocketNP.node()) for i in range(4): leg = BulletCylinderShape(0.02 * self.scale, 1 * self.scale, XUp) self.rocketNP.node().addShape( leg, TransformState.makePosHpr( Vec3(0.6 * self.scale * math.cos(i * math.pi / 2), 0.6 * self.scale * math.sin(i * math.pi / 2), -1.2 * self.scale), Vec3(i * 90, 0, 30))) shape = BulletConeShape(0.15 * self.scale, 0.3 * self.scale, ZUp) self.rocketNozzle = self.worldNP.attachNewNode( BulletRigidBodyNode('Cone')) self.rocketNozzle.node().setMass(1) self.rocketNozzle.node().addShape(shape) self.rocketNozzle.setPos(0, 0, 0.8 * self.scale) self.rocketNozzle.setCollideMask(BitMask32.allOn()) self.rocketNozzle.node().setCollisionResponse(0) self.world.attachRigidBody(self.rocketNozzle.node()) frameA = TransformState.makePosHpr(Point3(0, 0, -1 * self.scale), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(0, 0, 0.2 * self.scale), Vec3(0, 0, 90)) self.cone = BulletConeTwistConstraint(self.rocketNP.node(), self.rocketNozzle.node(), frameA, frameB) self.cone.enableMotor(1) #self.cone.setMaxMotorImpulse(2) #self.cone.setDamping(1000) self.cone.setDebugDrawSize(2.0) self.cone.setLimit(20, 20, 0, softness=1.0, bias=1.0, relaxation=10.0) self.world.attachConstraint(self.cone) self.npThrustForce = LineNodePath(self.render, 'Thrust', thickness=4, colorVec=VBase4(1, 0.5, 0, 1))
def create_constraints(root, joint_pairs, offset_scale): for joint_config, parent, child in joint_pairs: rb_parent = parent.node() rb_child = child.node() extents_parent = rb_parent.get_shape(0).getHalfExtentsWithMargin() extents_child = rb_child.get_shape(0).getHalfExtentsWithMargin() if "offset_parent" in joint_config: offset_parent = Point3(joint_config["offset_parent"]) else: offset_parent = (0, 1, 0) offset_parent_x, offset_parent_y, offset_parent_z = offset_parent offset_parent = Point3( offset_parent_x * extents_parent.getX(), offset_parent_y * extents_parent.getY(), offset_parent_z * extents_parent.getZ(), ) if "offset_child" in joint_config: offset_child = Point3(*joint_config["offset_child"]) else: offset_child = (0, -1, 0) offset_child_x, offset_child_y, offset_child_z = offset_child offset_child = Point3( offset_child_x * extents_child.getX(), offset_child_y * extents_child.getY(), offset_child_z * extents_child.getZ(), ) offset_parent = offset_parent * offset_scale offset_child = offset_child * offset_scale if joint_config["type"] == "hinge": axis_parent = Vec3(*joint_config["axis_parent"]) axis_child = Vec3(*joint_config["axis_child"]) constraint = BulletHingeConstraint( rb_parent, rb_child, offset_parent, offset_child, axis_parent, axis_child ) if "limit" in joint_config: low, high = joint_config["limit"] softness = 1.0 bias = 0.3 relaxation = 1.0 constraint.setLimit(low, high, softness, bias, relaxation) elif joint_config["type"] == "cone": frame_parent = TransformState.makePosHpr(offset_parent, Vec3(90, 0, 0)) frame_child = TransformState.makePosHpr(offset_child, Vec3(90, 0, 0)) constraint = BulletConeTwistConstraint(rb_parent, rb_child, frame_parent, frame_child) swing1, swing2, twist = joint_config["limit"] constraint.setLimit(swing1, swing2, twist) elif joint_config["type"] == "spherical": constraint = BulletSphericalConstraint(rb_parent, rb_child, offset_parent, offset_child) # constraint.setDebugDrawSize(1.0) yield constraint
def setupObstacleFour(self, pos, scale, turn): #Start Here # Box A shape = BulletBoxShape(Vec3(0.01, 0.01, 0.01) * scale) bodyA = BulletRigidBodyNode('Box A') bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 4) #(0, 0, 4) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.01, 0.01, 0.01)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletSphereShape(0.5*scale) bodyB = BulletRigidBodyNode('Sphere B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(10.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 5) #(0, 0, 0.001) visNP = loader.loadModel('media/models/ball.egg') visNP.clearModelNodes() visNP.setScale(1.25*scale) visNP.reparentTo(bodyNP) bodyNP.node().setLinearVelocity(100) self.world.attachRigidBody(bodyB) # Cone frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(2, 0, 0)*scale, Vec3(0, 0, 0)) cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB) cone.setDebugDrawSize(2.0) cone.setLimit(30, 90, 270, softness=1.0, bias=0.3, relaxation=10.0) self.world.attachConstraint(cone) # Box C shape = BulletBoxShape(Vec3(0.1, 0.1, 1)*scale) bodyC = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 3) self.world.attachRigidBody(bodyC) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP)
def setup(self): self.targetAlt = r.randrange(100,300) self.Valves = np.array([0.15,0.2,0.15]) self.EngObs = self.vulcain.predict_data_point(np.array(self.Valves).reshape(1,-1)) if self.VISUALIZE is True: self.worldNP = self.render.attachNewNode('World') else: self.root = NodePath(PandaNode("world root")) self.worldNP = self.root.attachNewNode('World') self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.80665)) # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(True) self.debugNP.show() self.world.setDebugNode(self.debugNP.node()) # self.debugNP.showTightBounds() # self.debugNP.showBounds() # Ground (static) shape = BulletPlaneShape(Vec3(0, 0, 1), 1) self.groundNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground')) self.groundNP.node().addShape(shape) self.groundNP.setPos(0, 0, 0) self.groundNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(self.groundNP.node()) # Rocket shape = BulletCylinderShape(self.radius, self.length, ZUp) self.rocketNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Cylinder')) self.rocketNP.node().setMass(27200 * self.scale) self.rocketNP.node().addShape(shape) #self.rocketNP.setPos(20,20,250) self.rocketNP.setPos(20, 20, r.randrange(100, 300)+200) #self.rocketNP.setPos(r.randrange(-self.lateralError, self.lateralError, 1), r.randrange(-self.lateralError, self.lateralError, 1), self.height) # self.rocketNP.setPos(0, 0, self.length*10) self.rocketNP.setCollideMask(BitMask32.allOn()) # self.rocketNP.node().setCollisionResponse(0) self.rocketNP.node().notifyCollisions(True) self.world.attachRigidBody(self.rocketNP.node()) for i in range(4): leg = BulletCylinderShape(0.1 * self.radius, 0.5 * self.length, XUp) self.rocketNP.node().addShape(leg, TransformState.makePosHpr( Vec3(6 * self.radius * math.cos(i * math.pi / 2), 6 * self.radius * math.sin(i * math.pi / 2), -0.6 * self.length), Vec3(i * 90, 0, 30))) shape = BulletConeShape(0.75 * self.radius, 0.5 * self.radius, ZUp) self.rocketNP.node().addShape(shape, TransformState.makePosHpr(Vec3(0, 0, -1 / 2 * self.length), Vec3(0, 0, 0))) # Fuel self.fuelRadius = 0.9 * self.radius shape = BulletCylinderShape(self.fuelRadius, 0.01 * self.length, ZUp) self.fuelNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Cone')) self.fuelNP.node().setMass(self.fuelMass_full * self.fuelMass_init) self.fuelNP.node().addShape(shape) self.fuelNP.setPos(0, 0, self.rocketNP.getPos().getZ() - self.length * 0.5 * (1 - self.fuelMass_init)) self.fuelNP.setCollideMask(BitMask32.allOn()) self.fuelNP.node().setCollisionResponse(0) self.world.attachRigidBody(self.fuelNP.node()) frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90)) self.fuelSlider = BulletSliderConstraint(self.rocketNP.node(), self.fuelNP.node(), frameA, frameB, 1) self.fuelSlider.setTargetLinearMotorVelocity(0) self.fuelSlider.setDebugDrawSize(2.0) self.fuelSlider.set_lower_linear_limit(0) self.fuelSlider.set_upper_linear_limit(0) self.world.attachConstraint(self.fuelSlider) self.npThrustForce = LineNodePath(self.rocketNP, 'Thrust', thickness=4, colorVec=Vec4(1, 0.5, 0, 1)) self.npDragForce = LineNodePath(self.rocketNP, 'Drag', thickness=4, colorVec=Vec4(1, 0, 0, 1)) self.npLiftForce = LineNodePath(self.rocketNP, 'Lift', thickness=4, colorVec=Vec4(0, 0, 1, 1)) self.npFuelState = LineNodePath(self.fuelNP, 'Fuel', thickness=20, colorVec=Vec4(0, 1, 0, 1)) self.rocketCSLon = self.radius ** 2 * math.pi self.rocketCSLat = self.length * 2 * self.radius if self.VISUALIZE is True: self.terrain = loader.loadModel("LZGrid2.egg") self.terrain.setScale(10) self.terrain.reparentTo(self.render) self.terrain.setColor(Vec4(0.1, 0.2, 0.1, 1)) self.toggleTexture() #self.fuelNP.setPos(0, 0, self.rocketNP.getPos().getZ() - self.length * 0.4 * (1 - self.fuelMass_init)) for i in range(5): self.world.doPhysics(self.dt, 5, 1.0 / 180.0) self.fuelSlider.set_lower_linear_limit(-self.length * 0.5 * (1 - self.fuelMass_init)) self.fuelSlider.set_upper_linear_limit(self.length * 0.5 * (1 - self.fuelMass_init)) for i in range(100): self.world.doPhysics(self.dt, 5, 1.0 / 180.0) self.rocketNP.node().applyForce(Vec3(0,0,300000), Vec3(0, 0, 0))
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)