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 __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 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 __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 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 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