Python BulletWorld.contactTestPair方法代码示例

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import contactTestPair [as 别名]
class CollisionChecker(object):
    """
    check the collision of a robot

    """

    def __init__(self, robotmesh):
        """
        set up the collision checker

        :param robotmesh is an object of robotsim/robot.robotmesh

        author: weiwei
        date: 20170608
        """

        self.bulletworld = BulletWorld()
        self.robotmesh = robotmesh
        self.counter = 0

    def __isSglArmCollided(self, sglbullnodes):
        """
        check the self-collision of a single arm

        :param sglbullnodes: a list of bullnodes starting from arm base to end-effector
        :return:

        author: weiwei
        date: 20170608
        """

        # collisioncheck, the near three links are not examined
        nlink = len(sglbullnodes)
        for i in range(nlink):
            for k in range(i+3, nlink):
                result = self.bulletworld.contactTestPair(sglbullnodes[i], sglbullnodes[k])
                if result.getNumContacts():
                    return True
        return False

    def __isSglArmBdyCollided(self, sglbullnodes, bdybullnodes):
        """
        check the self-collision of a single arm

        :param sglbullnodes: a list of bullnodes starting from arm base to end-effector
        :param bdybullnodes: a list of bullnodes for robot body
        :return:

        author: weiwei
        date: 20170615
        """

        # collisioncheck, the near three links are not examined
        nlinkarm = len(sglbullnodes)
        nlinkbdy = len(bdybullnodes)
        for i in range(1, nlinkarm):
            for k in range(0, nlinkbdy):
                result = self.bulletworld.contactTestPair(sglbullnodes[i], bdybullnodes[k])
                if result.getNumContacts():
                    return True
        return False

    def __isDualArmCollided(self, sglbullnodesrgt, sglbullnodeslft):
        """
        check the self-collision of a single arm
        the current implementation only check hands

        :param sglbullnodesrgt: a list of bullnodes starting from base to end-effector
        :param sglbullnodeslft: a list of bullnodes starting from base to end-effector
        :return:

        author: weiwei
        date: 20170608
        """

        # collisioncheck, the near three links are not examined
        nlinkrgt = len(sglbullnodesrgt)
        nlinklft = len(sglbullnodeslft)
        for i in range(nlinkrgt-1, nlinkrgt):
            for k in range(nlinklft-1, nlinklft):
                result = self.bulletworld.contactTestPair(sglbullnodesrgt[i], sglbullnodeslft[k])
                if result.getNumContacts():
                    return True
        return False


    def isSelfCollided(self, robot):
        """
        check the collision of a single arm

        :param armid: 'lft' or 'rgt'
        :return:

        author: weiwei
        date: 20170608
        """

        dualmnps = self.robotmesh.genmnp_list(robot)
        # single arm check
        sglmnps = dualmnps[0]
#.........这里部分代码省略.........

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import contactTestPair [as 别名]

