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]))
