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