def updateshow(rbd, rbdnp, framenp, task): for frame in framenp: frame.detachNode() dtime = 0.001 skipframe = 15 for i in range(skipframe): force, torque = genForce(rbd, dtime) [P, L] = doPhysics(rbd, force, torque, dtime) writer.writerow([str(L[0]), str(L[1]), str(L[2])]) updateRbdPR(rbd, dtime) if task.time > 2.0: writer.close() # model = loader.loadModel(model_filepath) # model.reparentTo(base.render) model.setMat(pg.cvtMat4(rbd.rotmat)) model.setPos(pg.npToV3(rbd.pos)) arrownp = pg.plotArrow(base.render, epos=rbd.angularw * 500.0, thickness=15, rgba=[1, 0.5, 0.5, 1]) framenp.append(arrownp) return task.again
def updateshow(ur5dualrobot, dfvalue, armid, robotnp, arrownp, task): for robot in robotnp: robot.detachNode() for arrow in arrownp: arrow.detachNode() ur5dualmnp = u5p.genUr5dualmnp(ur5dualrobot, handpkg) ur5dualmnp.reparentTo(base.render) robotnp.append(ur5dualmnp) if time.clock() > 2: dfvalue = 0 tcpmat4 = ur5dualrobot.gettcpframe(armid) dfvec = tcpmat4.getRow3(0) * dfvalue arrownp.append( pg.plotArrow(base.render, tcpmat4.getRow3(3), tcpmat4.getRow3(3) + dfvec * 30, thickness=10, rgba=[.7, .2, .2, 1])) deltax = impx.control(dfvec[0]) deltay = impy.control(dfvec[1]) deltaz = impz.control(dfvec[2]) print deltax, deltay, deltaz tgtpos = inittcppos + np.array([deltax, deltay, deltaz]) tgtrot = inittcprot armjntsgoal = u5dik.numik(ur5dualrobot, tgtpos, tgtrot, armid) if armjntsgoal is not None: ur5dualrobot.movearmfk(armjntsgoal, armid) return task.again
def update(task): dt = globalClock.getDt() world.doPhysics(dt) vecw= topbullnode.getAngularVelocity() arrownp = pg.plotArrow(base.render, epos = vecw*1000, thickness = 15) # print rotmat topbullnode.get return task.cont
def update(task): dt = globalClock.getDt() world.doPhysics(dt) vecw = topbullnode.getAngularVelocity() arrownp = pg.plotArrow(base.render, epos=vecw * 1000, thickness=15) # print rotmat topbullnode.get return task.cont
def updateshow(linknp, task): for link in linknp: link.detachNode() th = sgllink.control(10000) print th, math.degrees(th) arrownp = pg.plotArrow(base.render, spos = np.array([0,0,0]), epos = np.array([0, math.cos(th), math.sin(th)])*linklen) linknp.append(arrownp) # if time.clock() > 2: # dfvalue = 0 return task.again
def updateshow(linknp, task): for link in linknp: link.detachNode() th = sgllink.control(10000) print th, math.degrees(th) arrownp = pg.plotArrow( base.render, spos=np.array([0, 0, 0]), epos=np.array([0, math.cos(th), math.sin(th)]) * linklen) linknp.append(arrownp) # if time.clock() > 2: # dfvalue = 0 return task.again
def updateshow(rbd, rbdnp, framenp, task): for frame in framenp: frame.detachNode() dtime = 0.001 skipframe = 15 for i in range(skipframe): force, torque = genForce(rbd, dtime) [P, L] = doPhysics(rbd, force, torque, dtime) writer.writerow([str(L[0]), str(L[1]), str(L[2])]) updateRbdPR(rbd, dtime) if task.time > 2.0: writer.close() # model = loader.loadModel(model_filepath) # model.reparentTo(base.render) model.setMat(pg.cvtMat4(rbd.rotmat)) model.setPos(pg.npToV3(rbd.pos)) arrownp = pg.plotArrow(base.render, epos = rbd.angularw*500.0, thickness = 15, rgba=[1,0.5,0.5,1]) framenp.append(arrownp) return task.again
def updateshow(rbd, rbdnp, framenp, task): for frame in framenp: frame.detachNode() dtime = 0.002 angularmomentum = doEulerPhysics(rbd, dtime) model.setMat(pg.cvtMat4(rbd.rotmat)) # writer.writerow([str(rbd.anglew[0]), str(rbd.anglew[1]), str(rbd.anglew[2])]) writer.writerow([ str(angularmomentum[0]), str(angularmomentum[1]), str(angularmomentum[2]) ]) arrownp = pg.plotArrow(base.render, epos=rbd.anglew * 500, thickness=15) framenp.append(arrownp) if task.time > 10.0: writer.close() return task.again
pointsnormals.append(eigvec) return pointsnormals if __name__=='__main__': import os import pandaplotutils.pandactrl as pandactrl import pandaplotutils.pandageom as pg import CADTemp base = pandactrl.World(camp=[0,500,3000], lookatp=[0,0,0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "calibtable.stl") cadtemp = CADTemp.CADTemp(ompath=objpath, numpointsoververts=200) objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') temppnts = cadtemp.pcdtemp normals = estimatenormals(temppnts) temppntsnp = pg.genPntsnp(temppnts) temppntsnp.reparentTo(base.render) for i, normal in enumerate(normals): pg.plotArrow(base.render, spos = temppnts[i], epos = normal+temppnts[i], thickness = 5, length = 100) base.run()
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( 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]): pandageom.plotSphere( star, pos=apnt + plotoffsetf * i * self.facetnormals[i], radius=2.8, rgba=rgbapnts0) if togglenormals: for j, apnt in enumerate(self.objsamplepnts[i]): pandageom.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]): pandageom.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]): pandageom.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]): pandageom.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]): pandageom.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( 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]): pandageom.plotSphere( star, pos=apnt + plotoffsetf * i * self.facetnormals[i], radius=2.8, rgba=rgbapnts0) if togglenormals: for j, apnt in enumerate(self.objsamplepnts[i]): pandageom.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]): pandageom.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]): pandageom.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]): pandageom.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]): pandageom.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 updateshow(ur5u, ur5dualrobot, armid, robotnp, arrownp, counter, fvalhandinit, tcpinit, tpose, task): for robot in robotnp: robot.detachNode() for arrow in arrownp: arrow.detachNode() ur5dualmnp = u5p.genUr5dualmnp(ur5dualrobot, handpkg) ur5dualmnp.reparentTo(base.render) robotnp.append(ur5dualmnp) force = ur5u.getforcevec(armid=armid) basemat4 = Mat4.rotateMat(-145, Vec3(1, 0, 0)) tcpmat4 = ur5dualrobot.gettcpframe(armid) fvecx = basemat4.getRow3(0) * force[0] fvecy = basemat4.getRow3(1) * force[1] fvecz = basemat4.getRow3(2) * force[2] fvecxhand = fvecx.project(tcpmat4.getRow3(0)) + fvecy.project( tcpmat4.getRow3(0)) + fvecz.project(tcpmat4.getRow3(0)) fvecyhand = fvecx.project(tcpmat4.getRow3(1)) + fvecy.project( tcpmat4.getRow3(1)) + fvecz.project(tcpmat4.getRow3(1)) fveczhand = fvecx.project(tcpmat4.getRow3(2)) + fvecy.project( tcpmat4.getRow3(2)) + fvecz.project(tcpmat4.getRow3(2)) fvalxhand = fvecxhand.length() fvalyhand = fvecyhand.length() fvalzhand = fveczhand.length() fvecxworld = fvecx.project(Vec3(1, 0, 0)) + fvecy.project(Vec3( 1, 0, 0)) + fvecz.project(Vec3(1, 0, 0)) fvecyworld = fvecx.project(Vec3(0, 1, 0)) + fvecy.project(Vec3( 0, 1, 0)) + fvecz.project(Vec3(0, 1, 0)) fveczworld = fvecx.project(Vec3(0, 0, 1)) + fvecy.project(Vec3( 0, 0, 1)) + fvecz.project(Vec3(0, 0, 1)) fvalxworld = fvecxworld[0] fvalyworld = fvecyworld[1] fvalzworld = fveczworld[2] if counter[0] == 0: fvalhandinit[0] = fvalxhand fvalhandinit[1] = fvalyhand fvalhandinit[2] = fvalzhand tcpinit[0], tcpinit[1] = ur5dualrobot.gettcp(armid) counter[0] += 1 task.again if fvalxhand > fvalhandinit[0] + 20: deltax = impx.control(fvalxworld) arrownp.append( pg.plotArrow(base.render, tcpmat4.getRow3(3), tcpmat4.getRow3(3) + fvecxhand * 50, thickness=10, rgba=[.7, .2, .2, 1])) else: deltax = impx.control(0) if fvalyhand > fvalhandinit[1] + 20: deltay = impy.control(fvalyworld) arrownp.append( pg.plotArrow(base.render, tcpmat4.getRow3(3), tcpmat4.getRow3(3) + fvecyhand * 50, thickness=10, rgba=[.2, .7, .2, 1])) else: deltay = impy.control(0) if fvalzhand > fvalhandinit[2] + 20: deltaz = impz.control(fvalzworld) arrownp.append( pg.plotArrow(base.render, tcpmat4.getRow3(3), tcpmat4.getRow3(3) + fveczhand * 50, thickness=10, rgba=[.2, .2, .7, 1])) else: deltaz = impz.control(0) print tpose[0] print deltax, deltay, deltaz tcpgoalpos = tcpinit[0] + np.array([deltax, deltay, deltaz]) tcpgoalrot = tcpinit[1] armjntsgoal = u5dik.numik(ur5dualrobot, tcpgoalpos, tcpgoalrot, armid) if armjntsgoal is not None: ur5dualrobot.movearmfk(armjntsgoal, armid) # ur5u.movejntssgl(armjntsgoal.tolist(), armid) return task.again
this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "ttube.stl") ppfpath = os.path.join(this_dir, "models", "ttubegmd.pickle") # ppfmatch = PPFMatch(objpath, ppfpath = None, bsave = True) ppfmatch = PPFMatch(objpath, ppfpath=ppfpath) perceivedpnts, perceivednormals = ppfmatch.perceivePoints(1) # plotperceived pnts pntsnp = pg.genPntsnp(perceivedpnts) pntsnp.reparentTo(base.render) for i, normal in enumerate(perceivednormals): pg.plotArrow(base.render, spos=perceivedpnts[i], epos=normal + perceivedpnts[i], thickness=.2, length=10) rotmat4list, silist, milist = ppfmatch.match(perceivedpnts, perceivednormals, ddist=5.0, dangle=30.0) try: for i in range(len(rotmat4list)): # for i in range(1): rotmat4 = rotmat4list[i] si = silist[i] mi = milist[i] #object
colors = [] color = [ np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand() ] for pnt in objectpnts: colors.append(color) pntsnp = pg.genPntsnp(objectpnts, colors=colors) pntsnp.reparentTo(base.render) for i, normal in enumerate(normals): pg.plotArrow(base.render, spos=objectpnts[i], epos=normal + objectpnts[i], thickness=.2, length=10) # import robotsim.nextage.nxt as nxt # import robotsim.nextage.nxtplot as nxtplot # import pandaplotutils.pandageom as pg # from manipulation.grip.robotiq85 import rtq85nm # handpkg = rtq85nm # nxtrobot = nxt.NxtRobot() # nxtmnp = nxtplot.genmnp(nxtrobot, handpkg) # nxtmnp.reparentTo(base.render) # pg.plotAxisSelf(base.render) # objarray = caliber.getAllObjectPcd() # for objectpnts in objarray:
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 # np0 = base.render.find("**/pair0") # if np0: # np0.removeNode() # np1 = base.render.find("**/pair1") # if np1: # np1.removeNode() # # np0collection = base.render.findAllMatches("**/rtq85fgrpcc0") # for np0 in np0collection: # np0.removeNode() # np1collection = base.render.findAllMatches("**/rtq85fgrpcc1") # for np1 in np1collection: # np1.removeNode() # # npscollection = base.render.findAllMatches("**/sphere") # for nps in npscollection: # nps.removeNode() npbrchild = base.render.find("**/tempplot") if npbrchild: npbrchild.removeNode() # 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(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(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(rtq85pcc0) 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(rtq85pcc1) 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() 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)) 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) pandageom.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) pandageom.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 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.2, 0.7, 1, 1] rgbapnts2 = [1, 0.7, 0.2, 1] # offsetf = facet plotoffsetf = .0 faceplotted = False # plot the segments for i, facet in enumerate(self.facets): if not specificface: geom = pandageom.packpandageom(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]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0) if togglenormals: for j, apnt in enumerate(self.objsamplepnts[i]): pandageom.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]): pandageom.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]): pandageom.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]): pandageom.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]): pandageom.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 = .1 if faceplotted: continue else: if len(self.objsamplepnts[i])>25: faceplotted = True geom = pandageom.packpandageom(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.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]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0) if togglenormals: for j, apnt in enumerate(self.objsamplepnts[i]): pandageom.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]): pandageom.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]): pandageom.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]): pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3.5, rgba=rgbapnts2) if togglenormals_refcls: for j, apnt in enumerate(self.objsamplepnts_refcls[i]): pandageom.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 removebadfacetsshow(self, base, doverh=.1): """ remove the facets that cannot support stable placements :param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com when fh>dmg, the object tends to fall over. setting doverh to 0.033 means when f>0.1mg, the object is judged to be unstable :return: author: weiwei date: 20161213 """ plotoffsetfp = 10 # print self.counter if self.counter < len(self.ocfacets): i = self.counter # for i in range(len(self.ocfacets)): geom = pg.packpandageom(self.objtrimeshconv.vertices, self.objtrimeshconv.face_normals[self.ocfacets[i]], self.objtrimeshconv.faces[self.ocfacets[i]]) geombullnode = cd.genCollisionMeshGeom(geom) self.bulletworldray.attachRigidBody(geombullnode) pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2]) pTo = self.objcom+self.ocfacetnormals[i]*99999 pTo = Point3(pTo[0], pTo[1], pTo[2]) result = self.bulletworldray.rayTestClosest(pFrom, pTo) self.bulletworldray.removeRigidBody(geombullnode) if result.hasHit(): hitpos = result.getHitPos() pg.plotArrow(base.render, spos=self.objcom, epos = self.objcom+self.ocfacetnormals[i], length=100) facetinterpnt = np.array([hitpos[0],hitpos[1],hitpos[2]]) facetnormal = np.array(self.ocfacetnormals[i]) bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(self.objtrimeshconv, self.ocfacets[i], facetinterpnt, facetnormal) for j in range(len(bdverts3d)-1): spos = bdverts3d[j] epos = bdverts3d[j+1] pg.plotStick(base.render, spos, epos, thickness = 1, rgba=[.5,.5,.5,1]) facetp = Polygon(bdverts2d) facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2] apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1]) dist2p = apntpnt.distance(facetp.exterior) dist2c = np.linalg.norm(np.array([hitpos[0],hitpos[1],hitpos[2]])-np.array([pFrom[0],pFrom[1],pFrom[2]])) if dist2p/dist2c < doverh: print "not stable" # return else: print dist2p/dist2c pol_ext = LinearRing(bdverts2d) d = pol_ext.project(apntpnt) p = pol_ext.interpolate(d) closest_point_coords = list(p.coords)[0] closep = np.array([closest_point_coords[0], closest_point_coords[1], 0]) closep3d = rm.transformmat4(rm.homoinverse(facetmat4), closep)[:3] pg.plotDumbbell(base.render, spos=facetinterpnt, epos=closep3d, thickness=1.5, rgba=[0,0,1,1]) for j in range(len(bdverts3d)-1): spos = bdverts3d[j] epos = bdverts3d[j+1] pg.plotStick(base.render, spos, epos, thickness = 1.5, rgba=[0,1,0,1]) # geomoff = pg.packpandageom(self.objtrimeshconv.vertices + # np.tile(plotoffsetfp * self.ocfacetnormals[i], # [self.objtrimeshconv.vertices.shape[0], 1]), # self.objtrimeshconv.face_normals[self.ocfacets[i]], # self.objtrimeshconv.faces[self.ocfacets[i]]) # # nodeoff = GeomNode('supportfacet') # nodeoff.addGeom(geomoff) # staroff = NodePath('supportfacet') # staroff.attachNewNode(nodeoff) # staroff.setColor(Vec4(1,0,1,1)) # staroff.setTransparency(TransparencyAttrib.MAlpha) # staroff.setTwoSided(True) # staroff.reparentTo(base.render) self.counter+=1 else: self.counter=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 # 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 facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] geomfacet0 = pandageom.packpandageom(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(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] pandageom.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 pandageom.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] pandageom.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 pandageom.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)
self.__newsamplesnoverts.append(sample) self.__newsamplesnovertsnormals.append(normal) else: continue if __name__=='__main__': base = pandactrl.World(camp=[0,0,300], lookatp=[0,0,0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "ttube.stl") cadtemp = CADTemp(ompath = objpath) objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') # objnp.reparentTo(base.render) colors = [] color = [1,0,0,1] for pnt in cadtemp.pcdtempnovertsinner: colors.append(color) pntsnp = pg.genPntsnp(cadtemp.pcdtempnovertsinner, colors=colors) pntsnp.reparentTo(base.render) for i, normal in enumerate(cadtemp.pcdtempnovertsnormalsinner): pg.plotArrow(base.render, spos = cadtemp.pcdtempnovertsinner[i], epos = normal+cadtemp.pcdtempnovertsinner[i], thickness = .2, length =10) base.run()
if __name__ == '__main__': import os import pandaplotutils.pandactrl as pandactrl import pandaplotutils.pandageom as pg import CADTemp base = pandactrl.World(camp=[0, 500, 3000], lookatp=[0, 0, 0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "calibtable.stl") cadtemp = CADTemp.CADTemp(ompath=objpath, numpointsoververts=200) objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') temppnts = cadtemp.pcdtemp normals = estimatenormals(temppnts) temppntsnp = pg.genPntsnp(temppnts) temppntsnp.reparentTo(base.render) for i, normal in enumerate(normals): pg.plotArrow(base.render, spos=temppnts[i], epos=normal + temppnts[i], thickness=5, length=100) base.run()
base = pandactrl.World(camp=[0,0,700], lookatp=[0,0,0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "ttube.stl") ppfpath = os.path.join(this_dir, "models", "ttubegmd.pickle") # ppfmatch = PPFMatch(objpath, ppfpath = None, bsave = True) ppfmatch = PPFMatch(objpath, ppfpath = ppfpath) perceivedpnts, perceivednormals = ppfmatch.perceivePoints(1) # plotperceived pnts pntsnp = pg.genPntsnp(perceivedpnts) pntsnp.reparentTo(base.render) for i, normal in enumerate(perceivednormals): pg.plotArrow(base.render, spos = perceivedpnts[i], epos = normal+perceivedpnts[i], thickness = .2, length = 10) rotmat4list, silist, milist = ppfmatch.match(perceivedpnts, perceivednormals, ddist = 5.0, dangle = 30.0) try: for i in range(len(rotmat4list)): # for i in range(1): rotmat4 = rotmat4list[i] si = silist[i] mi = milist[i] #object # ppfmatch.objnp.setMat(rotmat4) # ppfmatch.objnp.reparentTo(base.render) # point arrow pg.plotArrow(base.render, spos = perceivedpnts[si], epos = perceivednormals[si]+perceivedpnts[si], thickness = 4, length = 50) # temp arrow
def updateshow(ur5u, ur5dualrobot, armid, robotnp, arrownp, counter, fvalhandinit, tcpinit, tpose, task): for robot in robotnp: robot.detachNode() for arrow in arrownp: arrow.detachNode() ur5dualmnp = u5p.genmnp(ur5dualrobot, handpkg) ur5dualmnp.reparentTo(base.render) robotnp.append(ur5dualmnp) force = ur5u.getforcevec(armid = armid) basemat4 = Mat4.rotateMat(-145, Vec3(1,0,0)) tcpmat4 = ur5dualrobot.gettcpframe(armid) fvecx = basemat4.getRow3(0)*force[0] fvecy = basemat4.getRow3(1)*force[1] fvecz = basemat4.getRow3(2)*force[2] fvecxhand = fvecx.project(tcpmat4.getRow3(0)) + fvecy.project(tcpmat4.getRow3(0)) + fvecz.project(tcpmat4.getRow3(0)) fvecyhand = fvecx.project(tcpmat4.getRow3(1)) + fvecy.project(tcpmat4.getRow3(1)) + fvecz.project(tcpmat4.getRow3(1)) fveczhand = fvecx.project(tcpmat4.getRow3(2)) + fvecy.project(tcpmat4.getRow3(2)) + fvecz.project(tcpmat4.getRow3(2)) fvalxhand = fvecxhand.length() fvalyhand = fvecyhand.length() fvalzhand = fveczhand.length() fvecxworld = fvecx.project(Vec3(1,0,0)) + fvecy.project(Vec3(1,0,0)) + fvecz.project(Vec3(1,0,0)) fvecyworld = fvecx.project(Vec3(0,1,0)) + fvecy.project(Vec3(0,1,0)) + fvecz.project(Vec3(0,1,0)) fveczworld = fvecx.project(Vec3(0,0,1)) + fvecy.project(Vec3(0,0,1)) + fvecz.project(Vec3(0,0,1)) fvalxworld = fvecxworld[0] fvalyworld = fvecyworld[1] fvalzworld = fveczworld[2] if counter[0] == 0: fvalhandinit[0] = fvalxhand fvalhandinit[1] = fvalyhand fvalhandinit[2] = fvalzhand tcpinit[0], tcpinit[1] = ur5dualrobot.gettcp(armid) counter[0] += 1 task.again if fvalxhand > fvalhandinit[0]+20: deltax = impx.control(fvalxworld) arrownp.append(pg.plotArrow(base.render, tcpmat4.getRow3(3), tcpmat4.getRow3(3)+fvecxhand*50, thickness = 10, rgba = [.7,.2,.2,1])) else: deltax = impx.control(0) if fvalyhand > fvalhandinit[1]+20: deltay = impy.control(fvalyworld) arrownp.append(pg.plotArrow(base.render, tcpmat4.getRow3(3), tcpmat4.getRow3(3)+fvecyhand*50, thickness = 10, rgba = [.2,.7,.2,1])) else: deltay = impy.control(0) if fvalzhand > fvalhandinit[2]+20: deltaz = impz.control(fvalzworld) arrownp.append(pg.plotArrow(base.render, tcpmat4.getRow3(3), tcpmat4.getRow3(3)+fveczhand*50, thickness = 10, rgba = [.2,.2,.7,1])) else: deltaz = impz.control(0) print tpose[0] print deltax, deltay, deltaz tcpgoalpos = tcpinit[0] + np.array([deltax, deltay, deltaz]) tcpgoalrot = tcpinit[1] armjntsgoal = u5dik.numik(ur5dualrobot, tcpgoalpos, tcpgoalrot, armid) if armjntsgoal is not None: ur5dualrobot.movearmfk(armjntsgoal, armid) # ur5u.movejntssgl(armjntsgoal.tolist(), armid) return task.again
def removebadfacetsshow(self, base, doverh=.1): """ remove the facets that cannot support stable placements :param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com when fh>dmg, the object tends to fall over. setting doverh to 0.033 means when f>0.1mg, the object is judged to be unstable :return: author: weiwei date: 20161213 """ plotoffsetfp = 10 # print self.counter if self.counter < len(self.ocfacets): i = self.counter # for i in range(len(self.ocfacets)): geom = pg.packpandageom( self.objtrimeshconv.vertices, self.objtrimeshconv.face_normals[self.ocfacets[i]], self.objtrimeshconv.faces[self.ocfacets[i]]) geombullnode = cd.genCollisionMeshGeom(geom) self.bulletworldray.attachRigidBody(geombullnode) pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2]) pTo = self.objcom + self.ocfacetnormals[i] * 99999 pTo = Point3(pTo[0], pTo[1], pTo[2]) result = self.bulletworldray.rayTestClosest(pFrom, pTo) self.bulletworldray.removeRigidBody(geombullnode) if result.hasHit(): hitpos = result.getHitPos() pg.plotArrow(base.render, spos=self.objcom, epos=self.objcom + self.ocfacetnormals[i], length=100) facetinterpnt = np.array([hitpos[0], hitpos[1], hitpos[2]]) facetnormal = np.array(self.ocfacetnormals[i]) bdverts3d, bdverts2d, facetmat4 = pg.facetboundary( self.objtrimeshconv, self.ocfacets[i], facetinterpnt, facetnormal) for j in range(len(bdverts3d) - 1): spos = bdverts3d[j] epos = bdverts3d[j + 1] pg.plotStick(base.render, spos, epos, thickness=1, rgba=[.5, .5, .5, 1]) facetp = Polygon(bdverts2d) facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2] apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1]) dist2p = apntpnt.distance(facetp.exterior) dist2c = np.linalg.norm( np.array([hitpos[0], hitpos[1], hitpos[2]]) - np.array([pFrom[0], pFrom[1], pFrom[2]])) if dist2p / dist2c < doverh: print "not stable" # return else: pol_ext = LinearRing(bdverts2d) d = pol_ext.project(apntpnt) p = pol_ext.interpolate(d) closest_point_coords = list(p.coords)[0] closep = np.array( [closest_point_coords[0], closest_point_coords[1], 0]) closep3d = rm.transformmat4(rm.homoinverse(facetmat4), closep)[:3] pg.plotDumbbell(base.render, spos=facetinterpnt, epos=closep3d, thickness=1.5, rgba=[0, 0, 1, 1]) for j in range(len(bdverts3d) - 1): spos = bdverts3d[j] epos = bdverts3d[j + 1] pg.plotStick(base.render, spos, epos, thickness=1.5, rgba=[0, 1, 0, 1]) # geomoff = pg.packpandageom(self.objtrimeshconv.vertices + # np.tile(plotoffsetfp * self.ocfacetnormals[i], # [self.objtrimeshconv.vertices.shape[0], 1]), # self.objtrimeshconv.face_normals[self.ocfacets[i]], # self.objtrimeshconv.faces[self.ocfacets[i]]) # # nodeoff = GeomNode('supportfacet') # nodeoff.addGeom(geomoff) # staroff = NodePath('supportfacet') # staroff.attachNewNode(nodeoff) # staroff.setColor(Vec4(1,0,1,1)) # staroff.setTransparency(TransparencyAttrib.MAlpha) # staroff.setTwoSided(True) # staroff.reparentTo(base.render) self.counter += 1 else: self.counter = 0
def computePPFwithAlpha(self, ddist=5.0, dangle=2 * math.pi / 12.0): """ compute the point pair feature f1, f2, f3, f4, and the alpha_m :return: a dictionary author: weiwei date: 20170714 """ # global model descriptor, gmd gmd = {} ntemppoint = self.temppnts.shape[0] for i in range(ntemppoint): print i, ntemppoint for j in range(ntemppoint): # for i in range(0,1): # for j in range(3,4): m_0 = np.asarray(self.temppnts[i]) m_1 = np.asarray(self.temppnts[j]) v_m0m1 = m_0 - m_1 v_m1m0 = m_1 - m_0 n_m0 = self.tempnormals[i] n_m1 = self.tempnormals[j] # f1, namely ||d||2 f1 = np.linalg.norm(m_0 - m_1) # f2, namely angle between n_m0 and v_m1m0 f2 = rm.radian_between(n_m0, v_m1m0) # f3, namely angle between n_m1 and v_m0m1 f3 = rm.radian_between(n_m1, v_m0m1) # f4, namely angle between n_m0 and n_m1 f4 = rm.radian_between(n_m0, n_m1) # discretize the values f1d = math.floor(f1 / ddist) * ddist + ddist f2d = math.floor(f2 / dangle) * dangle + dangle f3d = math.floor(f3 / dangle) * dangle + dangle f4d = math.floor(f4 / dangle) * dangle + dangle key = (f1d, f2d, f3d, f4d) # angle between n_m0 and x+ xplus = np.asarray([1, 0, 0]) yplus = np.asarray([0, 1, 0]) nm0xangle = math.degrees(rm.radian_between(n_m0, xplus)) rotax = np.cross(xplus, n_m0) if np.isnan(rotax).any() or not rotax.any(): continue rotmat = rm.rodrigues(rotax, nm0xangle) v_m1m0onxplus = np.dot(v_m1m0, rotmat) v_m1m0onxplusyzproj = np.asarray( [0, v_m1m0onxplus[1], v_m1m0onxplus[2]]) alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus) if v_m1m0onxplus[2] < 0: alpha_m0 = 2 * math.pi - alpha_m0 # debug # before transform pg.plotArrow(base.render, spos=m_0, epos=m_1, rgba=Vec4(0, 1, 0, 1)) pg.plotArrow(base.render, spos=m_0, epos=m_0 + n_m0, rgba=Vec4(1, 0, 0, 1)) # after transform # print v_m1m0onxplus # print v_m1m0onxplusyzproj pg.plotArrow(base.render, spos=m_0, epos=v_m1m0onxplus + m_0, rgba=Vec4(0, .7, .7, 1)) pg.plotArrow(base.render, spos=m_0, epos=v_m1m0onxplusyzproj + m_0, rgba=Vec4(.70, .7, .7, 1)) pg.plotArrow(base.render, spos=m_0, epos=m_0 + xplus, rgba=Vec4(.7, 0, .7, 1)) # alpha_m0 # print np.degrees(alpha_m0) # plot aixs zplus = np.asarray([0, 0, 1]) pg.plotArrow(base.render, spos=m_0, epos=m_0 + xplus * 10, rgba=Vec4(.3, 0, 0, .3)) pg.plotArrow(base.render, spos=m_0, epos=m_0 + yplus * 10, rgba=Vec4(0, .3, 0, .3)) pg.plotArrow(base.render, spos=m_0, epos=m_0 + zplus * 10, rgba=Vec4(0, 0, .3, .3)) if key in gmd.keys(): gmd[key].append([m_0, m_1, alpha_m0]) else: gmd[key] = [[m_0, m_1, alpha_m0]]
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 # np0 = base.render.find("**/pair0") # if np0: # np0.removeNode() # np1 = base.render.find("**/pair1") # if np1: # np1.removeNode() # # np0collection = base.render.findAllMatches("**/rtq85fgrpcc0") # for np0 in np0collection: # np0.removeNode() # np1collection = base.render.findAllMatches("**/rtq85fgrpcc1") # for np1 in np1collection: # np1.removeNode() # # npscollection = base.render.findAllMatches("**/sphere") # for nps in npscollection: # nps.removeNode() npbrchild = base.render.find("**/tempplot") if npbrchild: npbrchild.removeNode() # 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( 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( 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(rtq85pcc0) 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(rtq85pcc1) 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() 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)) 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) pandageom.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) pandageom.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)
# # normals normals = tools.estimatenormals(objectpnts) # transform objectpntscenter = np.mean(objectpnts, axis=0) for i in range(objectpnts.shape[0]): objectpnts[i] = objectpnts[i]-objectpntscenter colors = [] color = [np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand()] for pnt in objectpnts: colors.append(color) pntsnp = pg.genPntsnp(objectpnts, colors=colors) pntsnp.reparentTo(base.render) for i, normal in enumerate(normals): pg.plotArrow(base.render, spos = objectpnts[i], epos = normal+objectpnts[i], thickness = .2, length = 2) # import robotsim.nextage.nxt as nxt # import robotsim.nextage.nxtplot as nxtplot # import pandaplotutils.pandageom as pg # from manipulation.grip.robotiq85 import rtq85nm # handpkg = rtq85nm # nxtrobot = nxt.NxtRobot() # nxtmnp = nxtplot.genmnp(nxtrobot, handpkg) # nxtmnp.reparentTo(base.render) # pg.plotAxisSelf(base.render) # objarray = kface.getAllObjectPcd() # for objectpnts in objarray: # colors = [] # color = [np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand()]
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 # 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 facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] geomfacet0 = pandageom.packpandageom( 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( 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] pandageom.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 pandageom.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] pandageom.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 pandageom.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)
if __name__ == '__main__': base = pandactrl.World(camp=[0, 0, 700], lookatp=[0, 0, 0]) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "models", "ttube.stl") ppfmatch = PPFMatch(objpath) # ppfmatch.computePPFwithAlpha() colors = [] color = [ np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand() ] for pnt in ppfmatch.temppnts: colors.append(color) pntsnp = pg.genPntsnp(ppfmatch.temppnts, colors=colors) for i, normal in enumerate(ppfmatch.tempnormals): pg.plotArrow(base.render, spos=ppfmatch.temppnts[i], epos=normal + ppfmatch.temppnts[i], thickness=.2, length=2) # pg.plotAxisSelf(base.render) base.run()