def genHandPairs(self, base, loadser=False):
self.handpairList = []
if loadser is False:
# hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
# hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
pairidlist = list(
itertools.combinations(range(len(self.freegripids)), 2))
print len(pairidlist) / 5000 + 1
for i in range(100, len(pairidlist), len(pairidlist) / 5000 + 1):
# for i0, i1 in pairidlist:
i0, i1 = pairidlist[i]
print i, len(pairidlist)
self.hand0.setMat(pandanpmat4=self.freegriprotmats[i0])
self.hand0.setJawwidth(self.freegripjawwidth[i0])
self.hand1.setMat(pandanpmat4=self.freegriprotmats[i1])
self.hand1.setJawwidth(self.freegripjawwidth[i1])
hndbullnodei0 = cd.genCollisionMeshMultiNp(
self.hand0.handnp, base.render)
hndbullnodei1 = cd.genCollisionMeshMultiNp(
self.hand1.handnp, base.render)
result = self.bulletworld.contactTestPair(
hndbullnodei0, hndbullnodei1)
if not result.getNumContacts():
self.handpairList.append(
[self.freegripids[i0], self.freegripids[i1]])
pickle.dump(self.handpairList, open("tmp.pickle", mode="wb"))
else:
self.handpairList = pickle.load(open("tmp.pickle", mode="rb"))
def __genHandPairs(self, base):
self.handpairList = []
self.__genFreeAssGrips()
ass0gripcontacts, ass0gripnormals, ass0griprotmat4s, ass0gripjawwidth, ass0gripidfreeair = self.obj0grips
ass1gripcontacts, ass1gripnormals, ass1griprotmat4s, ass1gripjawwidth, ass1gripidfreeair = self.obj1grips
hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
i0list = range(len(ass0gripidfreeair))
i1list = range(len(ass1gripidfreeair))
pairidlist = list(itertools.product(i0list, i1list))
print(">>>>> " + str(len(pairidlist)))
print(len(pairidlist)/10000+1)
# for i in np.arange(0,len(pairidlist),len(pairidlist)/10000+1):
for i in range(0,len(pairidlist),len(pairidlist)/10000+1):
# for i0, i1 in pairidlist:
i0, i1 = pairidlist[i]
print(i, len(pairidlist))
hand0.setMat(ass0griprotmat4s[i0])
hand0.setJawwidth(ass0gripjawwidth[i0])
hand1.setMat(ass1griprotmat4s[i1])
hand1.setJawwidth(ass1gripjawwidth[i1])
hndbullnodei0 = cd.genCollisionMeshMultiNp(hand0.handnp, base.render)
hndbullnodei1 = cd.genCollisionMeshMultiNp(hand1.handnp, base.render)
result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1)
if not result.getNumContacts():
self.handpairList.append([ass0gripidfreeair[i0], ass1gripidfreeair[i1]])
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 removeFgrpccShowLeft(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
Plot the available grips
:return:
author: weiwei
date: 20161212, tsukuba
"""
plotoffsetfp = 6
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
return
else:
print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1)
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(
self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
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("rtq85fgrpcc1")
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(base.render)
handfgrpcc1.reparentTo(base.render)
def removeFgrpcc(self, base):
"""
Fgrpcc means finger pre collision detection
:return:
author: weiwei
date: 20161212, tsukuba
"""
self.gripcontactpairs_precc = []
self.gripcontactpairnormals_precc = []
self.gripcontactpairfacets_precc = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
print self.gripcontactpairs
self.gripcontactpairs_precc.append([])
self.gripcontactpairnormals_precc.append([])
self.gripcontactpairfacets_precc.append([])
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
self.gripcontactpairs_precc[-1].append(contactpair)
self.gripcontactpairnormals_precc[-1].append(self.gripcontactpairnormals[self.counter][j])
self.gripcontactpairfacets_precc[-1].append(self.gripcontactpairfacets[self.counter])
self.counter += 1
self.counter=0
def removeFgrpcc(self, base):
"""
Fgrpcc means finger pre collision detection
:return:
author: weiwei
date: 20161212, tsukuba
"""
self.gripcontactpairs_precc = []
self.gripcontactpairnormals_precc = []
self.gripcontactpairfacets_precc = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
print(str(self.counter) + "/" + str(self.facetpairs.shape[0]-1))
print(self.gripcontactpairs)
self.gripcontactpairs_precc.append([])
self.gripcontactpairnormals_precc.append([])
self.gripcontactpairfacets_precc.append([])
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
self.gripcontactpairs_precc[-1].append(contactpair)
self.gripcontactpairnormals_precc[-1].append(self.gripcontactpairnormals[self.counter][j])
self.gripcontactpairfacets_precc[-1].append(self.gripcontactpairfacets[self.counter])
self.counter += 1
self.counter=0
def gentpsgrip(self, base):
"""
Originally the code of this function is embedded in the removebadfacet function
It is separated on 20170608 to enable common usage of placements for different hands
:return:
author: weiwei
date: 20170608
"""
self.tpsgripcontacts = []
self.tpsgripnormals = []
self.tpsgriprotmats = []
self.tpsgripjawwidth = []
# the id of the grip in freeair
self.tpsgripidfreeair = []
for i in range(len(self.tpsmat4s)):
self.tpsgripcontacts.append([])
self.tpsgripnormals.append([])
self.tpsgriprotmats.append([])
self.tpsgripjawwidth.append([])
self.tpsgripidfreeair.append([])
for j, rotmat in enumerate(self.freegriprotmats):
tpsgriprotmat = rotmat * self.tpsmat4s[i]
# check if the hand collide with tabletop
# tmprtq85 = self.rtq85hnd
tmphnd = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, 1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# open the hand to ensure it doesnt collide with surrounding obstacles
# tmprtq85.setJawwidth(self.freegripjawwidth[j])
tmphnd.setJawwidth(80)
tmphnd.setMat(tpsgriprotmat)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworldhp.contactTest(hndbullnode)
# print result.getNumContacts()
if not result.getNumContacts():
self.tpsgriprotmats[-1].append(tpsgriprotmat)
cct0 = self.tpsmat4s[i].xformPoint(
self.freegripcontacts[j][0])
cct1 = self.tpsmat4s[i].xformPoint(
self.freegripcontacts[j][1])
self.tpsgripcontacts[-1].append([cct0, cct1])
cctn0 = self.tpsmat4s[i].xformVec(
self.freegripnormals[j][0])
cctn1 = self.tpsmat4s[i].xformVec(
self.freegripnormals[j][1])
self.tpsgripnormals[-1].append([cctn0, cctn1])
self.tpsgripjawwidth[-1].append(self.freegripjawwidth[j])
self.tpsgripidfreeair[-1].append(self.freegripid[j])
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.sucrotmats = []
self.succontacts = []
self.succontactnormals = []
# collided
self.sucrotmatscld = []
self.succontactscld = []
self.succontactnormalscld = []
plotoffsetfp = 3
self.counter = 0
while self.counter < self.facets.shape[0]:
# print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
for i in range(self.objsamplepnts_refcls[self.counter].shape[0]):
for angleid in range(discretesize):
cctpnt = self.objsamplepnts_refcls[self.counter][i] + plotoffsetfp * self.objsamplenrmls_refcls[self.counter][i]
# check torque resistance
print Vec3(cctpnt[0],cctpnt[1],cctpnt[2]).length()
if Vec3(cctpnt[0],cctpnt[1],cctpnt[2]).length() < self.torqueresist:
cctnrml = self.objsamplenrmls_refcls[self.counter][i]
rotangle = 360.0 / discretesize * angleid
tmphand = self.hand
tmphand.attachTo(cctpnt[0], cctpnt[1], cctpnt[2], cctnrml[0], cctnrml[1], cctnrml[2], rotangle)
hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.succontacts.append(self.objsamplepnts_refcls[self.counter][i])
self.succontactnormals.append(self.objsamplenrmls_refcls[self.counter][i])
self.sucrotmats.append(tmphand.getMat())
else:
self.succontactscld.append(self.objsamplepnts_refcls[self.counter][i])
self.succontactnormalscld.append(self.objsamplenrmls_refcls[self.counter][i])
self.sucrotmatscld.append(tmphand.getMat())
self.counter+=1
self.counter = 0
def gentpsgrip(self, base):
"""
Originally the code of this function is embedded in the removebadfacet function
It is separated on 20170608 to enable common usage of placements for different hands
:return:
author: weiwei
date: 20170608
"""
self.tpsgripcontacts = []
self.tpsgripnormals = []
self.tpsgriprotmats = []
self.tpsgripjawwidth = []
# the id of the grip in freeair
self.tpsgripidfreeair = []
for i in range(len(self.tpsmat4s)):
self.tpsgripcontacts.append([])
self.tpsgripnormals.append([])
self.tpsgriprotmats.append([])
self.tpsgripjawwidth.append([])
self.tpsgripidfreeair.append([])
for j, rotmat in enumerate(self.freegriprotmats):
tpsgriprotmat = rotmat * self.tpsmat4s[i]
# check if the hand collide with tabletop
# tmprtq85 = self.rtq85hnd
tmphnd = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, 1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# open the hand to ensure it doesnt collide with surrounding obstacles
# tmprtq85.setJawwidth(self.freegripjawwidth[j])
tmphnd.setJawwidth(80)
tmphnd.setMat(pandanpmat4 = tpsgriprotmat)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworldhp.contactTest(hndbullnode)
# print result.getNumContacts()
if not result.getNumContacts():
self.tpsgriprotmats[-1].append(tpsgriprotmat)
cct0 = self.tpsmat4s[i].xformPoint(self.freegripcontacts[j][0])
cct1 = self.tpsmat4s[i].xformPoint(self.freegripcontacts[j][1])
self.tpsgripcontacts[-1].append([cct0, cct1])
cctn0 = self.tpsmat4s[i].xformVec(self.freegripnormals[j][0])
cctn1 = self.tpsmat4s[i].xformVec(self.freegripnormals[j][1])
self.tpsgripnormals[-1].append([cctn0, cctn1])
self.tpsgripjawwidth[-1].append(self.freegripjawwidth[j])
self.tpsgripidfreeair[-1].append(self.freegripid[j])
tmphnd.setMat(pandanpmat4 = initmat)
tmphnd.setJawwidth(initjawwidth)
def removeFgrpccShowLeft(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
Plot the available grips
:return:
author: weiwei
date: 20161212, tsukuba
"""
plotoffsetfp = 6
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
return
else:
print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
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("rtq85fgrpcc1")
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(base.render)
handfgrpcc1.reparentTo(base.render)
def genHandPairs(self, base, loadser=False):
self.handpairList = []
if loadser is False:
# hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
# hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
pairidlist = list(itertools.combinations(range(len(self.freegripids)), 2))
print len(pairidlist)/5000+1
for i in range(100,len(pairidlist),len(pairidlist)/5000+1):
# for i0, i1 in pairidlist:
i0, i1 = pairidlist[i]
print i, len(pairidlist)
self.hand0.setMat(pandanpmat4 = self.freegriprotmats[i0])
self.hand0.setJawwidth(self.freegripjawwidth[i0])
self.hand1.setMat(pandanpmat4 = self.freegriprotmats[i1])
self.hand1.setJawwidth(self.freegripjawwidth[i1])
hndbullnodei0 = cd.genCollisionMeshMultiNp(self.hand0.handnp, base.render)
hndbullnodei1 = cd.genCollisionMeshMultiNp(self.hand1.handnp, base.render)
result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1)
if not result.getNumContacts():
self.handpairList.append([self.freegripids[i0], self.freegripids[i1]])
pickle.dump(self.handpairList, open("tmp.pickle", mode="wb"))
else:
self.handpairList = pickle.load(open("tmp.pickle", mode="rb"))
def __genHandPairs(self, base):
self.handpairList = []
self.__genFreeAssGrips()
ass0gripcontacts, ass0gripnormals, ass0griprotmat4s, ass0gripjawwidth, ass0gripidfreeair = self.obj0grips
ass1gripcontacts, ass1gripnormals, ass1griprotmat4s, ass1gripjawwidth, ass1gripidfreeair = self.obj1grips
hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
i0list = range(len(ass0gripidfreeair))
i1list = range(len(ass1gripidfreeair))
pairidlist = list(itertools.product(i0list, i1list))
print len(pairidlist)/10000+1
for i in range(0,len(pairidlist),len(pairidlist)/10000+1):
# for i0, i1 in pairidlist:
i0, i1 = pairidlist[i]
print i, len(pairidlist)
hand0.setMat(ass0griprotmat4s[i0])
hand0.setJawwidth(ass0gripjawwidth[i0])
hand1.setMat(ass1griprotmat4s[i1])
hand1.setJawwidth(ass1gripjawwidth[i1])
hndbullnodei0 = cd.genCollisionMeshMultiNp(hand0.handnp, base.render)
hndbullnodei1 = cd.genCollisionMeshMultiNp(hand1.handnp, base.render)
result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1)
if not result.getNumContacts():
self.handpairList.append([ass0gripidfreeair[i0], ass1gripidfreeair[i1]])
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 removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = tmphand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
tmphand.setJawwidth(fgrdist)
tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmphand.setMat(pandageom.cvtMat4(rotmat) * tmphand.getMat())
axx = tmphand.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
tmphand.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmphand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j])
# pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
# tmprtq85.setColor([.5, .5, .5, 1])
# tmprtq85.reparentTo(base.render)
# self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
# reset initial hand pose
tmphand.setMat(initmat)
self.counter+=1
self.counter = 0
def isCollided(self, robot, obstaclelist):
"""
simultaneously check self-collision and robot-obstacle collision
this function should be faster than calling isselfcollided and isobstaclecollided one after another
:param robot:
:param obstaclelist:
:return:
author: weiwei
date: 20170613
"""
print self.counter
if self.counter > 200:
self.counter = 0
self.bulletworld = BulletWorld()
gc.collect()
self.counter += 1
dualmnps = self.robotmesh.genmnp_list(robot)
sglbullnodesrgt = []
sglbullnodeslft = []
# rgt arm
# single arm check
sglmnps = dualmnps[0]
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
# lft arm
sglmnps = dualmnps[1]
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
# arm obstacle check
# only check hands
nlinkrgt = len(sglbullnodesrgt)
nlinklft = len(sglbullnodeslft)
for obstaclemnp in obstaclelist:
obstaclebullnode = cd.genCollisionMeshNp(obstaclemnp, basenodepath=None, name='autogen')
for i in range(nlinkrgt - 1, nlinkrgt):
result = self.bulletworld.contactTestPair(sglbullnodesrgt[i], obstaclebullnode)
if result.getNumContacts():
print "rgtarm-obstacle collision!"
return True
for i in range(nlinklft - 1, nlinklft):
result = self.bulletworld.contactTestPair(sglbullnodeslft[i], obstaclebullnode)
if result.getNumContacts():
print "lftarm-obstacle collision!"
return True
#
# # sgl arm check
# if self.__isSglArmCollided(sglbullnodesrgt):
# print "right arm self collision!"
# return True
# else:
# if self.__isSglArmCollided(sglbullnodeslft):
# print "left arm self collision!"
# return True
#
# # dual arm check
# if self.__isDualArmCollided(sglbullnodesrgt, sglbullnodeslft):
# print "left-right arm self collision!"
# return True
# #
# # arm body check
# bodymnps = dualmnps[2]
# bdybullnodes = []
# if bodymnps:
# for bodymnp in bodymnps:
# bodybullnode = cd.genCollisionMeshMultiNp(bodymnp, basenodepath=None, name='autogen')
# bdybullnodes.append(bodybullnode)
# if self.__isSglArmBdyCollided(sglbullnodesrgt, bdybullnodes):
# print "right arm body self collision!"
# return True
# if self.__isSglArmBdyCollided(sglbullnodeslft, bdybullnodes):
# print "left right body arm self collision!"
# return True
# for bullnode in sglbullnodesrgt:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in sglbullnodeslft:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in bdybullnodes:
# self.bulletworld.attachRigidBody(bullnode)
return False
def isSelfCollided(self, robot):
"""
check the collision of a single arm
:param armid: 'lft' or 'rgt'
:return:
author: weiwei
date: 20170608
"""
dualmnps = self.robotmesh.genmnp_list(robot)
# single arm check
sglmnps = dualmnps[0]
sglbullnodesrgt = []
sglbullnodeslft = []
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodesrgt.append(sglbullnode)
if self.__isSglArmCollided(sglbullnodesrgt):
print "right arm self collision!"
return True
else:
sglmnps = dualmnps[1]
# armbase is not examined
for sglmnp in sglmnps[1:len(sglmnps)-1]:
sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
# hand is multinp
sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen')
sglbullnodeslft.append(sglbullnode)
if self.__isSglArmCollided(sglbullnodeslft):
print "left arm self collision!"
return True
# dual arm check
if self.__isDualArmCollided(sglbullnodesrgt, sglbullnodeslft):
print "left-right arm self collision!"
return True
#
# arm body check
bodymnps = dualmnps[2]
bdybullnodes = []
if bodymnps:
for bodymnp in bodymnps:
bodybullnode = cd.genCollisionMeshMultiNp(bodymnp, basenodepath=None, name='autogen')
bdybullnodes.append(bodybullnode)
if self.__isSglArmBdyCollided(sglbullnodesrgt, bdybullnodes):
print "right arm body self collision!"
return True
if self.__isSglArmBdyCollided(sglbullnodeslft, bdybullnodes):
print "left right body arm self collision!"
return True
# for bullnode in sglbullnodesrgt[-4:]:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in sglbullnodeslft:
# self.bulletworld.attachRigidBody(bullnode)
# for bullnode in bdybullnodes:
# self.bulletworld.attachRigidBody(bullnode)
return False
def __addstartgoal(self, startrotmat4, goalrotmat4, base):
"""
add start and goal for the regg
:param startrotmat4 and goalrotmat4: both are 4by4 panda3d matrix
:return:
author: weiwei
date: 20161216, sapporo
"""
### start
# the node id of a globalgripid in startnode
nodeidofglobalidinstart= {}
# the startnodeids is also for quick access
self.startnodeids = []
for j, rotmat in enumerate(self.freegriprotmats):
print j, len(self.freegriprotmats)
# print rotmat
ttgsrotmat = rotmat * startrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(startrotmat4)
ttgsrotmatx0y0.setCell(3,0,0)
ttgsrotmatx0y0.setCell(3,1,0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
# tmphnd = self.robothand
tmphnd = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# set jawwidth to 80 to avoid collision with surrounding obstacles
# set to gripping with is unnecessary
tmphnd.setJawwidth(80)
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
# tmphnd.setMat(ttgsrotmat)
# tmphnd.reparentTo(base.render)
# if j > 3:
# base.run()
if not result.getNumContacts():
ttgscct0=startrotmat4.xformPoint(self.freegripcontacts[j][0])
ttgscct1=startrotmat4.xformPoint(self.freegripcontacts[j][1])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
# handxworldz is not necessary for start
# ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsjawwidth = self.freegripjawwidth[j]
ttgsidfreeair = self.freegripid[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
# handxworldz is not necessary for start
# ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
print "solving starting iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = startrotmat4.getRow3(3)
startrotmat4worlda = Mat4(startrotmat4)
startrotmat4worlda.setRow(3, startrotmat4.getRow3(3)+self.worlda*self.retworlda)
startrotmat4worldaworldz = Mat4(startrotmat4worlda)
startrotmat4worldaworldz.setRow(3, startrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('start'+str(j), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = 'na',
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = 'na',
tabletopplacementrotmat = startrotmat4,
tabletopplacementrotmathandx = startrotmat4,
tabletopplacementrotmathandxworldz = 'na',
tabletopplacementrotmatworlda = startrotmat4worlda,
tabletopplacementrotmatworldaworldz = startrotmat4worldaworldz,
angle = 'na', tabletopposition = tabletopposition)
nodeidofglobalidinstart[ttgsidfreeair]='start'+str(j)
self.startnodeids.append('start'+str(j))
# tmprtq85.reparentTo(base.render)
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
if len(self.startnodeids) == 0:
raise ValueError("No available starting grip!")
# add start transit edge
for edge in list(itertools.combinations(self.startnodeids, 2)):
self.regg.add_edge(*edge, weight = 1, edgetype = 'starttransit')
# add start transfer edge
for reggnode, reggnodedata in self.regg.nodes(data=True):
if reggnode.startswith('mid'):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidinstart.keys():
startnodeid = nodeidofglobalidinstart[globalgripid]
self.regg.add_edge(startnodeid, reggnode, weight=1, edgetype = 'starttransfer')
### goal
# the node id of a globalgripid in goalnode, for quick setting up edges
nodeidofglobalidingoal= {}
# the goalnodeids is also for quick access
self.goalnodeids = []
for j, rotmat in enumerate(self.freegriprotmats):
print j, len(self.freegriprotmats)
ttgsrotmat = rotmat * goalrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(goalrotmat4)
ttgsrotmatx0y0.setCell(3,0,0)
ttgsrotmatx0y0.setCell(3,1,0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
tmphnd = self.robothand
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
tmphnd.setJawwidth(self.freegripjawwidth[j])
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
ttgscct0=goalrotmat4.xformPoint(self.freegripcontacts[j][0])
ttgscct1=goalrotmat4.xformPoint(self.freegripcontacts[j][1])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsjawwidth = self.freegripjawwidth[j]
ttgsidfreeair = self.freegripid[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
print "solving goal iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikcxz = self.robot.numikr(ttgsfgrcenternp_handxworldz, ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (ikcxz is not None) \
and (ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = goalrotmat4.getRow3(3)
goalrotmat4worlda = Mat4(goalrotmat4)
goalrotmat4worlda.setRow(3, goalrotmat4.getRow3(3)+self.worlda*self.retworlda)
goalrotmat4worldaworldz = Mat4(goalrotmat4worlda)
goalrotmat4worldaworldz.setRow(3, goalrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('goal'+str(j), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz,
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = 'na',
tabletopplacementrotmat = goalrotmat4,
tabletopplacementrotmathandx = goalrotmat4,
tabletopplacementrotmathandxworldz = goalrotmat4,
tabletopplacementrotmatworlda = goalrotmat4worlda,
tabletopplacementrotmatworldaworldz = goalrotmat4worldaworldz,
angleid = 'na', tabletopposition = tabletopposition)
nodeidofglobalidingoal[ttgsidfreeair]='goal'+str(j)
self.goalnodeids.append('goal'+str(j))
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
if len(self.goalnodeids) == 0:
raise ValueError("No available goal grip!")
# add goal transit edges
for edge in list(itertools.combinations(self.goalnodeids, 2)):
self.regg.add_edge(*edge, weight = 1, edgetype = 'goaltransit')
# add goal transfer edge
for reggnode, reggnodedata in self.regg.nodes(data=True):
if reggnode.startswith('mid'):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidingoal.keys():
goalnodeid = nodeidofglobalidingoal[globalgripid]
self.regg.add_edge(goalnodeid, reggnode, weight=1, edgetype = 'goaltransfer')
# add start to goal direct edges
for startnodeid in self.startnodeids:
for goalnodeid in self.goalnodeids:
# startnodeggid = start node global grip id
startnodeggid = self.regg.node[startnodeid]['globalgripid']
goalnodeggid = self.regg.node[goalnodeid]['globalgripid']
if startnodeggid == goalnodeggid:
self.regg.add_edge(startnodeid, goalnodeid, weight=1, edgetype = 'startgoaltransfer')
def __addstartgoal(self, startrotmat4, goalrotmat4, base):
"""
add start and goal for the regg
:param startrotmat4 and goalrotmat4: both are 4by4 panda3d matrix
:return:
author: weiwei
date: 20161216, sapporo
"""
### start
# the node id of a globalgripid in startnode
nodeidofglobalidinstart= {}
# the startnodeids is also for quick access
self.startnodeids = []
for j, rotmat in enumerate(self.freegriprotmats1):
#print j, len(self.freegriprotmats)
# print rotmat
ttgsrotmat = rotmat * startrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(startrotmat4)
ttgsrotmatx0y0.setCell(3,0,0)
ttgsrotmatx0y0.setCell(3,1,0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
# tmphnd = self.robothand
tmphnd = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# set jawwidth to 80 to avoid collision with surrounding obstacles
# set to gripping with is unnecessary
tmphnd.setJawwidth(80)
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
# tmphnd.setMat(ttgsrotmat)
# tmphnd.reparentTo(base.render)
# if j > 3:
# base.run()
if not result.getNumContacts():
ttgscct0=startrotmat4.xformPoint(self.freegripcontacts1[j][0])
ttgscct1=startrotmat4.xformPoint(self.freegripcontacts1[j][1])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
# handxworldz is not necessary for start
# ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsjawwidth = self.freegripjawwidth1[j]
ttgsidfreeair = self.freegripid1[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
# handxworldz is not necessary for start
# ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
#print "solving starting iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = startrotmat4.getRow3(3)
startrotmat4worlda = Mat4(startrotmat4)
startrotmat4worlda.setRow(3, startrotmat4.getRow3(3)+self.worlda*self.retworlda)
startrotmat4worldaworldz = Mat4(startrotmat4worlda)
startrotmat4worldaworldz.setRow(3, startrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('start'+str(j), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = 'na',
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = 'na',
tabletopplacementrotmat = startrotmat4,
tabletopplacementrotmathandx = startrotmat4,
tabletopplacementrotmathandxworldz = 'na',
tabletopplacementrotmatworlda = startrotmat4worlda,
tabletopplacementrotmatworldaworldz = startrotmat4worldaworldz,
angle = 'na', tabletopposition = tabletopposition)
nodeidofglobalidinstart[ttgsidfreeair]='start'+str(j)
self.startnodeids.append('start'+str(j))
# tmprtq85.reparentTo(base.render)
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
if len(self.startnodeids) == 0:
#raise ValueError("No available starting grip!")
print ("No available start grip!")
return False
# add start transit edge
for edge in list(itertools.combinations(self.startnodeids, 2)):
self.regg.add_edge(*edge, weight = 1, edgetype = 'starttransit')
# add start transfer edge
for reggnode, reggnodedata in self.regg.nodes(data=True):
if reggnode.startswith('mid'):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidinstart.keys():
startnodeid = nodeidofglobalidinstart[globalgripid]
self.regg.add_edge(startnodeid, reggnode, weight=1, edgetype = 'starttransfer')
### goal
# the node id of a globalgripid in goalnode, for quick setting up edges
nodeidofglobalidingoal= {}
# the goalnodeids is also for quick access
self.goalnodeids = []
for j, rotmat in enumerate(self.freegriprotmats1):
#print j, len(self.freegriprotmats)
ttgsrotmat = rotmat * goalrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(goalrotmat4)
ttgsrotmatx0y0.setCell(3,0,0)
ttgsrotmatx0y0.setCell(3,1,0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
tmphnd = self.robothand
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
tmphnd.setJawwidth(self.freegripjawwidth1[j])
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
ttgscct0=goalrotmat4.xformPoint(self.freegripcontacts1[j][0])
ttgscct1=goalrotmat4.xformPoint(self.freegripcontacts1[j][1])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsjawwidth = self.freegripjawwidth1[j]
ttgsidfreeair = self.freegripid1[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
#print "solving goal iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikcxz = self.robot.numikr(ttgsfgrcenternp_handxworldz, ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (ikcxz is not None) \
and (ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = goalrotmat4.getRow3(3)
goalrotmat4worlda = Mat4(goalrotmat4)
goalrotmat4worlda.setRow(3, goalrotmat4.getRow3(3)+self.worlda*self.retworlda)
goalrotmat4worldaworldz = Mat4(goalrotmat4worlda)
goalrotmat4worldaworldz.setRow(3, goalrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('goal'+str(j), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz,
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = 'na',
tabletopplacementrotmat = goalrotmat4,
tabletopplacementrotmathandx = goalrotmat4,
tabletopplacementrotmathandxworldz = goalrotmat4,
tabletopplacementrotmatworlda = goalrotmat4worlda,
tabletopplacementrotmatworldaworldz = goalrotmat4worldaworldz,
angleid = 'na', tabletopposition = tabletopposition)
nodeidofglobalidingoal[ttgsidfreeair]='goal'+str(j)
self.goalnodeids.append('goal'+str(j))
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
if len(self.goalnodeids) == 0:
#raise ValueError("No available goal grip!")
print ("No available goal grip!")
return False
# add goal transit edges
for edge in list(itertools.combinations(self.goalnodeids, 2)):
self.regg.add_edge(*edge, weight = 1, edgetype = 'goaltransit')
# add goal transfer edge
for reggnode, reggnodedata in self.regg.nodes(data=True):
if reggnode.startswith('mid'):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidingoal.keys():
goalnodeid = nodeidofglobalidingoal[globalgripid]
self.regg.add_edge(goalnodeid, reggnode, weight=1, edgetype = 'goaltransfer')
# add start to goal direct edges
for startnodeid in self.startnodeids:
for goalnodeid in self.goalnodeids:
# startnodeggid = start node global grip id
startnodeggid = self.regg.node[startnodeid]['globalgripid']
goalnodeggid = self.regg.node[goalnodeid]['globalgripid']
if startnodeggid == goalnodeggid:
self.regg.add_edge(startnodeid, goalnodeid, weight=1, edgetype = 'startgoaltransfer')
return True
# print result1
# print result1.getContacts()
# print result2
# print result2.getContacts()
# for contact in result.getContacts():
# cp = contact.getManifoldPoint()
# print cp.getLocalPointA()
return task.cont
base = pandactrl.World()
# first hand
hrp5three = newHandNM(hndcolor=[.5, .5,.5,.7])
hrp5three.gripAt(100,0,0,0,0,1,35,jawwidth = 30)
hrp5three.reparentTo(base.render)
hrp5three.handpccnp.reparentTo(base.render)
handbullnp = cd.genCollisionMeshMultiNp(hrp5three.handnp)
# # second hand
# hrp5three1 = newHandNM()
# # hrp5three1.reparentTo(base.render)
# hand1bullnp = cd.genCollisionMeshMultiNp(hrp5three.handnp)
#
pg.plotAxisSelf(base.render, Vec3(0,0,0))
#
bullcldrnp = base.render.attachNewNode("bulletcollider")
base.world = BulletWorld()
#
base.taskMgr.add(updateworld, "updateworld", extraArgs=[base.world], appendTask=True)
# result = base.world.contactTest(hand1bullnp)
base.world.attachRigidBody(handbullnp)
# for contact in result.getContacts():
# cp = contact.getManifoldPoint()
def addEnds(self, rotmat4, armname):
# the node id of a globalgripid in end
nodeidofglobalidinend = {}
# the endnodeids is also for quick access
self.endnodeids[armname] = []
for j, rotmat in enumerate(self.freegriprotmats):
assgsrotmat = rotmat * rotmat4
# for collision detection, we move the object back to x=0,y=0
assgsrotmatx0y0 = Mat4(rotmat4)
assgsrotmatx0y0.setCell(3, 0, 0)
assgsrotmatx0y0.setCell(3, 1, 0)
assgsrotmatx0y0 = rotmat * assgsrotmatx0y0
# check if the hand collide with tabletop
tmphand = self.hand
initmat = tmphand.getMat()
initjawwidth = tmphand.jawwidth
# set jawwidth to 80 to avoid collision with surrounding obstacles
# set to gripping with is unnecessary
# tmphand.setJawwidth(self.freegripjawwidth[j])
tmphand.setJawwidth(tmphand.jawwidthopen)
tmphand.setMat(assgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
assgscct0 = rotmat4.xformPoint(self.freegripcontacts[j][0])
assgscct1 = rotmat4.xformPoint(self.freegripcontacts[j][1])
assgsfgrcenter = (assgscct0 + assgscct1) / 2
handx = assgsrotmat.getRow3(0)
assgsfgrcenterhandx = assgsfgrcenter + handx * self.rethandx
# handxworldz is not necessary for start
# assgsfgrcenterhandxworldz = assgsfgrcenterhandx + self.worldz*self.retworldz
assgsfgrcenterworlda = assgsfgrcenter + self.worlda * self.retworlda
assgsfgrcenterworldaworldz = assgsfgrcenterworlda + self.worldz * self.retworldz
assgsjawwidth = self.freegripjawwidth[j]
assgsidfreeair = self.freegripid[j]
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsfgrcenternp_handx = pg.v3ToNp(assgsfgrcenterhandx)
# handxworldz is not necessary for start
# assgsfgrcenternp_handxworldz = pg.v3ToNp(assgsfgrcenterhandxworldz)
assgsfgrcenternp_worlda = pg.v3ToNp(assgsfgrcenterworlda)
assgsfgrcenternp_worldaworldz = pg.v3ToNp(
assgsfgrcenterworldaworldz)
assgsrotmat3np = pg.mat3ToNp(assgsrotmat.getUpper3())
ikc = self.robot.numikr(assgsfgrcenternp, assgsrotmat3np)
ikcx = self.robot.numikr(assgsfgrcenternp_handx,
assgsrotmat3np)
ikca = self.robot.numikr(assgsfgrcenternp_worlda,
assgsrotmat3np)
ikcaz = self.robot.numikr(assgsfgrcenternp_worldaworldz,
assgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (
ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = rotmat4.getRow3(3)
startrotmat4worlda = Mat4(rotmat4)
startrotmat4worlda.setRow(
3,
rotmat4.getRow3(3) + self.worlda * self.retworlda)
startrotmat4worldaworldz = Mat4(startrotmat4worlda)
startrotmat4worldaworldz.setRow(
3,
startrotmat4worlda.getRow3(3) +
self.worldz * self.retworldz)
self.graphtpp.regg.add_node(
'end' + armname + str(j),
fgrcenter=assgsfgrcenternp,
fgrcenterhandx=assgsfgrcenternp_handx,
fgrcenterhandxworldz='na',
fgrcenterworlda=assgsfgrcenternp_worlda,
fgrcenterworldaworldz=assgsfgrcenternp_worldaworldz,
jawwidth=assgsjawwidth,
hndrotmat3np=assgsrotmat3np,
globalgripid=assgsidfreeair,
freetabletopplacementid='na',
tabletopplacementrotmat=rotmat4,
tabletopplacementrotmathandx=rotmat4,
tabletopplacementrotmathandxworldz='na',
tabletopplacementrotmatworlda=startrotmat4worlda,
tabletopplacementrotmatworldaworldz=
startrotmat4worldaworldz,
angle='na',
tabletopposition=tabletopposition)
nodeidofglobalidinend[assgsidfreeair] = 'start' + str(j)
self.endnodeids[armname].append('start' + str(j))
tmphand.setMat(initmat)
tmphand.setJawwidth(initjawwidth)
if len(self.endnodeids[armname]) == 0:
raise ValueError("No available end grip at " + armname)
# add start transit edge
for edge in list(itertools.combinations(self.endnodeids[armname], 2)):
self.graphtpp.regg.add_edge(*edge, weight=1, edgetype='endtransit')
# add start transfer edge
for reggnode, reggnodedata in self.graphtpp.regg.nodes(data=True):
if reggnode.startswith(self.armname):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidinend.keys():
endnodeid = nodeidofglobalidinend[globalgripid]
self.graphtpp.regg.add_edge(endnodeid,
reggnode,
weight=1,
edgetype='endtransfer')
for nid in self.graphtpp.regg.nodes():
if nid.startswith('end'):
ggid = self.graphtpp.regg.node[nid]['globalgripid']
tabletopposition = self.graphtpp.regg.node[nid][
'tabletopposition']
xyzpos = map(add, self.__xyzglobalgrippos_startgoal[ggid], [
tabletopposition[0], tabletopposition[1],
tabletopposition[2]
])
self.gnodesplotpos[nid] = xyzpos[:2]
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.sucrotmats = []
self.succontacts = []
self.succontactnormals = []
# collided
self.sucrotmatscld = []
self.succontactscld = []
self.succontactnormalscld = []
plotoffsetfp = 3
self.counter = 0
while self.counter < self.facets.shape[0]:
# print(str(self.counter) + "/" + str(self.facetpairs.shape[0]-1))
# print(self.gripcontactpairs_precc)
for i in range(self.objsamplepnts_refcls[self.counter].shape[0]):
for angleid in range(discretesize):
cctpnt = self.objsamplepnts_refcls[self.counter][
i] + plotoffsetfp * self.objsamplenrmls_refcls[
self.counter][i]
# check torque resistance
print(Vec3(cctpnt[0], cctpnt[1], cctpnt[2]).length())
if Vec3(cctpnt[0], cctpnt[1],
cctpnt[2]).length() < self.torqueresist:
cctnrml = self.objsamplenrmls_refcls[self.counter][i]
rotangle = 360.0 / discretesize * angleid
tmphand = self.hand
tmphand.attachTo(cctpnt[0], cctpnt[1], cctpnt[2],
cctnrml[0], cctnrml[1], cctnrml[2],
rotangle)
hndbullnode = cd.genCollisionMeshMultiNp(
tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.succontacts.append(
self.objsamplepnts_refcls[self.counter][i])
self.succontactnormals.append(
self.objsamplenrmls_refcls[self.counter][i])
self.sucrotmats.append(tmphand.getMat())
else:
self.succontactscld.append(
self.objsamplepnts_refcls[self.counter][i])
self.succontactnormalscld.append(
self.objsamplenrmls_refcls[self.counter][i])
self.sucrotmatscld.append(tmphand.getMat())
self.counter += 1
self.counter = 0
# print result1
# print result1.getContacts()
# print result2
# print result2.getContacts()
# for contact in result.getContacts():
# cp = contact.getManifoldPoint()
# print cp.getLocalPointA()
return task.cont
base = pandactrl.World()
# first hand
hrp5three = newHand(hndid='lft')
hrp5three.gripAt(100,0,0,0,0,1,35,jawwidth = 30)
hrp5three.reparentTo(base.render)
hrp5three.handpccnp.reparentTo(base.render)
handbullnp = cd.genCollisionMeshMultiNp(hrp5three.handnp)
# second hand
# hrp5three1 = newHand(hndid='rgt')
# hrp5three1.reparentTo(base.render)
# hrp5three1.fgrtippccleft.reparentTo(base.render)
# hrp5three1.fgrtippccright.reparentTo(base.render)
# hand1bullnp = cd.genCollisionMeshNp(hrp5three.handnp)
pg.plotAxisSelf(base.render, Vec3(0,0,0))
bullcldrnp = base.render.attachNewNode("bulletcollider")
base.world = BulletWorld()
base.taskMgr.add(updateworld, "updateworld", extraArgs=[base.world], appendTask=True)
base.world.attachRigidBody(handbullnp)
# result = base.world.contactTestPair(handbullnp, hand1bullnp)
def addEnds(self, rotmat4, armname):
# the node id of a globalgripid in end
nodeidofglobalidinend = {}
# the endnodeids is also for quick access
self.endnodeids[armname] = []
for j, rotmat in enumerate(self.freegriprotmats):
assgsrotmat = rotmat * rotmat4
# for collision detection, we move the object back to x=0,y=0
assgsrotmatx0y0 = Mat4(rotmat4)
assgsrotmatx0y0.setCell(3,0,0)
assgsrotmatx0y0.setCell(3,1,0)
assgsrotmatx0y0 = rotmat * assgsrotmatx0y0
# check if the hand collide with tabletop
tmphand = self.hand
initmat = tmphand.getMat()
initjawwidth = tmphand.jawwidth
# set jawwidth to 80 to avoid collision with surrounding obstacles
# set to gripping with is unnecessary
# tmphand.setJawwidth(self.freegripjawwidth[j])
tmphand.setJawwidth(tmphand.jawwidthopen)
tmphand.setMat(assgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
assgscct0=rotmat4.xformPoint(self.freegripcontacts[j][0])
assgscct1=rotmat4.xformPoint(self.freegripcontacts[j][1])
assgsfgrcenter = (assgscct0+assgscct1)/2
handx = assgsrotmat.getRow3(0)
assgsfgrcenterhandx = assgsfgrcenter + handx*self.rethandx
# handxworldz is not necessary for start
# assgsfgrcenterhandxworldz = assgsfgrcenterhandx + self.worldz*self.retworldz
assgsfgrcenterworlda = assgsfgrcenter + self.worlda*self.retworlda
assgsfgrcenterworldaworldz = assgsfgrcenterworlda+ self.worldz*self.retworldz
assgsjawwidth = self.freegripjawwidth[j]
assgsidfreeair = self.freegripid[j]
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsfgrcenternp_handx = pg.v3ToNp(assgsfgrcenterhandx)
# handxworldz is not necessary for start
# assgsfgrcenternp_handxworldz = pg.v3ToNp(assgsfgrcenterhandxworldz)
assgsfgrcenternp_worlda = pg.v3ToNp(assgsfgrcenterworlda)
assgsfgrcenternp_worldaworldz = pg.v3ToNp(assgsfgrcenterworldaworldz)
assgsrotmat3np = pg.mat3ToNp(assgsrotmat.getUpper3())
ikc = self.robot.numikr(assgsfgrcenternp, assgsrotmat3np)
ikcx = self.robot.numikr(assgsfgrcenternp_handx, assgsrotmat3np)
ikca = self.robot.numikr(assgsfgrcenternp_worlda, assgsrotmat3np)
ikcaz = self.robot.numikr(assgsfgrcenternp_worldaworldz, assgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = rotmat4.getRow3(3)
startrotmat4worlda = Mat4(rotmat4)
startrotmat4worlda.setRow(3, rotmat4.getRow3(3)+self.worlda*self.retworlda)
startrotmat4worldaworldz = Mat4(startrotmat4worlda)
startrotmat4worldaworldz.setRow(3, startrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.graphtpp.regg.add_node('end'+armname+str(j), fgrcenter=assgsfgrcenternp,
fgrcenterhandx = assgsfgrcenternp_handx,
fgrcenterhandxworldz = 'na',
fgrcenterworlda = assgsfgrcenternp_worlda,
fgrcenterworldaworldz = assgsfgrcenternp_worldaworldz,
jawwidth=assgsjawwidth, hndrotmat3np=assgsrotmat3np,
globalgripid = assgsidfreeair, freetabletopplacementid = 'na',
tabletopplacementrotmat = rotmat4,
tabletopplacementrotmathandx = rotmat4,
tabletopplacementrotmathandxworldz = 'na',
tabletopplacementrotmatworlda = startrotmat4worlda,
tabletopplacementrotmatworldaworldz = startrotmat4worldaworldz,
angle = 'na', tabletopposition = tabletopposition)
nodeidofglobalidinend[assgsidfreeair]='start'+str(j)
self.endnodeids[armname].append('start'+str(j))
tmphand.setMat(initmat)
tmphand.setJawwidth(initjawwidth)
if len(self.endnodeids[armname]) == 0:
raise ValueError("No available end grip at " + armname)
# add start transit edge
for edge in list(itertools.combinations(self.endnodeids[armname], 2)):
self.graphtpp.regg.add_edge(*edge, weight = 1, edgetype = 'endtransit')
# add start transfer edge
for reggnode, reggnodedata in self.graphtpp.regg.nodes(data=True):
if reggnode.startswith(self.armname):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidinend.keys():
endnodeid = nodeidofglobalidinend[globalgripid]
self.graphtpp.regg.add_edge(endnodeid, reggnode, weight=1, edgetype = 'endtransfer')
for nid in self.graphtpp.regg.nodes():
if nid.startswith('end'):
ggid = self.graphtpp.regg.node[nid]['globalgripid']
tabletopposition = self.graphtpp.regg.node[nid]['tabletopposition']
xyzpos = map(add, self.__xyzglobalgrippos_startgoal[ggid],
[tabletopposition[0], tabletopposition[1], tabletopposition[2]])
self.gnodesplotpos[nid] = xyzpos[:2]
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 savegoalik(self, goalrotmat4, goalid, base):
### goal
# the node id of a globalgripid in goalnode, for quick setting up edges
nodeidofglobalidingoal = {}
# the goalnodeids is also for quick access
self.goalnodeids = []
for j, rotmat in enumerate(self.freegriprotmats):
# print j, len(self.freegriprotmats)
ttgsrotmat = rotmat * goalrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(goalrotmat4)
ttgsrotmatx0y0.setCell(3, 0, 0)
ttgsrotmatx0y0.setCell(3, 1, 0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
tmphnd = self.robothand
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
tmphnd.setJawwidth(self.freegripjawwidth[j])
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
ttgscct0 = goalrotmat4.xformPoint(self.freegripcontacts[j][0])
ttgscct1 = goalrotmat4.xformPoint(self.freegripcontacts[j][1])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx * self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz * self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda * self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + self.worldz * self.retworldz
ttgsjawwidth = self.freegripjawwidth[j]
ttgsidfreeair = self.freegripid[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(
ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(
ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
# print "solving goal iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikcxz = self.robot.numikr(ttgsfgrcenternp_handxworldz,
ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz,
ttgsrotmat3np)
feasibility = {}
feasibility_handx = {}
feasibility_handxworldz = {}
feasibility_worlda = {}
feasibility_worldaworldz = {}
if ikc is not None:
feasibility[j] = 'True'
else:
feasibility[j] = 'False'
if ikcx is not None:
feasibility_handx[j] = 'True'
else:
feasibility_handx[j] = 'False'
if ikcxz is not None:
feasibility_handxworldz[j] = 'True'
else:
feasibility_handxworldz[j] = 'False'
if ikca is not None:
feasibility_worlda[j] = 'True'
else:
feasibility_worlda[j] = 'False'
if ikcaz is not None:
feasibility_worldaworldz[j] = 'True'
else:
feasibility_worldaworldz[j] = 'False'
sql = "INSERT IGNORE INTO ik_goal(idgoal,idfreeairgrip, feasibility, feasibility_handx, feasibility_handxworldz, \
feasibility_worlda, feasibility_worldaworldz) VALUES (%d,%d, '%s', '%s', '%s', '%s', '%s')" % \
(goalid, j, feasibility[j], feasibility_handx[j], \
feasibility_handxworldz[j], \
feasibility_worlda[j], feasibility_worldaworldz[j])
self.gdb.execute(sql)
def removeHndccShow(self, base, discretesize=8):
"""
Handcc means hand collision detection
This one is developed for demonstration
This function should be called after executing removeHndcc
:param discretesize: the number of hand orientations
:return: delayTime
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
if self.rtq85plotlist:
for rtq85plotnode in self.rtq85plotlist:
rtq85plotnode.removeNode()
self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
if self.counter2 == 0:
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
self.counter2 += 1
if self.counter2 >= discretesize:
self.counter2 = 0
print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1)
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(
self.gripcontactpairs_precc[self.counter]):
if j == 0:
print j, contactpair
# for angleid in range(discretesize):
angleid = self.counter2
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[
self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
# TODO setting jawwidth to 80 is enough
# since fgrpcc already detects inner collisions
tmprtq85.setJawwidth(fgrdist)
tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat())
axx = tmprtq85.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array(
[axx[0], axx[1], axx[2]])
tmprtq85.setPos(
Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
self.hndbullnode = cd.genCollisionMeshMultiNp(
tmprtq85.rtq85np, base.render)
result = self.bulletworld.contactTest(self.hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmprtq85.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(
self.gripcontactpairnormals_precc[self.counter][j])
# pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
tmprtq85.setColor([1, 1, 1, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
else:
for contact in result.getContacts():
# cp = contact.getManifoldPoint()
# pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1))
# pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1))
tmprtq85.setColor([.5, 0, 0, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
def removeHndccShow(self, base, discretesize=8):
"""
Handcc means hand collision detection
This one is developed for demonstration
This function should be called after executing removeHndcc
:param discretesize: the number of hand orientations
:return: delayTime
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
if self.rtq85plotlist:
for rtq85plotnode in self.rtq85plotlist:
rtq85plotnode.removeNode()
self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
if self.counter2 == 0:
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
self.counter2 += 1
if self.counter2 >= discretesize:
self.counter2 = 0
print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]):
if j == 0:
print j, contactpair
# for angleid in range(discretesize):
angleid = self.counter2
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
# TODO setting jawwidth to 80 is enough
# since fgrpcc already detects inner collisions
tmprtq85.setJawwidth(fgrdist)
tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat())
axx = tmprtq85.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
tmprtq85.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
self.hndbullnode = cd.genCollisionMeshMultiNp(tmprtq85.rtq85np, base.render)
result = self.bulletworld.contactTest(self.hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmprtq85.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j])
# pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
tmprtq85.setColor([1, 1, 1, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
else:
for contact in result.getContacts():
# cp = contact.getManifoldPoint()
# pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1))
# pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1))
tmprtq85.setColor([.5, 0, 0, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
# print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(
self.gripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[
self.counter][j][0]
cctnormal1 = [
-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]
]
tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = tmphand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
tmphand.setJawwidth(fgrdist)
tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmphand.setMat(
pandageom.cvtMat4(rotmat) * tmphand.getMat())
axx = tmphand.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array(
[axx[0], axx[1], axx[2]])
tmphand.setPos(
Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(
tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmphand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(
self.gripcontactpairnormals_precc[self.counter][j])
# pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
# tmprtq85.setColor([.5, .5, .5, 1])
# tmprtq85.reparentTo(base.render)
# self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
# reset initial hand pose
tmphand.setMat(initmat)
self.counter += 1
self.counter = 0
def savestartik(self, startrotmat4, startid, base):
nodeidofglobalidinstart = {}
# the startnodeids is also for quick access
self.startnodeids = []
for j, rotmat in enumerate(self.freegriprotmats):
# print j, len(self.freegriprotmats)
# # print rotmat
ttgsrotmat = rotmat * startrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(startrotmat4)
ttgsrotmatx0y0.setCell(3, 0, 0)
ttgsrotmatx0y0.setCell(3, 1, 0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
# tmphnd = self.robothand
tmphnd = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# set jawwidth to 80 to avoid collision with surrounding obstacles
# set to gripping with is unnecessary
tmphnd.setJawwidth(80)
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
ttgscct0 = startrotmat4.xformPoint(self.freegripcontacts[j][0])
ttgscct1 = startrotmat4.xformPoint(self.freegripcontacts[j][1])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx * self.rethandx
# handxworldz is not necessary for start
# ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda * self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + self.worldz * self.retworldz
ttgsjawwidth = self.freegripjawwidth[j]
ttgsidfreeair = self.freegripid[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
# handxworldz is not necessary for start
# ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(
ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
# print "solving starting iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz,
ttgsrotmat3np)
feasibility = {}
feasibility_handx = {}
#feasibility_handxworldz = {}
feasibility_worlda = {}
feasibility_worldaworldz = {}
if ikc is not None:
feasibility[j] = 'True'
else:
feasibility[j] = 'False'
if ikcx is not None:
feasibility_handx[j] = 'True'
else:
feasibility_handx[j] = 'False'
if ikca is not None:
feasibility_worlda[j] = 'True'
else:
feasibility_worlda[j] = 'False'
if ikcaz is not None:
feasibility_worldaworldz[j] = 'True'
else:
feasibility_worldaworldz[j] = 'False'
sql = "INSERT IGNORE INTO ik_start(idstart,idfreeairgrip, feasibility, feasibility_handx, \
feasibility_worlda, feasibility_worldaworldz) VALUES (%d,%d, '%s', '%s', '%s', '%s')" % \
(startid, j, feasibility[j], feasibility_handx[j],\
feasibility_worlda[j], feasibility_worldaworldz[j])
self.gdb.execute(sql)