#.........这里部分代码省略.........

        self.mat4list = []
        self.icos = trimesh.creation.icosphere(icolevel)
        initmat4 = self.objnp.getMat()
        for vert in self.icos.vertices:
            self.mat4list.append([])
            self.objnp.lookAt(vert[0], vert[1], vert[2])
            ytozmat4 = Mat4.rotateMat(-90, self.objnp.getMat().getRow3(0))
            newobjmat4 = self.objnp.getMat()*ytozmat4
            for angle in angles:
                tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2))
                tmppandamat4 = newobjmat4*tmppandamat4
                self.mat4list[-1].append(tmppandamat4)
            self.objnp.setMat(initmat4)
        self.floatingposemat4 = [e for l in self.mat4list for e in l]

    def genHandPairs(self, base, loadser=False):
        self.handpairList = []
        if loadser is False:
            # hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
            # hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
            pairidlist = list(itertools.combinations(range(len(self.freegripids)), 2))
            print len(pairidlist)/5000+1
            for i in range(100,len(pairidlist),len(pairidlist)/5000+1):
            # for i0, i1 in pairidlist:
                i0, i1 = pairidlist[i]
                print i, len(pairidlist)
                self.hand0.setMat(pandanpmat4 = self.freegriprotmats[i0])
                self.hand0.setJawwidth(self.freegripjawwidth[i0])
                self.hand1.setMat(pandanpmat4 = self.freegriprotmats[i1])
                self.hand1.setJawwidth(self.freegripjawwidth[i1])
                hndbullnodei0 = cd.genCollisionMeshMultiNp(self.hand0.handnp, base.render)
                hndbullnodei1 = cd.genCollisionMeshMultiNp(self.hand1.handnp, base.render)
                result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1)
                if not result.getNumContacts():
                    self.handpairList.append([self.freegripids[i0], self.freegripids[i1]])
            pickle.dump(self.handpairList, open("tmp.pickle", mode="wb"))
        else:
            self.handpairList = pickle.load(open("tmp.pickle", mode="rb"))


    def genFPandGs(self, grids, icolevel=1, angles=[0,45,90,135,180,225,270,315]):
        """
        genterate floating poses and their grasps, this function leverages genPandaRotmat4 and transformGrips

        :param icolevel
        :param angles see genPandaRotmat4
        :return:

        author: weiwei
        date: 20170221
        """

        self.gridsfloatingposemat4s = []
        self.__genPandaRotmat4(icolevel, angles)
        for gridposition in grids:
            for posemat4 in self.floatingposemat4:
                tmpposemat4 = Mat4(posemat4)
                tmpposemat4.setRow(3, Vec3(gridposition[0], gridposition[1], gridposition[2]))
                self.gridsfloatingposemat4s.append(tmpposemat4)
        self.transformGrips()

    def saveToDB(self):
        """
        save the floatingposes and their grasps to the database

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import contactTestPair [as 别名]

#.........这里部分代码省略.........
                        lftcctn1 = dc.strToV3(resultrow[12])
                        icoassgrippairsnormals.append([[rgtcctn0, rgtcctn1], [lftcctn0, lftcctn1]])
                        icoassgrippairshndmat4s.append([dc.strToMat4(resultrow[6]), dc.strToMat4(resultrow[13])])
                        icoassgrippairsjawwidths.append([float(resultrow[7]), float(resultrow[14])])
                        icoassgrippairsidfreeairs.append([int(resultrow[8]), int(resultrow[15])])
                self.icoassgrippairsids.append(icoassgrippairsids)
                self.icoassgrippairscontacts.append(icoassgrippairscontacts)
                self.icoassgrippairsnormals.append(icoassgrippairsnormals)
                self.icoassgrippairshndmat4s.append(icoassgrippairshndmat4s)
                self.icoassgrippairsjawwidths.append(icoassgrippairsjawwidths)
                self.icoassgrippairsidfreeairs.append(icoassgrippairsidfreeairs)

    def __genHandPairs(self, base):
        self.handpairList = []
        self.__genFreeAssGrips()
        ass0gripcontacts, ass0gripnormals, ass0griprotmat4s, ass0gripjawwidth, ass0gripidfreeair = self.obj0grips
        ass1gripcontacts, ass1gripnormals, ass1griprotmat4s, ass1gripjawwidth, ass1gripidfreeair = self.obj1grips
        hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
        hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
        i0list = range(len(ass0gripidfreeair))
        i1list = range(len(ass1gripidfreeair))
        pairidlist = list(itertools.product(i0list, i1list))
        print len(pairidlist)/10000+1
        for i in range(0,len(pairidlist),len(pairidlist)/10000+1):
            # for i0, i1 in pairidlist:
            i0, i1 = pairidlist[i]
            print i, len(pairidlist)
            hand0.setMat(ass0griprotmat4s[i0])
            hand0.setJawwidth(ass0gripjawwidth[i0])
            hand1.setMat(ass1griprotmat4s[i1])
            hand1.setJawwidth(ass1gripjawwidth[i1])
            hndbullnodei0 = cd.genCollisionMeshMultiNp(hand0.handnp, base.render)
            hndbullnodei1 = cd.genCollisionMeshMultiNp(hand1.handnp, base.render)
            result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1)
            if not result.getNumContacts():
                self.handpairList.append([ass0gripidfreeair[i0], ass1gripidfreeair[i1]])
        # print ass0gripidfreeair
        # print self.icoass0gripidfreeair[0]
        # print ass1gripidfreeair
        # print self.icoass1gripidfreeair[0]
        # print self.handpairList
        # assert "compare"

    def __saveIKtoDB(self, idrobot):
        """
        saveupdated IK to DB
        this function is separated from updateDBwithIK for illustration

        :return:
        """

        for fpid in range(len(self.gridsfloatingposemat4s)):
            # right arm
            idarm = self.gdb.loadIdArm('rgt')
            for i, idicoass0grip in enumerate(self.icoass0gripids[fpid]):
                sql = "INSERT IGNORE INTO ikassemblyxgrips0(idrobot, idarm, idassemblyxgrips0, feasibility, \
                      feasibility_assdir) VALUES (%d, %d, %d, '%s', '%s')" \
                      % (idrobot, idarm, idicoass0grip, self.icoass0gripsik[fpid][i],
                         self.icoass0gripsik_retassdir[fpid][i])
                gdb.execute(sql)
            # left arm
            idarm = self.gdb.loadIdArm('lft')
            for i, idicoass1grip in enumerate(self.icoass1gripids[fpid]):
                sql = "INSERT IGNORE INTO ikassemblyxgrips1(idrobot, idarm, idassemblyxgrips1, feasibility, \
                      feasibility_assdir) VALUES (%d, %d, %d, '%s', '%s')" \
                      % (idrobot, idarm, idicoass1grip, self.icoass1gripsik[fpid][i],

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import contactTestPair [as 别名]

#.........这里部分代码省略.........
            image_warning.setTransparency(TransparencyAttrib.MAlpha)
            image_warning.hide()
            
            image_ok = OnscreenImage(image = '../data/Texture/signal_ok.png', pos=(-1 + i*w, 0, 0.85), parent=self._playing_node2d)
            image_ok.setScale(0.1)
            image_ok.setTransparency(TransparencyAttrib.MAlpha)
            image_ok.show()
            self.vidas_imgs.append((image_ok, image_warning))
            
        self._level_time_O = OnscreenText(text = '', pos = (0, 0.85), scale = 0.14, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0), parent=self._playing_node2d)
        
        
    def loadMap(self):
        if (self._modulos is not None):
            for m in self._modulos:
                m.remove()
        if (self._paneles is not None):
            for p in self._paneles:
                p.remove()
                
        self._tp = TiledParser("map"+str(self._num_lvl))
        self._modulos, self._paneles = self._tp.load_models(self.world, self._playing_node)
      
    def setAI(self):
        taskMgr.add(self.update, 'Update')
            
    def update(self, task):
        if (task.frame > 1):
            self.world.doPhysics(globalClock.getDt())
            self._level_time -= globalClock.getDt()
            self._level_time_O.setText(str(int(self._level_time)))
        
        for panel in self._paneles:
            contact = self.world.contactTestPair(self._player.getRBNode(), panel.getRBNode())
            if contact.getNumContacts() > 0:
                panel.manipulate()
                
        brokens = 0
        for panel in self._paneles:
            if panel.isBroken():
                brokens += 1
        #print brokens
        
        for i, vida_imgs in enumerate(self.vidas_imgs):
            if i < len(self.vidas_imgs) - brokens:
                vida_imgs[0].show()
                vida_imgs[1].hide()
            else:
                vida_imgs[0].hide()
                vida_imgs[1].show()
            
        if brokens > self.VIDAS:
            self.gameOver()
            return task.done
            
        if self._level_time <= 0:
            self._num_lvl += 1
            if self._num_lvl <= self._num_lvls:
                self.nextLevel()
            else:
                self.theEnd()
            return task.done
        return task.cont

    def gameOver(self):
        taskMgr.remove('player-task')

# 需要导入模块: from panda3d.bullet import BulletWorld [as 别名]
# 或者: from panda3d.bullet.BulletWorld import contactTestPair [as 别名]

#.........这里部分代码省略.........
        textNodePath.setScale(0.08)
        randNum = random.sample(range(0, 1500, 200), 6)

        # coins
        for i in range(6):
            randX = random.uniform(-3.0, 3.2)
            randY = float(randNum[i])
            shape = BulletSphereShape(0.3)
            coinNode = BulletGhostNode('Coin-' + str(i))
            coinNode.addShape(shape)
            np = self.render.attachNewNode(coinNode)
            np.setCollideMask(BitMask32.allOff())
            np.setPos(randX, randY, 2)

            # Adding sphere model
            sphereNp = loader.loadModel('models/smiley.egg')
            sphereNp_tex = loader.loadTexture("models/sky/coin_2_tex.jpg")
            sphereNp.setTexture(sphereNp_tex, 1)
            sphereNp.reparentTo(np)
            sphereNp.setScale(0.45)
            sphereNp.hprInterval(2.5, Vec3(360, 0, 0)).loop()

            self.world.attachGhost(coinNode)
            self.coins.append(coinNode)
            print "node name:" + str(coinNode.getName())

    def processContacts(self):
        for coin in self.coins:
            self.testWithSingleBody(coin)

        self.coinsCollected = len(self.dictOfCoins)

    def testWithSingleBody(self, secondNode):
        contactResult = self.world.contactTestPair(self.character, secondNode)

        if contactResult.getNumContacts() > 0:
            self.collectSound.play()
            for contact in contactResult.getContacts():
                cp = contact.getManifoldPoint()
                node0 = contact.getNode0()
                node1 = contact.getNode1()
                self.dictOfCoins[node1.getName()] = 1
                np = self.render.find(node1.getName())
                np.node().removeAllChildren()
                self.world.removeGhost(np.node())

    def setupObstacles(self):
        # Obstacle
        origin = Point3(2, 0, 0)
        size = Vec3(2, 2.75, 1.5)
        shape = BulletBoxShape(size * 0.55)
        randNum1 = random.sample(range(0, 1500, 300), 3)
        randNum2 = random.sample(range(0, 1500, 500), 3)
        for i in range(2):
            randX = random.uniform(-3.5, 3.5)
            randY = float(randNum1[i])
            pos = origin + size * i
            ObstacleNP = self.render.attachNewNode(BulletRigidBodyNode('Obstacle%i' % i))
            ObstacleNP.node().addShape(shape)
            ObstacleNP.node().setMass(1.0)
            ObstacleNP.setPos(randX, randY, 3)
            ObstacleNP.setCollideMask(BitMask32.allOn())

            modelNP = loader.loadModel('models/box.egg')
            modelNP_tex = loader.loadTexture("models/sky/milkyway_tex.jpg")
            modelNP.setTexture(modelNP_tex, 1)