def grpshow(self, base, gdb): sql = "SELECT tabletopplacements.idtabletopplacements, tabletopplacements.rotmat \ FROM tabletopplacements,freetabletopplacement,object,angle WHERE \ tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \ freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s' AND \ tabletopplacements.idangle = angle.idangle AND \ freetabletopplacement.idfreetabletopplacement = %d AND angle.value = %d" % (self.dbobjname, 1, 45) result = gdb.execute(sql) if len(result) != 0: for resultrow in result: idtabletopplacements = int(resultrow[0]) objrotmat = dc.strToMat4(resultrow[1]) objpos = objrotmat.getRow3(3) base.changeLookAt(lookatp=[objpos[0],objpos[1],objpos[2]]) # show object geom = pg.packpandageom_fn(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) node = GeomNode('obj') node.addGeom(geom) star = NodePath('obj') star.attachNewNode(node) star.setColor(Vec4(.77,.67,0,1)) star.setTransparency(TransparencyAttrib.MAlpha) star.setMat(objrotmat) star.reparentTo(base.render) sql = "SELECT tabletopgrips.rotmat, tabletopgrips.jawwidth FROM tabletopgrips WHERE \ tabletopgrips.idtabletopplacements=%d" % idtabletopplacements result = gdb.execute(sql) for resultrow in result: hndrotmat = dc.strToMat4(resultrow[0]) hndjawwidth = float(resultrow[1]) # show grasps tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .1]) tmphnd.setMat(pandanpmat4 = hndrotmat) # tmphnd.setMat(npmat4=hndrotmat) tmphnd.setJawwidth(hndjawwidth) tmphnd.reparentTo(base.render)
def removeFgrpccShow(self, base): """ Fgrpcc means finger pre collision detection This one is specially written for demonstration :return: author: weiwei date: 20161201, osaka """ # 6 is used because I am supposing 4+2 where 4 is the default # margin of bullet in panda3d. (NOTE: This is a guess) plotoffsetfp = 6 npbrchild = base.render.find("**/tempplot") if npbrchild: # npbrchild.removeNode() pass # for fast delete brchild = NodePath('tempplot') brchild.reparentTo(base.render) self.counter += 1 if self.counter >= self.facetpairs.shape[0]: self.counter = 0 facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] geomfacet0 = pandageom.packpandageom_fn( self.objtrimesh.vertices + np.tile(plotoffsetfp * self.facetnormals[facetidx0], [self.objtrimesh.vertices.shape[0], 1]), self.objtrimesh.face_normals[self.facets[facetidx0]], self.objtrimesh.faces[self.facets[facetidx0]]) geomfacet1 = pandageom.packpandageom_fn( self.objtrimesh.vertices + np.tile(plotoffsetfp * self.facetnormals[facetidx1], [self.objtrimesh.vertices.shape[0], 1]), self.objtrimesh.face_normals[self.facets[facetidx1]], self.objtrimesh.faces[self.facets[facetidx1]]) # show the facetpair node0 = GeomNode('pair0') node0.addGeom(geomfacet0) star0 = NodePath('pair0') star0.attachNewNode(node0) facetcolorarray = self.facetcolorarray star0.setColor( Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1], facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3])) star0.setTwoSided(True) star0.reparentTo(brchild) node1 = GeomNode('pair1') node1.addGeom(geomfacet1) star1 = NodePath('pair1') star1.attachNewNode(node1) star1.setColor( Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1], facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3])) star1.setTwoSided(True) star1.reparentTo(brchild) for j, contactpair in enumerate(self.gripcontactpairs[self.counter]): cctpnt0 = contactpair[ 0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[ 1] + plotoffsetfp * self.facetnormals[facetidx1] # the following two choices decide the way to detect contacts cctnormal00 = np.array( self.gripcontactpairnormals[self.counter][j][0]) cctnormal01 = -np.array( self.gripcontactpairnormals[self.counter][j][1]) cctnormal0raw = (cctnormal00 + cctnormal01) cctnormal0 = (cctnormal0raw / np.linalg.norm(cctnormal0raw)).tolist() # the following two choices decide the way to detect contacts cctnormal10 = -cctnormal00 cctnormal11 = -cctnormal01 cctnormal1raw = (cctnormal10 + cctnormal11) cctnormal1 = (cctnormal1raw / np.linalg.norm(cctnormal1raw)).tolist() handfgrpcc0 = NodePath("handfgrpcc0") self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0) handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2]) handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0], cctpnt0[1] + cctnormal0[1], cctpnt0[2] + cctnormal0[2]) handfgrpcc1 = NodePath("handfgrpcc1") self.handfgrpcc_uninstanced.instanceTo(handfgrpcc1) handfgrpcc1.setPos(cctpnt1[0], cctpnt1[1], cctpnt1[2]) handfgrpcc1.lookAt(cctpnt1[0] + cctnormal1[0], cctpnt1[1] + cctnormal1[1], cctpnt1[2] + cctnormal1[2]) handfgrpcc = NodePath("handfgrpcc") handfgrpcc0.reparentTo(handfgrpcc) handfgrpcc1.reparentTo(handfgrpcc) # prepare the model for collision detection facetmeshbullnode = cd.genCollisionMeshMultiNp(handfgrpcc, brchild) result = self.bulletworld.contactTest(facetmeshbullnode) for contact in result.getContacts(): cp = contact.getManifoldPoint() base.pggen.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1)) base.pggen.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1)) if result.getNumContacts(): handfgrpcc0.setColor(1, 0, 0, .3) handfgrpcc1.setColor(1, 0, 0, .3) else: handfgrpcc0.setColor(1, 1, 1, .3) handfgrpcc1.setColor(1, 1, 1, .3) handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha) handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha) handfgrpcc0.reparentTo(brchild) handfgrpcc1.reparentTo(brchild) base.pggen.plotArrow(star0, spos=cctpnt0, epos=cctpnt0 + plotoffsetfp * self.facetnormals[facetidx0] + cctnormal0, rgba=[ facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1], facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3] ], length=10) base.pggen.plotArrow(star1, spos=cctpnt1, epos=cctpnt1 + plotoffsetfp * self.facetnormals[facetidx1] + cctnormal1, rgba=[ facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1], facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3] ], length=10)
def __init__(self, objpath, handpkg, readser=False, torqueresist=50, dotnormplan=.95, dotnoarmovlp=.95, dotnormpara=-.75): """ initialization :param objpath: path of the object :param ser: True use pre-computed template file for debug (in order to debug large models like tool.stl :param torqueresist: the maximum allowable distance to com (see FreegripContactpairs.planContactpairs) author: weiwei date: 20161201, osaka """ super(self.__class__, self).__init__(objpath, dotnormplan=dotnormplan, dotnoarmovlp=dotnoarmovlp, readser=readser) if readser is False: tic = time.time() self.removeBadSamples(mindist=2, maxdist=20) toc = time.time() print "remove bad sample cost", toc - tic tic = time.time() self.clusterFacetSamplesRNN(reduceRadius=10) toc = time.time() print "cluster samples cost", toc - tic tic = time.time() self.planContactpairs(torqueresist, dotnormpara=dotnormpara) toc = time.time() print "plan contact pairs cost", toc - tic self.saveSerialized("tmpcp.pickle") else: self.loadSerialized("tmpcp.pickle", objpath) self.handpkg = handpkg self.hand = handpkg.newHandNM(hndcolor=[0, 1, 0, .1]) self.handfgrpcc_uninstanced = handpkg.newHandFgrpcc() self.handname = handpkg.getHandName() # gripcontactpairs_precc is the gripcontactpairs ([[p0,p1,p2],[p0',p1',p2']] pairs) after precc (collision free) # gripcontactpairnormals_precc is the gripcontactpairnormals ([[n0,n1,n2],[n0',n1',n2']] pairs) after precc # likewise, gripcontactpairfacets_precc is the [faceid0, faceid1] pair corresponding to the upper two self.gripcontactpairs_precc = None self.gripcontactpairnormals_precc = None self.gripcontactpairfacets_precc = None # the final results: gripcontacts: a list of [cct0, cct1] # griprotmats: a list of Mat4 # gripcontactnormals: a list of [nrml0, nrml1] self.gripcontacts = None self.griprotmats = None self.gripjawwidth = None self.gripcontactnormals = None self.bulletworld = BulletWorld() # prepare the model for collision detection self.objgeom = pandageom.packpandageom_fn(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) print "number of vertices", len(self.objtrimesh.vertices) print "number of faces", len(self.objtrimesh.faces) self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom) self.bulletworld.attachRigidBody(self.objmeshbullnode) # for plot self.rtq85plotlist = [] self.counter2 = 0 # for dbupdate self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
def pairShow(self, base, togglecontacts = False, togglecontactnormals = False): # the following sentence requires segshow to be executed first facetcolorarray = self.facetcolorarray # offsetfp = facetpair # plotoffsetfp = np.random.random()*50 plotoffsetfp = 0.0 # plot the pairs and their contacts # for i in range(self.counter+1, len(self.facetpairs)): # if self.gripcontactpairs[i]: # self.counter = i # break # if i is len(self.facetpairs): # return # delete the facetpair after show # np0 = base.render.find("**/pair0") # if np0: # np0.removeNode() # np1 = base.render.find("**/pair1") # if np1: # np1.removeNode() self.counter += 1 if self.counter >= self.facetpairs.shape[0]: # self.counter = 0 return n = 30 if self.counter > n or self.counter <= n-1: # self.counter = 0 return facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] geomfacet0 = pandageom.packpandageom_fn(self.objtrimesh.vertices+ np.tile(plotoffsetfp*self.facetnormals[facetidx0], [self.objtrimesh.vertices.shape[0],1]), self.objtrimesh.face_normals[self.facets[facetidx0]], self.objtrimesh.faces[self.facets[facetidx0]]) geomfacet1 = pandageom.packpandageom_fn(self.objtrimesh.vertices+ np.tile(plotoffsetfp*self.facetnormals[facetidx1], [self.objtrimesh.vertices.shape[0],1]), self.objtrimesh.face_normals[self.facets[facetidx1]], self.objtrimesh.faces[self.facets[facetidx1]]) # show the facetpair node0 = GeomNode('pair0') node0.addGeom(geomfacet0) star0 = NodePath('pair0') star0.attachNewNode(node0) star0.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1], facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3])) star0.setTwoSided(True) star0.reparentTo(base.render) node1 = GeomNode('pair1') node1.addGeom(geomfacet1) star1 = NodePath('pair1') star1.attachNewNode(node1) # star1.setColor(Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1], # facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3])) # set to the same color star1.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1], facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3])) star1.setTwoSided(True) star1.reparentTo(base.render) if togglecontacts: for j, contactpair in enumerate(self.gripcontactpairs[self.counter]): cttpnt0 = contactpair[0] cttpnt1 = contactpair[1] self.pggen.plotSphere(star0, pos=cttpnt0+plotoffsetfp*self.facetnormals[facetidx0], radius=4, rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1], facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]]) # pandageom.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4, # rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1], # facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]]) # use the same color self.pggen.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4, rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1], facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]]) if togglecontactnormals: for j, contactpair in enumerate(self.gripcontactpairs[self.counter]): cttpnt0 = contactpair[0] cttpnt1 = contactpair[1] self.pggen.plotArrow(star0, spos=cttpnt0+plotoffsetfp*self.facetnormals[facetidx0], epos=cttpnt0 + plotoffsetfp*self.facetnormals[facetidx0] + self.gripcontactpairnormals[self.counter][j][0], rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1], facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10) # pandageom.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], # epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] + # self.gripcontactpairnormals[self.counter][j][1], # rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1], # facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]], length=10) # use the same color self.pggen.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] + self.gripcontactpairnormals[self.counter][j][1], rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1], facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10)
def segShow2(self, base, togglesamples=False, togglenormals=False, togglesamples_ref=False, togglenormals_ref=False, togglesamples_refcls=False, togglenormals_refcls=False, specificface = True): """ :param base: :param togglesamples: :param togglenormals: :param togglesamples_ref: toggles the sampled points that fulfills the dist requirements :param togglenormals_ref: :return: """ nfacets = self.facets.shape[0] facetcolorarray = self.facetcolorarray rgbapnts0 = [1, 1, 1, 1] rgbapnts1 = [0, 0, 1, 1] rgbapnts2 = [1, 0, 0, 1] # offsetf = facet plotoffsetf = .0 faceplotted = False # plot the segments for i, facet in enumerate(self.facets): if not specificface: geom = pandageom.packpandageom_fn(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i], [self.objtrimesh.vertices.shape[0],1]), self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet]) node = GeomNode('piece') node.addGeom(geom) star = NodePath('piece') star.attachNewNode(node) star.setColor(Vec4(.77, .17, 0, 1)) star.setTransparency(TransparencyAttrib.MAlpha) star.setTwoSided(True) star.reparentTo(base.render) # sampledpnts = samples[sample_idxes[i]] # for apnt in sampledpnts: # pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba) if togglesamples: for j, apnt in enumerate(self.objsamplepnts[i]): self.pggen.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0) if togglenormals: for j, apnt in enumerate(self.objsamplepnts[i]): self.pggen.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j], rgba=rgbapnts0, length=10) if togglesamples_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): self.pggen.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.9, rgba=rgbapnts1) if togglenormals_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): self.pggen.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j], rgba=rgbapnts1, length=10) if togglesamples_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): self.pggen.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts2) if togglenormals_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): self.pggen.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j], rgba=rgbapnts2, length=10) if specificface: plotoffsetf = .3 if faceplotted: continue else: if len(self.objsamplepnts[i])>85: faceplotted = True geom = pandageom.packpandageom_fn(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i], [self.objtrimesh.vertices.shape[0],1]), self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet]) node = GeomNode('piece') node.addGeom(geom) star = NodePath('piece') star.attachNewNode(node) star.setColor(Vec4(facetcolorarray[i][0], facetcolorarray[i][1], facetcolorarray[i][2], 1)) star.setColor(Vec4(.7,.3,.3, 1)) star.setTransparency(TransparencyAttrib.MAlpha) star.setTwoSided(True) star.reparentTo(base.render) # sampledpnts = samples[sample_idxes[i]] # for apnt in sampledpnts: # pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba) if togglesamples: for j, apnt in enumerate(self.objsamplepnts[i]): self.pggen.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0) if togglenormals: for j, apnt in enumerate(self.objsamplepnts[i]): self.pggen.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j], rgba=rgbapnts0, length=10) if togglesamples_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): self.pggen.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.9, rgba=rgbapnts1) if togglenormals_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): self.pggen.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j], rgba=rgbapnts1, length=10) if togglesamples_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): self.pggen.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts2) if togglenormals_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): self.pggen.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j], rgba=rgbapnts2, length=10)
def segShow(self, base, togglesamples=False, togglenormals=False, togglesamples_ref=False, togglenormals_ref=False, togglesamples_refcls=False, togglenormals_refcls=False, alpha = .1): """ :param base: :param togglesamples: :param togglenormals: :param togglesamples_ref: toggles the sampled points that fulfills the dist requirements :param togglenormals_ref: :return: """ nfacets = self.facets.shape[0] facetcolorarray = self.facetcolorarray # offsetf = facet plotoffsetf = .0 # plot the segments print("number of facets", len(self.facets)) print("average triangles", np.array([len(facet) for facet in self.facets]).mean()) for i, facet in enumerate(self.facets): geom = pandageom.packpandageom_fn(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i], [self.objtrimesh.vertices.shape[0],1]), self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet]) node = GeomNode('piece') node.addGeom(geom) star = NodePath('piece') star.attachNewNode(node) star.setColor(Vec4(facetcolorarray[i][0], facetcolorarray[i][1], facetcolorarray[i][2], alpha)) star.setTransparency(TransparencyAttrib.MAlpha) star.setTwoSided(True) star.reparentTo(base.render) # sampledpnts = samples[sample_idxes[i]] # for apnt in sampledpnts: # pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba) rgbapnts0 = [1,1,1,1] rgbapnts1 = [.5,.5,0,1] rgbapnts2 = [1,0,0,1] if togglesamples: for j, apnt in enumerate(self.objsamplepnts[i]): self.pggen.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts0) if togglenormals: for j, apnt in enumerate(self.objsamplepnts[i]): self.pggen.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j], rgba=rgbapnts0, length=10) if togglesamples_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): self.pggen.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts1) if togglenormals_ref: for j, apnt in enumerate(self.objsamplepnts_ref[i]): self.pggen.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j], rgba=rgbapnts1, length=10) if togglesamples_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): self.pggen.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts2) if togglenormals_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): self.pggen.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i], epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j], rgba=rgbapnts2, length=10)
def planContactpairs(self, torqueresist = 50, fgrtipdist = 82, dotnormpara = -0.75): """ find the grasps using parallel pairs :param: torqueresist the maximum allowable distance to com :param: fgrtipdist the maximum dist between finger tips :param: dotnormpara parallelity :return: author: weiwei date: 20161130, harada office @ osaka university """ # note that pairnormals and pairfacets are duplicated for each contactpair # the duplication is performed on purpose for convenient access # also, each contactpair"s" corresponds to a facetpair # it is empty when no contactpair is available self.gripcontactpairs = [] # gripcontactpairnormals and gripcontactpairfacets are not helpful # they are kept for convenience (they could be accessed using facetnormals and facetpairs) self.gripcontactpairnormals = [] self.gripcontactpairfacets = [] # for precollision detection # bulletworldprecc = BulletWorld() # # build the collision shape of objtrimesh # geomobj = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, # self.objtrimesh.faces) # objmesh = BulletTriangleMesh() # objmesh.addGeom(geomobj) # objmeshnode = BulletRigidBodyNode('objmesh') # objmeshnode.addShape(BulletTriangleMeshShape(objmesh, dynamic=True)) # bulletworldprecc.attachRigidBody(objmeshnode) # for raytracing bulletworldray = BulletWorld() nfacets = self.facets.shape[0] self.facetpairs = list(itertools.combinations(range(nfacets), 2)) for facetpair in self.facetpairs: # print("facetpair") # print(facetpair, len(self.facetpairs)) self.gripcontactpairs.append([]) self.gripcontactpairnormals.append([]) self.gripcontactpairfacets.append([]) # if one of the facet doesnt have samples, jump to next if self.objsamplepnts_refcls[facetpair[0]].shape[0] is 0 or \ self.objsamplepnts_refcls[facetpair[1]].shape[0] is 0: # print("no sampled points") continue # check if the faces are opposite and parallel dotnorm = np.dot(self.facetnormals[facetpair[0]], self.facetnormals[facetpair[1]]) if dotnorm < dotnormpara: # check if any samplepnts's projection from facetpairs[i][0] falls in the polygon of facetpairs[i][1] facet0pnts = self.objsamplepnts_refcls[facetpair[0]] facet0normal = self.facetnormals[facetpair[0]] facet1normal = self.facetnormals[facetpair[1]] # generate collision mesh facetmesh = BulletTriangleMesh() faceidsonfacet = self.facets[facetpair[1]] geom = pandageom.packpandageom_fn(self.objtrimesh.vertices, self.objtrimesh.face_normals[faceidsonfacet], self.objtrimesh.faces[faceidsonfacet]) facetmesh.addGeom(geom) facetmeshbullnode = BulletRigidBodyNode('facet') facetmeshbullnode.addShape(BulletTriangleMeshShape(facetmesh, dynamic=True)) bulletworldray.attachRigidBody(facetmeshbullnode) # check the projection of a ray for facet0pnt in facet0pnts: pFrom = Point3(facet0pnt[0], facet0pnt[1], facet0pnt[2]) pTo = pFrom + Vec3(facet1normal[0], facet1normal[1], facet1normal[2])*9999 result = bulletworldray.rayTestClosest(pFrom, pTo) if result.hasHit(): hitpos = result.getHitPos() if np.linalg.norm(np.array(facet0pnt.tolist())-np.array([hitpos[0], hitpos[1], hitpos[2]])) < fgrtipdist: fgrcenter = (np.array(facet0pnt.tolist())+np.array([hitpos[0], hitpos[1], hitpos[2]]))/2.0 # avoid large torque if np.linalg.norm(self.objtrimesh.center_mass - fgrcenter) < torqueresist: self.gripcontactpairs[-1].append([facet0pnt.tolist(), [hitpos[0], hitpos[1], hitpos[2]]]) self.gripcontactpairnormals[-1].append([[facet0normal[0], facet0normal[1], facet0normal[2]], [facet1normal[0], facet1normal[1], facet1normal[2]]]) self.gripcontactpairfacets[-1].append(facetpair) # pre collision checking: it takes one finger as a cylinder and # computes its collision with the object ## first finger # cylindernode0 = BulletRigidBodyNode('cylindernode0') # cylinder0height = 50 # cylinder0normal = Vec3(facet0normal[0], facet0normal[1], facet0normal[2]) # cylindernode0.addShape(BulletCylinderShape(radius=self.preccradius, # height=cylinder0height, # up=cylinder0normal), # TransformState.makePos(pFrom+cylinder0normal*cylinder0height*.6)) # bulletworldprecc.attachRigidBody(cylindernode0) # ## second finger # cylindernode1 = BulletRigidBodyNode('cylindernode1') # cylinder1height = cylinder1height # # use the inverse of facet0normal instead of facet1normal to follow hand orientation # cylinder1normal = Vec3(-facet0normal[0], -facet0normal[1], -facet0normal[2]) # cylindernode1.addShape(BulletCylinderShape(radius=self.preccradius, # height=cylinder1height, # up=cylinder1normal), # TransformState.makePos(pFrom+cylinder1normal*cylinder1height*.6)) # bulletworldprecc.attachRigidBody(cylindernode1) # if bulletworldprecc.contactTestPair(cylindernode0, objmeshnode) and \ # bulletworldprecc.contactTestPair(cylindernode1, objmeshnode): bulletworldray.removeRigidBody(facetmeshbullnode) # update the pairs availablepairids = np.where(self.gripcontactpairs)[0] self.facetpairs = np.array(self.facetpairs)[availablepairids] self.gripcontactpairs = np.array(self.gripcontactpairs)[availablepairids] self.gripcontactpairnormals = np.array(self.gripcontactpairnormals)[availablepairids] self.gripcontactpairfacets = np.array(self.gripcontactpairfacets)[availablepairids]
# freegriptst.removeHndcc(base) # toc = time.clock() # print toc-tic # fo.write(os.path.basename(objpath)+' '+str(toc-tic)+'\n') # fo.close() # geom = None for i, faces in enumerate(freegriptst.objtrimesh.facets()): rgba = [np.random.random(),np.random.random(),np.random.random(),1] # geom = pandageom.packpandageom(freegriptst.objtrimesh.vertices, freegriptst.objtrimesh.face_normals[faces], freegriptst.objtrimesh.faces[faces]) # compute facet normal facetnormal = np.sum(freegriptst.objtrimesh.face_normals[faces], axis=0) facetnormal = facetnormal/np.linalg.norm(facetnormal) geom = pandageom.packpandageom_fn(freegriptst.objtrimesh.vertices + np.tile(0 * facetnormal, [freegriptst.objtrimesh.vertices.shape[0], 1]), freegriptst.objtrimesh.face_normals[faces], freegriptst.objtrimesh.faces[faces]) node = GeomNode('piece') node.addGeom(geom) star = NodePath('piece') star.attachNewNode(node) star.setColor(Vec4(rgba[0],rgba[1],rgba[2],rgba[3])) # star.setColor(Vec4(.7,.4,0,1)) star.setTwoSided(True) star.reparentTo(base.render) # freegriptst.removeFgrpcc(base) # def updateshow(task): # freegriptst.pairShow(base, togglecontacts=True, togglecontactnormals=True) # # print task.delayTime