def plotOneFPandGPairs(self, parentnp, fpid=0): """ plot the gpairss at a specific rotmat4 :param fpid: the index of self.floatingposemat4 :return: author: weiwei date: 20170301, tsukuba """ objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) objnp.setMat(self.gridsfloatingposemat4s[fpid]) objnp.setColor(Vec4(.7,0.3,0,1)) objnp.reparentTo(parentnp) print self.floatinggrippairshndmat4s[fpid] for i, hndrotmat4pair in enumerate(self.floatinggrippairshndmat4s[fpid]): # if i == 9: # show grasps hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .5]) hand0mat4 = Mat4(hndrotmat4pair[0]) # h0row3 = hand0mat4.getRow3(3) # h0row3[2] = h0row3[2]+i*20.0 # hand0mat4.setRow(3, h0row3[2]) hand0.setMat(pandanpmat4 = hand0mat4) hand0.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][0]) hand0.reparentTo(parentnp) hand1 = self.handpkg.newHandNM(hndcolor=[0, .0, 1, .5]) hand1mat4 = Mat4(hndrotmat4pair[1]) # h1row3 = hand1mat4.getRow3(3) # h1row3[2] = h1row3[2]+i*20.0 # hand1mat4.setRow(3, h1row3[2]) hand1.setMat(pandanpmat4 = hand1mat4) hand1.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][1]) hand1.reparentTo(parentnp)
def __init__(self, ompath, ppfpath=None, bsave=False): """ :param ompath: path of the mesh template author: weiwei date: 20170711 """ cadtemp = CADTemp.CADTemp(ompath=ompath) self.objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') self.temppnts = cadtemp.pcdtempnovertsinner self.tempnormals = cadtemp.pcdtempnovertsnormalsinner self.kif = kif.KinectInterface() # global model descriptor, gmd if ppfpath: self.gmd = self.loadGMD(ppfpath) if bsave is True: bsave = False print "Warning: bsave must be false to load ppfpath" else: self.gmd = self.computePPFwithAlpha(self.temppnts, self.tempnormals) if bsave: pickle.dump(self.gmd, open("tmp.pickle", mode="wb"))
def __init__(self, ompath, ppfpath = None, bsave = False): """ :param ompath: path of the mesh template author: weiwei date: 20170711 """ cadtemp = CADTemp.CADTemp(ompath = ompath) self.objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') self.temppnts = cadtemp.pcdtempnovertsinner self.tempnormals = cadtemp.pcdtempnovertsnormalsinner self.kif = kif.KinectInterface() # global model descriptor, gmd if ppfpath: self.gmd = self.loadGMD(ppfpath) if bsave is True: bsave = False print "Warning: bsave must be false to load ppfpath" else: self.gmd = self.computePPFwithAlpha(self.temppnts, self.tempnormals) if bsave: pickle.dump(self.gmd, open("tmp.pickle", mode="wb"))
def __init__(self, ompath, npntsonverts=200): """ :param ompath: path of the mesh template author: weiwei date: 20170711 """ cadtemp = CADTemp.CADTemp(ompath=ompath, numpointsoververts=npntsonverts) self.objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') self.temppnt = cadtemp.pcdtemp self.kinect = PyKinectRuntime.PyKinectRuntime( PyKinectV2.FrameSourceTypes_Depth) self.dbscan = DBSCAN(eps=50, min_samples=100, n_jobs=-1) self.randsac = linear_model.RANSACRegressor( linear_model.LinearRegression(), residual_threshold=15) self.tablepnt = [] self.objectpnt = []
def plotOneFPandGPairs(self, parentnp, fpid=0): """ plot the gpairss at a specific rotmat4 :param fpid: the index of self.floatingposemat4 :return: author: weiwei date: 20170301, tsukuba """ objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) objnp.setMat(self.gridsfloatingposemat4s[fpid]) objnp.setColor(Vec4(.7,0.3,0,1)) objnp.reparentTo(parentnp) print(self.floatinggrippairshndmat4s[fpid]) for i, hndrotmat4pair in enumerate(self.floatinggrippairshndmat4s[fpid]): # if i == 9: # show grasps hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .5]) hand0mat4 = Mat4(hndrotmat4pair[0]) # h0row3 = hand0mat4.getRow3(3) # h0row3[2] = h0row3[2]+i*20.0 # hand0mat4.setRow(3, h0row3[2]) hand0.setMat(pandanpmat4 = hand0mat4) hand0.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][0]) hand0.reparentTo(parentnp) hand1 = self.handpkg.newHandNM(hndcolor=[0, .0, 1, .5]) hand1mat4 = Mat4(hndrotmat4pair[1]) # h1row3 = hand1mat4.getRow3(3) # h1row3[2] = h1row3[2]+i*20.0 # hand1mat4.setRow(3, h1row3[2]) hand1.setMat(pandanpmat4 = hand1mat4) hand1.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][1]) hand1.reparentTo(parentnp)
def __init__(self, objpath, gdb, handpkg, base): """ initialization :param objpath: path of the object :param base: for the loadFreeAirGrip --> genHandPairs functions (for collision detection) author: weiwei date: 20161215, tsukuba """ self.handpkg = handpkg self.handname = handpkg.getHandName() self.hand0 = handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) self.hand1 = handpkg.newHandNM(hndcolor=[0, 0, 1, .1]) self.objtrimesh = trimesh.load_mesh(objpath) self.objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) self.dbobjname = os.path.splitext(os.path.basename(objpath))[0] # mat4list is nested list, floatingposemat4 is list (the flat version of mat4lsit), they are essentially the same self.mat4list = [] self.floatingposemat4 = [] # gridsfloatingposemat4s is self.floatingposemat4 translated to different grids self.gridsfloatingposemat4s = [] self.icos = None self.floatinggripids = [] self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] self.bulletworld = BulletWorld() self.handpairList = [] self.gdb = gdb self.__loadFreeAirGrip(base) # for IK self.floatinggripikfeas_rgt = [] self.floatinggripikfeas_handx_rgt = [] self.floatinggripikfeas_lft = [] self.floatinggripikfeas_handx_lft = [] # for ik-feasible pairs self.floatinggrippairsids = [] self.floatinggrippairshndmat4s = [] self.floatinggrippairscontacts = [] self.floatinggrippairsnormals = [] self.floatinggrippairsjawwidths = [] self.floatinggrippairsidfreeairs = [] self.__genPandaRotmat4()
def genCobtnp(polygonlist, steplength): """ generate a mesh model for polygonlist :param polygonlist: :param color: :return: author: weiwei date: 20170517 """ npolygon = len(polygonlist) vertices = [] facenormals = [] triangles = [] nvertperpolygon = len(polygonlist[0].exterior.coords)-1 height = -steplength for polygon in polygonlist: height = height+steplength for pointid in range(0, nvertperpolygon): point = polygon.exterior.coords[pointid] vertices.append(np.array([point[0], point[1], height])) for polygonid in range(0, npolygon-1): startingid = polygonid*nvertperpolygon startingidnext = (polygonid+1)*nvertperpolygon for vertid in range(0, nvertperpolygon): id0 = startingid+vertid id1 = startingid+vertid+1 if vertid == nvertperpolygon-1: id1 = startingid idnext0 = startingidnext+vertid idnext1 = startingidnext+vertid+1 if vertid == nvertperpolygon-1: idnext1 = startingidnext triangles.append(np.array([id0, id1, idnext0])) rawnormal0 = np.cross(vertices[id1]-vertices[id0], vertices[idnext0]-vertices[id1]) triangles.append(np.array([id1, idnext1, idnext0])) rawnormal1 = np.cross(vertices[idnext1]-vertices[id1], vertices[idnext0]-vertices[idnext1]) rawnormal = rawnormal0+rawnormal1 facenormals.append(rawnormal/np.linalg.norm(rawnormal)) facenormals.append(rawnormal/np.linalg.norm(rawnormal)) # top and bottom for vertid in range(2, nvertperpolygon): triangles.append(np.array([0, vertid, vertid-1])) rawnormal = np.cross(vertices[vertid]-vertices[0], vertices[vertid-1]-vertices[vertid]) facenormals.append(rawnormal/np.linalg.norm(rawnormal)) nverts = len(vertices) for vertid in range(2, nvertperpolygon): triangles.append(np.array([nverts-1, nverts-1-vertid, nverts-vertid])) rawnormal = np.cross(vertices[nverts-1-vertid]-vertices[nverts-1], vertices[nverts-vertid]-vertices[nverts-1-vertid]) facenormals.append(rawnormal/np.linalg.norm(rawnormal)) cobnp = pg.packpandanp(np.asarray(vertices), np.asarray(facenormals), np.asarray(triangles)) return cobnp
def genCobtnp(self, polygonlist, steplength): """ generate a mesh model for polygonlist :param polygonlist: :param color: :return: author: weiwei date: 20170517 """ npolygon = len(polygonlist) vertices = [] facenormals = [] triangles = [] nvertperpolygon = len(polygonlist[0].exterior.coords)-1 height = -steplength for polygon in polygonlist: height = height+steplength for pointid in range(0, nvertperpolygon): point = polygon.exterior.coords[pointid] vertices.append(np.array([point[0], point[1], height])) for polygonid in range(0, npolygon-1): startingid = polygonid*nvertperpolygon startingidnext = (polygonid+1)*nvertperpolygon for vertid in range(0, nvertperpolygon): id0 = startingid+vertid id1 = startingid+vertid+1 if vertid == nvertperpolygon-1: id1 = startingid idnext0 = startingidnext+vertid idnext1 = startingidnext+vertid+1 if vertid == nvertperpolygon-1: idnext1 = startingidnext triangles.append(np.array([id0, id1, idnext0])) rawnormal0 = np.cross(vertices[id1]-vertices[id0], vertices[idnext0]-vertices[id1]) triangles.append(np.array([id1, idnext1, idnext0])) rawnormal1 = np.cross(vertices[idnext1]-vertices[id1], vertices[idnext0]-vertices[idnext1]) rawnormal = rawnormal0+rawnormal1 facenormals.append(rawnormal/np.linalg.norm(rawnormal)) facenormals.append(rawnormal/np.linalg.norm(rawnormal)) # top and bottom for vertid in range(2, nvertperpolygon): triangles.append(np.array([0, vertid, vertid-1])) rawnormal = np.cross(vertices[vertid]-vertices[0], vertices[vertid-1]-vertices[vertid]) facenormals.append(rawnormal/np.linalg.norm(rawnormal)) nverts = len(vertices) for vertid in range(2, nvertperpolygon): triangles.append(np.array([nverts-1, nverts-1-vertid, nverts-vertid])) rawnormal = np.cross(vertices[nverts-1-vertid]-vertices[nverts-1], vertices[nverts-vertid]-vertices[nverts-1-vertid]) facenormals.append(rawnormal/np.linalg.norm(rawnormal)) cobnp = pg.packpandanp(np.asarray(vertices), np.asarray(facenormals), np.asarray(triangles)) return cobnp
def genAvailableFAGsSgl(base, basepath, baseMat4, objpath, objMat4, gdb, handpkg): """ find the collision freeairgrips of objpath without considering rotation :param base: panda base :param basepath: the path of base object :param objpath: the path of obj object, the object to be assembled :param baseMat4, objMat4: all in world coordinates, not relative :param gdb: grasp db :param handpkg: hand package :return: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair] author: weiwei date: 20170307 """ bulletworld = BulletWorld() robothand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) basetrimesh = trimesh.load_mesh(basepath) basenp = pg.packpandanp(basetrimesh.vertices, basetrimesh.face_normals, basetrimesh.faces) basenp.setMat(baseMat4) basebullnode = cd.genCollisionMeshNp(basenp, base.render) bulletworld.attachRigidBody(basebullnode) dbobjname = os.path.splitext(os.path.basename(objpath))[0] objfag = F*g(gdb, dbobjname, handpkg) assgripcontacts = [] assgripnormals = [] assgriprotmat4s = [] assgripjawwidth = [] assgripidfreeair = [] for i, rotmat in enumerate(objfag.freegriprotmats): assgrotmat = rotmat*objMat4 robothand.setMat(assgrotmat) # detect the collisions when hand is open! robothand.setJawwidth(robothand.jawwidthopen) hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp) result0 = bulletworld.contactTest(hndbullnode) robothand.setJawwidth(objfag.freegripjawwidth[i]) hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp) result1 = bulletworld.contactTest(hndbullnode) if (not result0.getNumContacts()) and (not result1.getNumContacts()): cct0 = objMat4.xformPoint(objfag.freegripcontacts[i][0]) cct1 = objMat4.xformPoint(objfag.freegripcontacts[i][1]) cctn0 = objMat4.xformPoint(objfag.freegripnormals[i][0]) cctn1 = objMat4.xformPoint(objfag.freegripnormals[i][1]) assgripcontacts.append([cct0, cct1]) assgripnormals.append([cctn0, cctn1]) assgriprotmat4s.append(assgrotmat) assgripjawwidth.append(objfag.freegripjawwidth[i]) assgripidfreeair.append(objfag.freegripids[i]) return [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
def genAvailableFAGsSgl(base, basepath, baseMat4, objpath, objMat4, gdb, handpkg): """ find the collision freeairgrips of objpath without considering rotation :param base: panda base :param basepath: the path of base object :param objpath: the path of obj object, the object to be assembled :param baseMat4, objMat4: all in world coordinates, not relative :param gdb: grasp db :param handpkg: hand package :return: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair] author: weiwei date: 20170307 """ bulletworld = BulletWorld() robothand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) basetrimesh = trimesh.load_mesh(basepath) basenp = pg.packpandanp(basetrimesh.vertices, basetrimesh.face_normals, basetrimesh.faces) basenp.setMat(baseMat4) basebullnode = cd.genCollisionMeshNp(basenp, base.render) bulletworld.attachRigidBody(basebullnode) dbobjname = os.path.splitext(os.path.basename(objpath))[0] objfag = F*g(gdb, dbobjname, handpkg) assgripcontacts = [] assgripnormals = [] assgriprotmat4s = [] assgripjawwidth = [] assgripidfreeair = [] for i, rotmat in enumerate(objfag.freegriprotmats): assgrotmat = rotmat*objMat4 robothand.setMat(assgrotmat) # detect the collisions when hand is open! robothand.setJawwidth(robothand.jawwidthopen) hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp) result0 = bulletworld.contactTest(hndbullnode) robothand.setJawwidth(objfag.freegripjawwidth[i]) hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp) result1 = bulletworld.contactTest(hndbullnode) if (not result0.getNumContacts()) and (not result1.getNumContacts()): cct0 = objMat4.xformPoint(objfag.freegripcontacts[i][0]) cct1 = objMat4.xformPoint(objfag.freegripcontacts[i][1]) cctn0 = objMat4.xformPoint(objfag.freegripnormals[i][0]) cctn1 = objMat4.xformPoint(objfag.freegripnormals[i][1]) assgripcontacts.append([cct0, cct1]) assgripnormals.append([cctn0, cctn1]) assgriprotmat4s.append(assgrotmat) assgripjawwidth.append(objfag.freegripjawwidth[i]) assgripidfreeair.append(objfag.freegripids[i]) return [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
def plotOneFPandG(self, parentnp, fpid=0): """ plot the objpose and grasps at a specific rotmat4 :param fpid: the index of self.floatingposemat4 :return: author: weiwei date: 20170221, tsukuba """ objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) objnp.setMat(self.gridsfloatingposemat4s[fpid]) objnp.reparentTo(parentnp) for i, hndrotmat4 in enumerate(self.gridsfloatingposemat4s[fpid]): if i == 7: # show grasps hand0 = self.handpkg.newHandNM(hndcolor=[0, 1, 0, 1]) hand0.setMat(pandanpmat4=hndrotmat4) hand0.setJawwidth(self.floatinggripjawwidth[fpid][i]) hand0.reparentTo(parentnp) print self.handpairList for handidpair in self.handpairList: if handidpair[0] == self.floatinggripidfreeair[fpid][i]: pairedilist = [ i1 for i1 in range( len(self.floatinggripidfreeair[fpid])) if self.floatinggripidfreeair[fpid][i1] == handidpair[1] ] print pairedilist i1 = pairedilist[0] # if self.floatinggripikfeas_lft[fpid][i1] == 'True': hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1]) hndrotmat4 = self.floatinggripmat4s[fpid][i1] hand1.setMat(pandanpmat4=hndrotmat4) hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1]) hand1.reparentTo(parentnp) if handidpair[1] == self.floatinggripidfreeair[fpid][i]: pairedilist = [ i1 for i1 in range( len(self.floatinggripidfreeair[fpid])) if self.floatinggripidfreeair[fpid][i1] == handidpair[0] ] print pairedilist i1 = pairedilist[0] # if self.floatinggripikfeas_lft[fpid][i1] == 'True': hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1]) hndrotmat4 = self.floatinggripmat4s[fpid][i1] hand1.setMat(pandanpmat4=hndrotmat4) hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1]) hand1.reparentTo(parentnp)
def __init__(self, objpath, gdb, handpkg, base): """ initialization :param objpath: path of the object :param base: for the loadFreeAirGrip --> genHandPairs functions (for collision detection) author: weiwei date: 20161215, tsukuba """ self.handpkg = handpkg self.handname = handpkg.getHandName() self.hand0 = handpkg.newHandNM(hndcolor=[1,0,0,.1]) self.hand1 = handpkg.newHandNM(hndcolor=[0,0,1,.1]) self.objtrimesh = trimesh.load_mesh(objpath) self.objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) self.dbobjname = os.path.splitext(os.path.basename(objpath))[0] # mat4list is nested list, floatingposemat4 is list (the flat version of mat4lsit), they are essentially the same self.mat4list = [] self.floatingposemat4 = [] # gridsfloatingposemat4s is self.floatingposemat4 translated to different grids self.gridsfloatingposemat4s = [] self.icos = None self.floatinggripids = [] self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] self.bulletworld = BulletWorld() self.handpairList = [] self.gdb = gdb self.__loadFreeAirGrip(base) # for IK self.floatinggripikfeas_rgt = [] self.floatinggripikfeas_handx_rgt = [] self.floatinggripikfeas_lft = [] self.floatinggripikfeas_handx_lft = [] # for ik-feasible pairs self.floatinggrippairsids = [] self.floatinggrippairshndmat4s = [] self.floatinggrippairscontacts = [] self.floatinggrippairsnormals = [] self.floatinggrippairsjawwidths = [] self.floatinggrippairsidfreeairs = [] self.__genPandaRotmat4()
def genCobtDisnp(self, polygonlist, steplength): """ generate a mesh model for polygonlistSeg, each polygonis a separate one :param polygonlist: :param color: :return: author: weiwei date: 20170924, Vancouver """ npolygon = len(polygonlist) vertices = [] facenormals = [] triangles = [] nvertperpolygon = len(polygonlist[0].exterior.coords) - 1 height = -steplength for polygon in polygonlist: height = height + steplength for pointid in range(0, nvertperpolygon): point = polygon.exterior.coords[pointid] vertices.append(np.array([point[0], point[1], height])) for polygonid in range(0, npolygon): startingid = polygonid * nvertperpolygon for vertid in range(1, nvertperpolygon - 1): id0 = startingid id1 = startingid + vertid id2 = startingid + vertid + 1 if vertid == nvertperpolygon - 1: id1 = startingid triangles.append(np.array([id0, id2, id1])) rawnormal = -np.cross(vertices[id1] - vertices[id0], vertices[id2] - vertices[id1]) facenormals.append(rawnormal / np.linalg.norm(rawnormal)) polygonnps = [] height = -steplength for polygon in polygonlist: height = height + steplength polygonnps.append( self.genPolygonsnp(polygon, height, color=[0, 0, 0, 1], thickness=2)[0]) cobnp = pg.packpandanp(np.asarray(vertices), np.asarray(facenormals[::-1]), np.asarray(triangles[::-1])) return [cobnp, polygonnps]
def plotOneFPandG(self, parentnp, fpid=0): """ plot the objpose and grasps at a specific rotmat4 :param fpid: the index of self.floatingposemat4 :return: author: weiwei date: 20170221, tsukuba """ objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) objnp.setMat(self.gridsfloatingposemat4s[fpid]) objnp.reparentTo(parentnp) for i, hndrotmat4 in enumerate(self.gridsfloatingposemat4s[fpid]): if i == 7: # show grasps hand0 = self.handpkg.newHandNM(hndcolor=[0, 1, 0, 1]) hand0.setMat(pandanpmat4 = hndrotmat4) hand0.setJawwidth(self.floatinggripjawwidth[fpid][i]) hand0.reparentTo(parentnp) print self.handpairList for handidpair in self.handpairList: if handidpair[0] == self.floatinggripidfreeair[fpid][i]: pairedilist = [i1 for i1 in range(len(self.floatinggripidfreeair[fpid])) if self.floatinggripidfreeair[fpid][i1]==handidpair[1]] print pairedilist i1 = pairedilist[0] # if self.floatinggripikfeas_lft[fpid][i1] == 'True': hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1]) hndrotmat4 = self.floatinggripmat4s[fpid][i1] hand1.setMat(pandanpmat4 = hndrotmat4) hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1]) hand1.reparentTo(parentnp) if handidpair[1] == self.floatinggripidfreeair[fpid][i]: pairedilist = [i1 for i1 in range(len(self.floatinggripidfreeair[fpid])) if self.floatinggripidfreeair[fpid][i1]==handidpair[0]] print pairedilist i1 = pairedilist[0] # if self.floatinggripikfeas_lft[fpid][i1] == 'True': hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1]) hndrotmat4 = self.floatinggripmat4s[fpid][i1] hand1.setMat(pandanpmat4 = hndrotmat4) hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1]) hand1.reparentTo(parentnp)
def __init__(self, ompath): """ :param ompath: path of the mesh template author: weiwei date: 20170711 """ cadtemp = CADTemp.CADTemp(ompath=ompath) self.objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') self.temppnts = cadtemp.pcdtempnoverts self.tempnormals = cadtemp.pcdtempnovertsnormals self.kif = kif.KinectInterface()
def genCobtDisnp(self, polygonlist, steplength): """ generate a mesh model for polygonlistSeg, each polygonis a separate one :param polygonlist: :param color: :return: author: weiwei date: 20170924, Vancouver """ npolygon = len(polygonlist) vertices = [] facenormals = [] triangles = [] nvertperpolygon = len(polygonlist[0].exterior.coords)-1 height = -steplength for polygon in polygonlist: height = height+steplength for pointid in range(0, nvertperpolygon): point = polygon.exterior.coords[pointid] vertices.append(np.array([point[0], point[1], height])) for polygonid in range(0, npolygon): startingid = polygonid*nvertperpolygon for vertid in range(1, nvertperpolygon-1): id0 = startingid id1 = startingid+vertid id2 = startingid+vertid+1 if vertid == nvertperpolygon-1: id1 = startingid triangles.append(np.array([id0, id2, id1])) rawnormal = -np.cross(vertices[id1]-vertices[id0], vertices[id2]-vertices[id1]) facenormals.append(rawnormal/np.linalg.norm(rawnormal)) polygonnps = [] height = -steplength for polygon in polygonlist: height = height+steplength polygonnps.append(self.genPolygonsnp(polygon, height, color=[0,0,0,1], thickness=2)[0]) cobnp = pg.packpandanp(np.asarray(vertices), np.asarray(facenormals[::-1]), np.asarray(triangles[::-1])) return [cobnp, polygonnps]
def __init__(self, ompath, density = 4.0): """ :param ompath: path of the mesh template author: weiwei date: 20170711 """ cadtemp = CADTemp.CADTemp(ompath = ompath, density = density) self.objnp = pg.packpandanp(cadtemp.objtrimesh.vertices, cadtemp.objtrimesh.face_normals, cadtemp.objtrimesh.faces, name='') self.temppnt = cadtemp.pcdtemp self.kinect = PyKinectRuntime.PyKinectRuntime(PyKinectV2.FrameSourceTypes_Depth) self.dbscan = DBSCAN(eps=50, min_samples=100, n_jobs=-1) self.randsac = linear_model.RANSACRegressor(linear_model.LinearRegression(), residual_threshold = 15) self.tablepnt = [] self.objectpnt = []
nxtrobot = nxt.NxtRobot() handpkg = rtq85nm gdb = db.GraspDB() this_dir, this_filename = os.path.split(__file__) obj0path = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "planefrontstay.stl") obj0Mat4 = Mat4.identMat() obj1path = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "planewheel.stl") obj1Mat4 = Mat4(obj0Mat4) obj1Mat4.setCell(3,1,32) obj1Mat4.setCell(3,2,10) obj0trimesh = trimesh.load_mesh(obj0path) obj0np = pg.packpandanp(obj0trimesh.vertices, obj0trimesh.face_normals, obj0trimesh.faces) obj0np.setMat(obj0Mat4) obj0np.setColor(1,.7,0.3) obj1trimesh = trimesh.load_mesh(obj1path) obj1np = pg.packpandanp(obj1trimesh.vertices, obj1trimesh.face_normals, obj1trimesh.faces) obj1np.setMat(obj1Mat4) obj1np.setColor(0.3,.3,0.3) # obj0np.reparentTo(base.render) # obj1np.reparentTo(base.render) # # obj0grips, obj1grips = genAvailableFAGs(base, obj0path, obj0Mat4, obj1path, obj1Mat4, gdb, handpkg) # # ass0gripcontacts, ass0gripnormals, ass0griprotmat4s, ass0gripjawwidth, ass0gripidfreeair = obj0grips # ass1gripcontacts, ass1gripnormals, ass1griprotmat4s, ass1gripjawwidth, ass1gripidfreeair = obj1grips # for i, assgriprotmat in enumerate(ass1griprotmat4s):
# freegriptst.clusterFacetSamplesKNN(reduceRatio=15, maxNPnts=5) freesuctst.clusterFacetSamplesRNN(reduceRadius=10) # freesuctst.segShow2(base, togglesamples=True, togglenormals=False, # togglesamples_ref=True, togglenormals_ref=False, # togglesamples_refcls=True, togglenormals_refcls=False, specificface = True) # import pandaplotutils.pandageom as pg # pg.plotAxisSelf(base.render, Vec3(0,0,0)) freesuctst.removeHndcc(base) toc = time.clock() print toc-tic # freesuctst.segShow(base, togglesamples=False, togglenormals=False, # togglesamples_ref=False, togglenormals_ref=False, # togglesamples_refcls=True, togglenormals_refcls=True, alpha = 1) objnp = pandageom.packpandanp(freesuctst.objtrimesh.vertices, freesuctst.objtrimesh.face_normals, freesuctst.objtrimesh.faces, name='') objnp.setColor(.37,.37,.35,1) objnp.reparentTo(base.render) for i, hndrot in enumerate(freesuctst.sucrotmats): if i == 1: tmphand = handpkg.newHandNM(hndcolor=[.7,.7,.7,.7]) centeredrot = Mat4(hndrot) # centeredrot.setRow(3,hndrot.getRow3(3)-Vec3(freesuctst.objcenter[0], freesuctst.objcenter[1], freesuctst.objcenter[2])) tmphand.setMat(centeredrot) tmphand.reparentTo(base.render) tmphand.setColor(.5,.5,.5,.3) # for i, hndrot in enumerate(freesuctst.sucrotmatscld): # tmphand = handpkg.newHandNM(hndcolor=[.7,.7,.7,.7]) # centeredrot = Mat4(hndrot) # # centeredrot.setRow(3,hndrot.getRow3(3)-Vec3(freesuctst.objcenter[0], freesuctst.objcenter[1], freesuctst.objcenter[2]))
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()
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()
nxtrobot = nxt.NxtRobot() handpkg = rtq85nm gdb = db.GraspDB() this_dir, this_filename = os.path.split(__file__) obj0path = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "planefrontstay.stl") obj0Mat4 = Mat4.identMat() obj1path = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "planewheel.stl") obj1Mat4 = Mat4(obj0Mat4) obj1Mat4.setCell(3,1,32) obj1Mat4.setCell(3,2,10) obj0trimesh = trimesh.load_mesh(obj0path) obj0np = pg.packpandanp(obj0trimesh.vertices, obj0trimesh.face_normals, obj0trimesh.faces) obj0np.setMat(obj0Mat4) obj0np.setColor(1,.7,0.3) obj1trimesh = trimesh.load_mesh(obj1path) obj1np = pg.packpandanp(obj1trimesh.vertices, obj1trimesh.face_normals, obj1trimesh.faces) obj1np.setMat(obj1Mat4) obj1np.setColor(0.3,.3,0.3) # obj0np.reparentTo(base.render) # obj1np.reparentTo(base.render) # # obj0grips, obj1grips = genAvailableFAGs(base, obj0path, obj0Mat4, obj1path, obj1Mat4, gdb, handpkg) # # ass0gripcontacts, ass0gripnormals, ass0griprotmat4s, ass0gripjawwidth, ass0gripidfreeair = obj0grips # ass1gripcontacts, ass1gripnormals, ass1griprotmat4s, ass1gripjawwidth, ass1gripidfreeair = obj1grips # for i, assgriprotmat in enumerate(ass1griprotmat4s):
# freegriptst.clusterFacetSamplesKNN(reduceRatio=15, maxNPnts=5) freesuctst.clusterFacetSamplesRNN(reduceRadius=10) # freesuctst.segShow2(base, togglesamples=True, togglenormals=False, # togglesamples_ref=True, togglenormals_ref=False, # togglesamples_refcls=True, togglenormals_refcls=False, specificface = True) # import pandaplotutils.pandageom as pg # pg.plotAxisSelf(base.render, Vec3(0,0,0)) freesuctst.removeHndcc(base) toc = time.clock() print(toc - tic) # freesuctst.segShow(base, togglesamples=False, togglenormals=False, # togglesamples_ref=False, togglenormals_ref=False, # togglesamples_refcls=True, togglenormals_refcls=True, alpha = 1) objnp = pandageom.packpandanp(freesuctst.objtrimesh.vertices, freesuctst.objtrimesh.face_normals, freesuctst.objtrimesh.faces, name='') objnp.setColor(.37, .37, .35, 1) objnp.reparentTo(base.render) for i, hndrot in enumerate(freesuctst.sucrotmats): if i == 1: tmphand = handpkg.newHandNM(hndcolor=[.7, .7, .7, .7]) centeredrot = Mat4(hndrot) # centeredrot.setRow(3,hndrot.getRow3(3)-Vec3(freesuctst.objcenter[0], freesuctst.objcenter[1], freesuctst.objcenter[2])) tmphand.setMat(centeredrot) tmphand.reparentTo(base.render) tmphand.setColor(.5, .5, .5, .3) # for i, hndrot in enumerate(freesuctst.sucrotmatscld): # tmphand = handpkg.newHandNM(hndcolor=[.7,.7,.7,.7]) # centeredrot = Mat4(hndrot) # # centeredrot.setRow(3,hndrot.getRow3(3)-Vec3(freesuctst.objcenter[0], freesuctst.objcenter[1], freesuctst.objcenter[2]))