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 __init__(self, cubeMapPath=None): yawOff = 198 self.sphere = loader.loadModel("InvertSphereBlend.egg") # self.sphere = loader.loadModel("InvertedSphere.egg") # Load a sphere with a radius of 1 unit and the faces directed inward. self.sphere.setTexGen(TextureStage.getDefault(), TexGenAttrib.MWorldPosition) self.sphere.setTexProjector(TextureStage.getDefault(), render, self.sphere) self.sphere.setTexTransform( TextureStage.getDefault(), TransformState.makeHpr(VBase3(yawOff, 0, 0))) # Create some 3D texture coordinates on the sphere. For more info on this, check the Panda3D manual. self.sphere.setPos((0, 0, 0)) self.sphere.setTexPos(TextureStage.getDefault(), 0, 0, 0) self.sphere.setTexScale(TextureStage.getDefault(), 1) # tex = loader.loadCubeMap(cubeMapPath) if cubeMapPath is None: cubeMapPath = renamer() tex = loader.loadCubeMap(cubeMapPath) self.sphere.setTexture(tex) # Load the cube map and apply it to the sphere. self.sphere.setLightOff() # Tell the sphere to ignore the lighting. self.sphere.setScale(10) # Increase the scale of the sphere so it will be larger than the scene. print self.sphere.getHpr() print self.sphere.getPos() self.sphere.reparentTo(render) # Reparent the sphere to render so you can see it. # result = self.sphere.writeBamFile(cubeMapPath.split('_#.tga')[0]+'.bam') print '/'.join(cubeMapPath.split('/')[:-1]) + '.bam' base.saveCubeMap('streetscene_cube_#.jpg', size=512) result = self.sphere.writeBamFile( '/'.join(cubeMapPath.split('/')[:-1]) + '.bam') # Save out the bam file. print(result)
def __setupConstraints__(self,actor_rigid_body_np): self.__cleanupConstraints__() self.left_constraint_ = BulletGenericConstraint(self.node(), actor_rigid_body_np.node(), TransformState.makeIdentity(), TransformState.makeHpr(Vec3(180,0,0)),False) self.right_constraint_ = BulletGenericConstraint(self.node(), actor_rigid_body_np.node(), TransformState.makeIdentity(), TransformState.makeIdentity(),False) self.left_constraint_.setEnabled(False) self.right_constraint_.setEnabled(False) for axis in range(0,6): self.left_constraint_.setParam(BulletGenericConstraint.CP_cfm,0,axis) self.right_constraint_.setParam(BulletGenericConstraint.CP_cfm,0,axis)
def __init__(self,cubeMapPath=None): yawOff=198 self.sphere = loader.loadModel("InvertSphereBlend.egg") # self.sphere = loader.loadModel("InvertedSphere.egg") # Load a sphere with a radius of 1 unit and the faces directed inward. self.sphere.setTexGen(TextureStage.getDefault(), TexGenAttrib.MWorldPosition) self.sphere.setTexProjector(TextureStage.getDefault(), render, self.sphere) self.sphere.setTexTransform(TextureStage.getDefault(), TransformState.makeHpr(VBase3(yawOff, 0, 0))) # Create some 3D texture coordinates on the sphere. For more info on this, check the Panda3D manual. self.sphere.setPos((0,0,0)) self.sphere.setTexPos(TextureStage.getDefault(), 0, 0, 0) self.sphere.setTexScale(TextureStage.getDefault(), 1) # tex = loader.loadCubeMap(cubeMapPath) if cubeMapPath is None: cubeMapPath=renamer() tex = loader.loadCubeMap(cubeMapPath) self.sphere.setTexture(tex) # Load the cube map and apply it to the sphere. self.sphere.setLightOff() # Tell the sphere to ignore the lighting. self.sphere.setScale(10) # Increase the scale of the sphere so it will be larger than the scene. print self.sphere.getHpr() print self.sphere.getPos() self.sphere.reparentTo(render) # Reparent the sphere to render so you can see it. # result = self.sphere.writeBamFile(cubeMapPath.split('_#.tga')[0]+'.bam') print '/'.join(cubeMapPath.split('/')[:-1])+'.bam' base.saveCubeMap('streetscene_cube_#.jpg', size=512) result = self.sphere.writeBamFile('/'.join(cubeMapPath.split('/')[:-1])+'.bam') # Save out the bam file. print(result)
def control(self,ValveCommands): self.gimbalX = 0 self.gimbalY = 0 self.Valves = ValveCommands-(ValveCommands-self.Valves)*np.exp(-self.dt/self.tau) self.EngObs = self.vulcain.predict_data_point(np.array(self.Valves).reshape(1,-1 )) #Brennkammerdruck, Gaskammertemp, H2Massenstrom, LOX MAssentrom, Schub #Bar,K,Kg/s,Kg/s,kN #self.dt = globalClock.getDt() pos = self.rocketNP.getPos() vel = self.rocketNP.node().getLinearVelocity() quat = self.rocketNP.getTransform().getQuat() Roll, Pitch, Yaw = quat.getHpr() rotVel = self.rocketNP.node().getAngularVelocity() # CHECK STATE if self.fuelMass <= 0: self.EMPTY = True #if pos.getZ() <= 36: # self.LANDED = True self.LANDED = False self.processContacts() P, T, rho = air_dens(pos[2]) rocketZWorld = quat.xform(Vec3(0, 0, 1)) AoA = math.acos(min(max(dot(norm(vel), norm(-rocketZWorld)), -1), 1)) dynPress = 0.5 * dot(vel, vel) * rho dragArea = self.rocketCSLon + (self.rocketCSLat - self.rocketCSLon) * math.sin(AoA) drag = norm(-vel) * dynPress * self.Cd * dragArea time = globalClock.getFrameTime() liftVec = norm(vel.project(rocketZWorld) - vel) if AoA > 0.5 * math.pi: liftVec = -liftVec lift = liftVec * (math.sin(AoA * 2) * self.rocketCSLat * dynPress) if self.CONTROL: self.throttle = self.heightPID.control(pos.getZ(), vel.getZ(), 33) pitchTgt = self.XPID.control(pos.getX(), vel.getX(), 0) self.gimbalX = -self.pitchPID.control(Yaw, rotVel.getY(), pitchTgt) rollTgt = self.YPID.control(pos.getY(), vel.getY(), 0) self.gimbalY = -self.rollPID.control(Pitch, rotVel.getX(), -rollTgt) self.thrust = self.EngObs[0][4]*1000 #print(self.EngObs) quat = self.rocketNP.getTransform().getQuat() quatGimbal = TransformState.makeHpr(Vec3(0, self.gimbalY, self.gimbalX)).getQuat() thrust = quatGimbal.xform(Vec3(0, 0, self.thrust)) thrustWorld = quat.xform(thrust) #print(thrustWorld) self.npDragForce.reset() self.npDragForce.drawArrow2d(Vec3(0, 0, 0), quat.conjugate().xform(drag) / 1000, 45, 2) self.npDragForce.create() self.npLiftForce.reset() self.npLiftForce.drawArrow2d(Vec3(0, 0, 0), quat.conjugate().xform(lift) / 1000, 45, 2) self.npLiftForce.create() self.npThrustForce.reset() if self.EMPTY is False & self.LANDED is False: self.npThrustForce.drawArrow2d(Vec3(0, 0, -0.5 * self.length), Vec3(0, 0, -0.5 * self.length) - thrust / 30000, 45, 2) self.npThrustForce.create() self.rocketNP.node().applyForce(drag, Vec3(0, 0, 0)) self.rocketNP.node().applyForce(lift, Vec3(0, 0, 0)) #print(self.EMPTY,self.LANDED) if self.EMPTY is False & self.LANDED is False: self.rocketNP.node().applyForce(thrustWorld, quat.xform(Vec3(0, 0, -1 / 2 * self.length))) self.updateRocket(self.EngObs[0][2]+self.EngObs[0][3], self.dt) self.rocketNP.node().setActive(True) self.fuelNP.node().setActive(True) self.processInput() self.world.doPhysics(self.dt) self.steps+=1 if self.steps > 1000: self.DONE = True telemetry = [] telemetry.append('Thrust: {}'.format(int(self.EngObs[0][4]))) telemetry.append('Fuel: {}%'.format(int(self.fuelMass / self.fuelMass_full * 100.0))) telemetry.append('Gimbal: {}'.format(int(self.gimbalX)) + ',{}'.format(int(self.gimbalY))) telemetry.append('AoA: {}'.format(int(AoA / math.pi * 180.0))) telemetry.append('\nPos: {},{}'.format(int(pos.getX()), int(pos.getY()))) telemetry.append('Height: {}'.format(int(pos.getZ()))) telemetry.append('RPY: {},{},{}'.format(int(Roll), int(Pitch), int(Yaw))) telemetry.append('Speed: {}'.format(int(np.linalg.norm(vel)))) telemetry.append('Vel: {},{},{}'.format(int(vel.getX()), int(vel.getY()), int(vel.getZ()))) telemetry.append('Rot: {},{},{}'.format(int(rotVel.getX()), int(rotVel.getY()), int(rotVel.getZ()))) telemetry.append('LANDED: {}'.format(self.LANDED)) telemetry.append('Time: {}'.format(self.steps*self.dt)) telemetry.append('TARGET: {}'.format(self.targetAlt)) #print(pos) if self.VISUALIZE is True: self.ostData.setText('\n'.join(telemetry)) self.cam.setPos(pos[0] - 120 * self.scale, pos[1] - 120 * self.scale, pos[2] + 80 * self.scale) self.cam.lookAt(pos[0], pos[1], pos[2]) self.taskMgr.step()
def createConstraints(self,bodyA,bodyB): left_constraint = BulletGenericConstraint(bodyA,bodyB,TransformState.makeIdentity(),TransformState.makeHpr(Vec3(180,0,0)),False) right_constraint = BulletGenericConstraint(bodyA,bodyB,TransformState.makeIdentity(),TransformState.makeIdentity(),False) left_constraint.setEnabled(False) right_constraint.setEnabled(False) return (right_constraint,left_constraint)