def grpshow(self, base): sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \ FROM freetabletopplacement,object WHERE \ freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname result = self.gdb.execute(sql) if len(result) != 0: idfreetabletopplacement = int(result[3][0]) objrotmat = dc.strToMat4(result[3][1]) # show object geom = pg.packpandageom(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, 0.67, 0, 1)) star.setTransparency(TransparencyAttrib.MAlpha) star.setMat(objrotmat) star.reparentTo(base.render) sql = "SELECT freetabletopgrip.rotmat, freetabletopgrip.jawwidth FROM freetabletopgrip WHERE \ freetabletopgrip.idfreetabletopplacement=%d" % idfreetabletopplacement result = self.gdb.execute(sql) for resultrow in result: hndrotmat = dc.strToMat4(resultrow[0]) hndjawwidth = float(resultrow[1]) # show grasps tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1]) tmprtq85.setMat(hndrotmat) tmprtq85.setJawwidth(hndjawwidth) # tmprtq85.setJawwidth(80) tmprtq85.reparentTo(base.render)
def updateshow(task): global countdlw, freegriprotmats if countdlw < (len(freegriprotmats) - 1): countdlw = countdlw + 1 freegriprotmat = freegriprotmats[countdlw] rtqhnd = rtq85nm.Rtq85NM(hndcolor=[1, 1, 1, .2]) rtqhnd.setMat(pandanpmat4=freegriprotmat) rtqhnd.setJawwidth(freegripjawwidth[countdlw]) rtqhnd.reparentTo(base.render) return task.again
def loadRtq85Models(self): """ load the rtq85 model and its fgrprecc model :return: """ self.rtq85hnd = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) handfgrpccpath = Filename.fromOsSpecific( os.path.join(this_dir, "robotiq85/rtq85egg", "robotiq_85_tip_precc.egg")) self.handfgrpcc_uninstanced = loader.loadModel(handfgrpccpath)
def grpsshow(self, base): sql = "SELECT dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth FROM dropworkcellgrip WHERE \ dropworkcellgrip.iddropstablepos=%d" % self.dbidstablepos result = self.gdb.execute(sql) for resultrow in result: hndrotmat = dc.strToMat4(resultrow[0]) hndjawwidth = float(resultrow[1]) # show grasps #tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1]) tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, 0.7]) tmprtq85.setMat(hndrotmat) tmprtq85.setJawwidth(hndjawwidth) # tmprtq85.setJawwidth(80) tmprtq85.reparentTo(base.render)
def showAllGrips(self): """ showAllGrips :return: author: weiwei date: 20170206 """ for i in range(len(self.gripcontacts)): # for i in range(2,3): hndrotmat = self.griprotmats[i] hndjawwidth = self.gripjawwidth[i] # show grasps # tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[.7, .7, 0.7, .7]) tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .5]) tmprtq85.setMat(hndrotmat) tmprtq85.setJawwidth(hndjawwidth) # tmprtq85.setJawwidth(80) tmprtq85.reparentTo(base.render)
def __init__(self, objpath, handpkg, gdb): self.objtrimesh = trimesh.load_mesh(objpath) self.objcom = self.objtrimesh.center_mass self.objtrimeshconv = self.objtrimesh.convex_hull # oc means object convex self.ocfacets, self.ocfacetnormals = self.objtrimeshconv.facets_over( .9999) # for dbaccess self.dbobjname = os.path.splitext(os.path.basename(objpath))[0] # use two bulletworld, one for the ray, the other for the tabletop self.bulletworldray = BulletWorld() self.bulletworldhp = BulletWorld() # plane to remove hand self.planebullnode = cd.genCollisionPlane(offset=0) self.bulletworldhp.attachRigidBody(self.planebullnode) self.handpkg = handpkg self.handname = handpkg.getHandName() print("HandName: " + str(self.handname)) # self.hand = handpkg.newHandNM(hndcolor=[0,1,0,.1]) self.rtq85hnd = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) # for dbsave # each tpsmat4 corresponds to a set of tpsgripcontacts/tpsgripnormals/tpsgripjawwidth list self.tpsmat4s = None self.tpsgripcontacts = None self.tpsgripnormals = None self.tpsgripjawwidth = None # for ocFacetShow self.counter = 0 self.gdb = gdb self.loadFreeAirGrip()
def removeHndccShow(self, base, discretesize=8): """ Handcc means hand collision detection This one is developed for demonstration This function should be called after executing removeHndcc :param discretesize: the number of hand orientations :return: delayTime author: weiwei date: 20161212, tsukuba """ # isplotted = 0 if self.rtq85plotlist: for rtq85plotnode in self.rtq85plotlist: rtq85plotnode.removeNode() self.rtq85plotlist = [] self.gripcontacts = [] self.griprotmats = [] self.gripjawwidth = [] self.gripcontactnormals = [] plotoffsetfp = 6 if self.counter2 == 0: self.counter += 1 if self.counter >= self.facetpairs.shape[0]: self.counter = 0 self.counter2 += 1 if self.counter2 >= discretesize: self.counter2 = 0 print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1) facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] for j, contactpair in enumerate( self.gripcontactpairs_precc[self.counter]): if j == 0: print j, contactpair # for angleid in range(discretesize): angleid = self.counter2 cctpnt0 = contactpair[ 0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[ 1] + plotoffsetfp * self.facetnormals[facetidx1] cctnormal0 = self.gripcontactpairnormals_precc[ self.counter][j][0] cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]] tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) # save initial hand pose fgrdist = np.linalg.norm((cctpnt0 - cctpnt1)) # TODO setting jawwidth to 80 is enough # since fgrpcc already detects inner collisions tmprtq85.setJawwidth(fgrdist) tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2]) rotax = [0, 1, 0] rotangle = 360.0 / discretesize * angleid rotmat = rm.rodrigues(rotax, rotangle) tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat()) axx = tmprtq85.getMat().getRow3(0) # 130 is the distance from hndbase to fingertip cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array( [axx[0], axx[1], axx[2]]) tmprtq85.setPos( Point3(cctcenter[0], cctcenter[1], cctcenter[2])) # collision detection self.hndbullnode = cd.genCollisionMeshMultiNp( tmprtq85.rtq85np, base.render) result = self.bulletworld.contactTest(self.hndbullnode) if not result.getNumContacts(): self.gripcontacts.append(contactpair) self.griprotmats.append(tmprtq85.getMat()) self.gripjawwidth.append(fgrdist) self.gripcontactnormals.append( self.gripcontactpairnormals_precc[self.counter][j]) # pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1]) # pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]), # tmprtq85.getMat(), length=30, thickness=2) tmprtq85.setColor([1, 1, 1, .3]) tmprtq85.reparentTo(base.render) self.rtq85plotlist.append(tmprtq85) # isplotted = 1 else: for contact in result.getContacts(): # cp = contact.getManifoldPoint() # pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1)) # pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1)) tmprtq85.setColor([.5, 0, 0, .3]) tmprtq85.reparentTo(base.render) self.rtq85plotlist.append(tmprtq85)
# debugNode.showBoundingBoxes(False) # debugNode.showNormals(False) # bullcldrnp = base.render.attachNewNode("bulletcollider") # debugNP = bullcldrnp.attachNewNode(debugNode) # debugNP.show() # freegriptst.bulletworld.setDebugNode(debugNP.node()) # taskMgr.add(updateworld, "updateworld", extraArgs=[freegriptst.bulletworld], appendTask=True) # freegriptst.showAllGrips() data = gdb.loadFreeAirGrip('bunnysim', 'rtq85') if data: freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth = data print(len(freegripid)) for i, freegriprotmat in enumerate(freegriprotmats): rtqhnd = rtq85nm.Rtq85NM(hndcolor=[1, 1, 1, .3]) newpos = freegriprotmat.getRow3( 3) - freegriprotmat.getRow3(2) * 0.0 freegriprotmat.setRow(3, newpos) rtqhnd.setMat(pandanpmat4=freegriprotmat) rtqhnd.setJawwidth(freegripjawwidth[i]) pandamat4 = rtqhnd.handnp.getMat() # x = pandamat4.getRow3(0) # if (Vec3(1,0,0).angleDeg(x)-45) < 15: rtqhnd.reparentTo(base.render) # base.pggen.plotAxis(base.render, spos=rtqhnd.handnp.getPos(),pandamat4=rtqhnd.handnp.getMat()) # base.pggen.plotSphere(base.render, freegripcontacts[i][0], radius = 10, rgba = [1.0,0,0,1]) # base.pggen.plotArrow(base.render, spos = freegripcontacts[i][0], epos = freegripcontacts[i][0]+freegripnormals[i][0], thickness = 5, rgba = [1.0,0,0,1]) # base.pggen.plotSphere(base.render, freegripcontacts[i][1], radius = 10, rgba = [0,1,0,1]) # base.run() base.run()
# armjntsgoal = hrp5robot.numikr(objpos, objrot, armid) # if armjntsgoal is not None: # hrp5robot.movearmfkr(armjntsgoal, armid) # hrp5plot.plotstick(base.render, hrp5robot) # hrp5mnp = hrp5plot.genHrp5mnp(hrp5robot) # hrp5mnp.reparentTo(base.render) armjntsgoal = hrp5robot.numik(objpos, objrot, armid) if armjntsgoal is not None: hrp5robot.movearmfk6(armjntsgoal, armid) hrp5plot.plotstick(base.render, hrp5robot) hrp5mnp = hrp5plot.genHrp5mnp(hrp5robot) # hrp5mnp.reparentTo(base.render) # goal hand from manipulation.grip.robotiq85 import rtq85nm hrp5robotrgthnd = rtq85nm.Rtq85NM() hrp5robotrgthnd.setColor([1, 0, 0, .3]) hrp5robotrgtarmlj9_rotmat = pandageom.cvtMat4(objrot, objpos + objrot[:, 0] * 130) pg.plotAxisSelf(base.render, objpos, hrp5robotrgtarmlj9_rotmat) hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat) hrp5robotrgthnd.reparentTo(base.render) # # angle = nxtik.eurgtbik(objpos) # nxtrobot.movewaist(angle) # armjntsgoal=nxtik.numik(nxtrobot, objpos, objrot) # # # nxtplot.plotstick(base.render, nxtrobot) # pandamat4=Mat4() # pandamat4.setRow(3,Vec3(0,0,250)) # # pg.plotAxis(base.render, pandamat4)