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
Beispiel #2
0
    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)
Beispiel #4
0
    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)
Beispiel #5
0
    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)