def checkDiffRot(self, rot1, rot2):
"""
:return:if the two pos the pos and quration is the same.
author: jiayao
date: 20170817
"""
print rot1, rot2
#diffrot = pg.v3ToNp(rot1)-pg.v3ToNp(rot2)
diffrot1 = pg.mat3ToNp(rot1)
print rot2
diffrot2 = pg.mat3ToNp(rot2)
print diffrot1, diffrot2
diffrot = diffrot1 - diffrot2
diff = (np.trace(np.dot(diffrot, diffrot)))**0.5
print "diffrot", diff
#haven't finished
# diffrot.get()
return diff
def __loadGripsToBuildGraph(self, armname = "rgt"):
"""
load tabletopgrips
retraction distance are also loaded from database
:param robot: an robot defined in robotsim.hrp5 or robotsim.nextage
:param gdb: an object of the database.GraspDB class
:param idarm: value = 1 "lft" or 2 "rgt", which arm to use
:return:
author: weiwei
date: 20170112
"""
# load idarm
idarm = self.gdb.loadIdArm(armname)
idhand = self.gdb.loadIdHand(self.handname)
# get the global grip ids
# and prepare the global edges
# for each globalgripid, find all its tabletopids (pertaining to placements)
globalidsedges = {}
sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand = %d" % (self.dbobjname, idhand)
# sql = "SELECT dropfreegrip.iddropfreegrip FROM dropfreegrip,object WHERE dropfreegrip.idobject=object.idobject AND \
# object.name LIKE '%s' AND dropfreegrip.idhand = %d" % (self.dbobjname, idhand)
# sql = "SELECT dropworkcellgrip.iddropworkcellgrip FROM dropworkcellgrip,object WHERE dropworkcellgrip.idobject=object.idobject AND \
# object.name LIKE '%s' AND dropworkcellgrip.idhand = %d" % (self.dbobjname, idhand)
result = self.gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan freeairgrip first!")
for ggid in result:
globalidsedges[str(ggid[0])] = []
self.globalgripids.append(ggid[0])
sql = "SELECT dropstablepos.iddropstablepos,dropstablepos.rot,dropstablepos.pos,angle_drop.value FROM \
dropstablepos,object,angle_drop WHERE \
dropstablepos.idobject=object.idobject AND \
object.name LIKE '%s' " % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
tpsrows = np.array(result)
#self.angles = list([0.0])
self.angles = list(set(map(float, tpsrows[:, 3])))
# for plotting
self.fttpsids = list(set(map(int, tpsrows[:, 0])))
self.nfttps = len(self.fttpsids)
idrobot = self.gdb.loadIdRobot(self.robot)
for i, idtps in enumerate(tpsrows[:,0]):
sql = "SELECT dropworkcellgrip.iddropworkcellgrip, dropworkcellgrip.contactpnt0, dropworkcellgrip.contactpnt1, \
dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth ,dropworkcellgrip.idfreeairgrip\
FROM dropworkcellgrip,dropfreegrip,freeairgrip,ik_drop\
WHERE \
dropworkcellgrip.iddropstablepos = %d \
AND dropworkcellgrip.iddropworkcellgrip=ik_drop.iddropworkcellgrip AND ik_drop.idrobot=%d AND ik_drop.idarm = %d AND\
ik_drop.feasibility='True' AND ik_drop.feasibility_handx='True' AND ik_drop.feasibility_handxworldz='True' \
AND ik_drop.feasibility_worlda='True' AND ik_drop.feasibility_worldaworldz='True' \
AND dropworkcellgrip.idfreeairgrip = freeairgrip.idfreeairgrip \
AND dropworkcellgrip.idhand = % d AND dropworkcellgrip.iddropfreegrip = dropfreegrip.iddropfreegrip " \
% (int(idtps),idrobot, idarm, idhand)
resultttgs = self.gdb.execute(sql)
if len(resultttgs)==0:
print "no result"
continue
localidedges = []
for ttgsrow in resultttgs:
ttgsid = int(ttgsrow[0])
ttgscct0 = dc.strToV3(ttgsrow[1])
ttgscct1 = dc.strToV3(ttgsrow[2])
ttgsrotmat = dc.strToMat4(ttgsrow[3])
ttgsjawwidth = float(ttgsrow[4])
ttgsidfreeair = int(ttgsrow[5])
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
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())
objrotmat4 = pg.npToMat4(np.transpose(pg.mat3ToNp(dc.strToMat3(tpsrows[:, 1][i]))),
pg.v3ToNp(dc.strToV3(tpsrows[:, 2][i])))
objrotmat4worlda = Mat4(objrotmat4)
objrotmat4worlda.setRow(3, objrotmat4.getRow3(3)+self.worlda*self.retworlda)
objrotmat4worldaworldz = Mat4(objrotmat4worlda)
objrotmat4worldaworldz.setRow(3, objrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('mid' + str(ttgsid), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz,
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair,
#freetabletopplacementid = int(tpsrows[:,2][i]),
freetabletopplacementid=int(tpsrows[:, 0][i]),
tabletopplacementrotmat = objrotmat4,
tabletopplacementrotmathandx = objrotmat4,
tabletopplacementrotmathandxworldz = objrotmat4,
tabletopplacementrotmatworlda = objrotmat4worlda,
tabletopplacementrotmatworldaworldz = objrotmat4worldaworldz,
angle = float(tpsrows[:,3][i]),
tabletopposition = dc.strToV3(tpsrows[:,2][i]))
print "str(ttgsidfreeair),str(ttgsid)",str(ttgsidfreeair),str(ttgsid)
globalidsedges[str(ttgsidfreeair)].append('mid' + str(ttgsid))
localidedges.append('mid' + str(ttgsid))
for edge in list(itertools.combinations(localidedges, 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transit')
#toc = time.clock()
#print "(toc - tic2)", (toc - tic)
if len(globalidsedges) == 0:
raise ValueError("Plan tabletopgrips first!")
#tic = time.clock()
for globalidedgesid in globalidsedges:
for edge in list(itertools.combinations(globalidsedges[globalidedgesid], 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transfer')
#toc = time.clock()
#print "(toc - tic3)", (toc - tic)
else:
print ('No placements planned!')
assert('No placements planned!')
def updateDBwithIK(self, robot):
"""
compute the IK feasible grasps of each pose
:return:
"""
# load idrobot
idrobot = self.gdb.loadIdRobot(robot)
self.assgikfeas0 = []
self.assgikfeas0_retassdir = []
self.assgikfeas1= []
self.assgikfeas1_retassdir = []
tic = time.time()
for fpid, apmat in enumerate(self.gridsfloatingposemat4s):
assdir1to0 = apmat.xformVec(self.assDirect1to0)
assdir0to1 = apmat.xformVec(-self.assDirect1to0)
toc = time.time()
print(toc-tic)
if fpid != 0:
print("remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic))
print(fpid, len(self.gridsfloatingposemat4s))
### right hand # rgt = 0
armname = 'rgt'
assgikfeas0 = []
assgikfeas0_retassdir = []
for i, hndrotmat4 in enumerate(self.icoass0griprotmat4s[fpid]):
assgikfeas0.append('False')
assgikfeas0_retassdir.append('False')
assgsfgrcenter = (self.icoass0gripcontacts[fpid][i][0]+self.icoass0gripcontacts[fpid][i][1])/2
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
minusworldy = Vec3(0,-1,0)
if Vec3(handx).angleDeg(minusworldy) < 60:
assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir1to0)
if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None:
assgikfeas0[i] = 'True'
if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None:
assgikfeas0_retassdir[i] = 'True'
self.icoass0gripsik.append(assgikfeas0)
self.icoass0gripsik_retassdir.append(assgikfeas0_retassdir)
### left hand
armname = 'lft'
assgikfeas1 = []
assgikfeas1_retassdir = []
for i, hndrotmat4 in enumerate(self.icoass1griprotmat4s[fpid]):
assgikfeas1.append('False')
assgikfeas1_retassdir.append('False')
assgsfgrcenter = (self.icoass1gripcontacts[fpid][i][0]+self.icoass1gripcontacts[fpid][i][1])/2
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
plusworldy = Vec3(0,1,0)
if Vec3(handx).angleDeg(plusworldy) < 60:
assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir0to1)
if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None:
assgikfeas1[i] = 'True'
if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None:
assgikfeas1_retassdir[i] = 'True'
self.icoass1gripsik.append(assgikfeas1)
self.icoass1gripsik_retassdir.append(assgikfeas1_retassdir)
self.__saveIKtoDB(idrobot)
def updateDBwithIK(self, robot):
"""
compute the IK feasible grasps of each pose
:return:
"""
# load retraction distances
rethandx, retworldz, retworlda, worldz = self.gdb.loadIKRet()
# load idrobot
idrobot = self.gdb.loadIdRobot(robot)
self.floatinggripikfeas_rgt = []
self.floatinggripikfeas_handx_rgt = []
self.floatinggripikfeas_lft = []
self.floatinggripikfeas_handx_lft = []
tic = time.clock()
for fpid in range(len(self.gridsfloatingposemat4s)):
toc = time.clock()
print toc-tic
if fpid != 0:
print "remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic)
print fpid, len(self.gridsfloatingposemat4s)
### right hand
armname = 'rgt'
feasibility = []
feasibility_handx = []
for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2
fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter)
fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
minusworldy = Vec3(0,-1,0)
if Vec3(handx).angleDeg(minusworldy) < 60:
fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx)
if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None:
feasibility[i] = 'True'
if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None:
feasibility_handx[i] = 'True'
self.floatinggripikfeas_rgt.append(feasibility)
self.floatinggripikfeas_handx_rgt.append(feasibility_handx)
### left hand
armname = 'lft'
feasibility = []
feasibility_handx = []
for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2
fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter)
fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
plusworldy = Vec3(0,1,0)
if Vec3(handx).angleDeg(plusworldy) < 60:
fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx)
if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None:
feasibility[i] = 'True'
if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None:
feasibility_handx[i] = 'True'
self.floatinggripikfeas_lft.append(feasibility)
self.floatinggripikfeas_handx_lft.append(feasibility_handx)
self.saveIKtoDB(idrobot)
def updateDBwithIK(self, robot):
"""
compute the IK feasible grasps of each pose
:return:
"""
# load idrobot
idrobot = self.gdb.loadIdRobot(robot)
self.assgikfeas0 = []
self.assgikfeas0_retassdir = []
self.assgikfeas1= []
self.assgikfeas1_retassdir = []
tic = time.clock()
for fpid, apmat in enumerate(self.gridsfloatingposemat4s):
assdir1to0 = apmat.xformVec(self.assDirect1to0)
assdir0to1 = apmat.xformVec(-self.assDirect1to0)
toc = time.clock()
print toc-tic
if fpid != 0:
print "remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic)
print fpid, len(self.gridsfloatingposemat4s)
### right hand # rgt = 0
armname = 'rgt'
assgikfeas0 = []
assgikfeas0_retassdir = []
for i, hndrotmat4 in enumerate(self.icoass0griprotmat4s[fpid]):
assgikfeas0.append('False')
assgikfeas0_retassdir.append('False')
assgsfgrcenter = (self.icoass0gripcontacts[fpid][i][0]+self.icoass0gripcontacts[fpid][i][1])/2
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
minusworldy = Vec3(0,-1,0)
if Vec3(handx).angleDeg(minusworldy) < 60:
assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir1to0)
if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None:
assgikfeas0[i] = 'True'
if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None:
assgikfeas0_retassdir[i] = 'True'
self.icoass0gripsik.append(assgikfeas0)
self.icoass0gripsik_retassdir.append(assgikfeas0_retassdir)
### left hand
armname = 'lft'
assgikfeas1 = []
assgikfeas1_retassdir = []
for i, hndrotmat4 in enumerate(self.icoass1griprotmat4s[fpid]):
assgikfeas1.append('False')
assgikfeas1_retassdir.append('False')
assgsfgrcenter = (self.icoass1gripcontacts[fpid][i][0]+self.icoass1gripcontacts[fpid][i][1])/2
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
plusworldy = Vec3(0,1,0)
if Vec3(handx).angleDeg(plusworldy) < 60:
assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir0to1)
if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None:
assgikfeas1[i] = 'True'
if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None:
assgikfeas1_retassdir[i] = 'True'
self.icoass1gripsik.append(assgikfeas1)
self.icoass1gripsik_retassdir.append(assgikfeas1_retassdir)
self.__saveIKtoDB(idrobot)
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 updateDBwithIK(self, robot):
"""
compute the IK feasible grasps of each pose
:return:
"""
# load retraction distances
rethandx, retworldz, retworlda, worldz = self.gdb.loadIKRet()
# load idrobot
idrobot = self.gdb.loadIdRobot(robot)
self.floatinggripikfeas_rgt = []
self.floatinggripikfeas_handx_rgt = []
self.floatinggripikfeas_lft = []
self.floatinggripikfeas_handx_lft = []
tic = time.clock()
for fpid in range(len(self.gridsfloatingposemat4s)):
toc = time.clock()
print(toc-tic)
if fpid != 0:
print("remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic))
print(fpid, len(self.gridsfloatingposemat4s))
### right hand
armname = 'rgt'
feasibility = []
feasibility_handx = []
for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2
fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter)
fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
minusworldy = Vec3(0,-1,0)
if Vec3(handx).angleDeg(minusworldy) < 60:
fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx)
if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None:
feasibility[i] = 'True'
if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None:
feasibility_handx[i] = 'True'
self.floatinggripikfeas_rgt.append(feasibility)
self.floatinggripikfeas_handx_rgt.append(feasibility_handx)
### left hand
armname = 'lft'
feasibility = []
feasibility_handx = []
for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]):
feasibility.append('False')
feasibility_handx.append('False')
fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2
fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter)
fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
handx = hndrotmat4.getRow3(0)
# check the angle between handx and minus y
plusworldy = Vec3(0,1,0)
if Vec3(handx).angleDeg(plusworldy) < 60:
fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx)
if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None:
feasibility[i] = 'True'
if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None:
feasibility_handx[i] = 'True'
self.floatinggripikfeas_lft.append(feasibility)
self.floatinggripikfeas_handx_lft.append(feasibility_handx)
self.saveIKtoDB(idrobot)
def __loadGripsToBuildGraph(self, armname="rgt"):
"""
load tabletopgrips
retraction distance are also loaded from database
:param robot: an robot defined in robotsim.hrp5 or robotsim.nextage
:param gdb: an object of the database.GraspDB class
:param idarm: value = 1 "lft" or 2 "rgt", which arm to use
:return:
author: weiwei
date: 20170112
"""
# load idarm
idarm = gdb.loadIdArm(armname)
# get the global grip ids
# and prepare the global edges
# for each globalgripid, find all its tabletopids (pertaining to placements)
globalidsedges = {}
sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan freeairgrip first!")
for ggid in result:
globalidsedges[str(ggid[0])] = []
self.globalgripids.append(ggid[0])
sql = "SELECT tabletopplacements.idtabletopplacements, angle.value, \
tabletopplacements.idfreetabletopplacement, tabletopplacements.tabletopposition, \
tabletopplacements.rotmat FROM \
tabletopplacements,freetabletopplacement,angle,object WHERE \
tabletopplacements.idangle=angle.idangle AND \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND \
object.name LIKE '%s' AND angle.value IN (0.0, 45.0, 90.0, 135.0, 180.0, 225.0, 270.0, 315.0)" \
% self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
tpsrows = np.array(result)
# nubmer of discreted rotation
self.angles = list(set(map(float, tpsrows[:, 1])))
# for plotting
self.fttpsids = list(set(map(int, tpsrows[:, 2])))
self.nfttps = len(self.fttpsids)
idrobot = self.gdb.loadIdRobot(self.robot)
for i, idtps in enumerate(tpsrows[:, 0]):
sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \
tabletopgrips.rotmat, tabletopgrips.jawwidth, tabletopgrips.idfreeairgrip \
FROM tabletopgrips,ik,freeairgrip,hand WHERE tabletopgrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND\
tabletopgrips.idtabletopgrips=ik.idtabletopgrips AND \
tabletopgrips.idtabletopplacements = %d AND ik.idrobot=%d AND \
ik.feasibility='True' AND ik.feasibility_handx='True' AND ik.feasibility_handxworldz='True' \
AND ik.feasibility_worlda='True' AND ik.feasibility_worldaworldz='True' AND ik.idarm = %d \
AND hand.name LIKE '%s'" \
% (int(idtps), idrobot, idarm, self.handpkg.getHandName())
resultttgs = self.gdb.execute(sql)
if len(resultttgs) == 0:
continue
localidedges = []
for ttgsrow in resultttgs:
ttgsid = int(ttgsrow[0])
ttgscct0 = dc.strToV3(ttgsrow[1])
ttgscct1 = dc.strToV3(ttgsrow[2])
ttgsrotmat = dc.strToMat4(ttgsrow[3])
ttgsjawwidth = float(ttgsrow[4])
ttgsidfreeair = int(ttgsrow[5])
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
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())
objrotmat4 = dc.strToMat4(tpsrows[:, 4][i])
objrotmat4worlda = Mat4(objrotmat4)
objrotmat4worlda.setRow(
3,
objrotmat4.getRow3(3) + self.worlda * self.retworlda)
objrotmat4worldaworldz = Mat4(objrotmat4worlda)
objrotmat4worldaworldz.setRow(
3,
objrotmat4worlda.getRow3(3) +
self.worldz * self.retworldz)
self.regg.add_node(
armname + str(ttgsid),
fgrcenter=ttgsfgrcenternp,
fgrcenterhandx=ttgsfgrcenternp_handx,
fgrcenterhandxworldz=ttgsfgrcenternp_handxworldz,
fgrcenterworlda=ttgsfgrcenternp_worlda,
fgrcenterworldaworldz=ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth,
hndrotmat3np=ttgsrotmat3np,
globalgripid=ttgsidfreeair,
freetabletopplacementid=int(tpsrows[:, 2][i]),
tabletopplacementrotmat=objrotmat4,
tabletopplacementrotmathandx=objrotmat4,
tabletopplacementrotmathandxworldz=objrotmat4,
tabletopplacementrotmatworlda=objrotmat4worlda,
tabletopplacementrotmatworldaworldz=
objrotmat4worldaworldz,
angle=float(tpsrows[:, 1][i]),
tabletopposition=dc.strToV3(tpsrows[:, 3][i]))
globalidsedges[str(ttgsidfreeair)].append(armname +
str(ttgsid))
localidedges.append(armname + str(ttgsid))
# print list(itertools.combinations(ttgrows[:,0], 2))
for edge in list(itertools.combinations(localidedges, 2)):
self.regg.add_edge(*edge, weight=1, edgetype='transit')
if len(globalidsedges) == 0:
raise ValueError("Plan tabletopgrips first!")
for globalidedgesid in globalidsedges:
for edge in list(
itertools.combinations(globalidsedges[globalidedgesid],
2)):
self.regg.add_edge(*edge, weight=1, edgetype='transfer')
# gen plot pos
# biggest circle: grips; big circle: rotation; small circle: placements
radiusplacement = 30
radiusrot = 6
radiusgrip = 1
xyplacementspos = {}
xydiscreterotspos = {}
xyzglobalgrippos = {}
for i, ttpsid in enumerate(self.fttpsids):
xydiscreterotspos[ttpsid] = {}
xyzglobalgrippos[ttpsid] = {}
xypos = [
radiusplacement * math.cos(2 * math.pi / self.nfttps * i),
radiusplacement * math.sin(2 * math.pi / self.nfttps * i)
]
xyplacementspos[ttpsid] = xypos
for j, anglevalue in enumerate(self.angles):
xyzglobalgrippos[ttpsid][anglevalue] = {}
xypos = [
radiusrot * math.cos(anglevalue),
radiusrot * math.sin(anglevalue)
]
xydiscreterotspos[ttpsid][anglevalue] = \
[xyplacementspos[ttpsid][0] + xypos[0], xyplacementspos[ttpsid][1] + xypos[1]]
for k, globalgripid in enumerate(self.globalgripids):
xypos = [
radiusgrip *
math.cos(2 * math.pi / len(self.globalgripids) * k),
radiusgrip *
math.sin(2 * math.pi / len(self.globalgripids) * k)
]
xyzglobalgrippos[ttpsid][anglevalue][globalgripid] = \
[xydiscreterotspos[ttpsid][anglevalue][0] + xypos[0],
xydiscreterotspos[ttpsid][anglevalue][1] + xypos[1], 0]
for nid in self.regg.nodes():
fttpid = self.regg.node[nid]['freetabletopplacementid']
anglevalue = self.regg.node[nid]['angle']
ggid = self.regg.node[nid]['globalgripid']
tabletopposition = self.regg.node[nid]['tabletopposition']
xyzpos = map(add, xyzglobalgrippos[fttpid][anglevalue][ggid], [
tabletopposition[0], tabletopposition[1], tabletopposition[2]
])
self.gnodesplotpos[nid] = xyzpos[:2]
def freegripRotMove(objname,handname):
"""
:param objname:
:return:
for each dropstablepos
caculate its grips after rot and move
and save to database dropfreegrip to remove the hand around workcell next
mention that the dropstablepos rot,pos
rotmat=(np.transpose(rot),pos)
"""
gdb = db.GraspDB()
idhand = gdb.loadIdHand(handname)
idobject=gdb.loadIdObject(objname)
sql = "SELECT dropstablepos.iddropstablepos\
FROM dropstablepos, object \
WHERE dropstablepos.idobject = object.idobject AND object.name like '%s'" % (objname)
result = gdb.execute(sql)
print result
if len(result) == 0:
print "no DropStablePos select"
return None
for idfree in result:
idfree = int(idfree[0])
sql = "SELECT dropstablepos.iddropstablepos, \
dropstablepos.pos, dropstablepos.rot,\
freeairgrip.contactpnt0, freeairgrip.contactpnt1, \
freeairgrip.contactnormal0, freeairgrip.contactnormal1, \
freeairgrip.rotmat, freeairgrip.jawwidth, freeairgrip.idfreeairgrip \
FROM dropstablepos,freeairgrip WHERE \
freeairgrip.idhand = %d AND \
dropstablepos.iddropstablepos = %d" % (idhand, idfree)
result1 = gdb.execute(sql)
if len(result1) == 0:
print "no free air grasp availalbe"
continue
if len(result1) > 20000:
result1 = result1[0::int(len(result1) / 20000.0)]
result1 = np.asarray(result1)
idtabletopplacementslist = [int(x) for x in result1[:, 0]]
tabletoppositionlist = [dc.strToV3(x) for x in result1[:, 1]]
#rotanglelist = [float(x) for x in result1[:, 2]]
rotanglelist = [dc.strToMat3(x) for x in result1[:, 2]]
freegripcontactpoint0list = [dc.strToV3(x) for x in result1[:, 3]]
freegripcontactpoint1list = [dc.strToV3(x) for x in result1[:, 4]]
freegripcontactnormal0list = [dc.strToV3(x) for x in result1[:, 5]]
freegripcontactnormal1list = [dc.strToV3(x) for x in result1[:, 6]]
freegriprotmatlist = [dc.strToMat4(x) for x in result1[:, 7]]
freegripjawwidthlist = [float(x) for x in result1[:, 8]]
freegripidlist = [int(x) for x in result1[:, 9]]
for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \
freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \
tabletoppositionlist, rotanglelist, \
freegripcontactpoint0list, freegripcontactpoint1list, \
freegripcontactnormal0list, freegripcontactnormal1list,
freegriprotmatlist, freegripjawwidthlist, \
freegripidlist):
rotmat4=pg.npToMat4(np.transpose(pg.mat3ToNp(rotangle)), ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat * rotmat4
#ttpgriprotmat = rotmat4*freegriprotmat
sql = "INSERT INTO dropfreegrip(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreeairgrip, iddropstablepos,idhand,idobject) VALUES \
('%s', '%s', '%s', '%s', '%s', '%s', %d, %d,%d,%d) " % \
(dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \
dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements, idhand, idobject)
gdb.execute(sql)
def updateDBwithIK(self, gdb, robot, armname = 'rgt'):
"""
:param gdb:
:param robot:
:param armname: the name of the arm used rgt or lft
:return:
author: weiwei
date: 20170111
"""
# load idarm
idarm = gdb.loadIdArm(armname)
# load retraction distances
rethanda, retworlda, worlda = gdb.loadIKRet()
# select idrobot
idrobot = gdb.loadIdRobot(robot)
feasibility = {}
feasibility_handa = {}
feasibility_worlda = {}
jnts = {}
jnts_handa = {}
jnts_worlda = {}
isttg, ttgresult = self.__getTtg(gdb)
if not isttg:
raise ValueError("Plan the tabletopgrips first!")
else:
for resultrow in ttgresult:
ttgsid = int(resultrow[0])
sql = "SELECT * FROM iktabletopgrips WHERE iktabletopgrips.idrobot = %d AND idarm = %d \
AND idtabletopgrips = %d" % (idrobot, idarm, ttgsid)
result = gdb.execute(sql)
if len(result) != 0:
print("IK for the "+armname+" arm is already updated!")
isredo = input("Do you want to overwrite the database? (Y/N)")
if isredo != "Y" and isredo != "y":
print("Updating IK is aborted.")
return
else:
for resultrow in ttgresult:
ttgsid = int(resultrow[0])
sql = "DELETE FROM iktabletopgrips WHERE iktabletopgrips.idrobot = %d AND idarm = %d \
AND idtabletopgrips = %d" % (idrobot, idarm, ttgsid)
gdb.execute(sql)
print("Old tabletopplacements are being deleted!")
print("Old tabletopplacements have been deleted!")
break
idcounter = 0
tic = time.time()
for resultrow in ttgresult:
toc = time.time()
print("Arm: ", armname, " Finished: ", "%.2f" % (idcounter*1.0/len(ttgresult)*100.0)+"%",
" Time past: ", "%.2f" % (toc-tic), "s",
" Expected remaining time: ", "%.2f" % ((toc-tic)/(idcounter*1.0/len(ttgresult)+1e-4)-(toc-tic)), "s")
idcounter += 1
ttgsid = int(resultrow[0])
ttgscct0 = dc.strToV3(resultrow[1])
ttgscct1 = dc.strToV3(resultrow[2])
ttgsrotmat = dc.strToMat4(resultrow[3])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2.0
handa = -ttgsrotmat.getRow3(2)
ttgsfgrcenterhanda = ttgsfgrcenter + handa*rethanda
ttgsfgrcenterworlda = ttgsfgrcenter + worlda*retworlda
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handa = pg.v3ToNp(ttgsfgrcenterhanda)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
msc = robot.numik(ttgsfgrcenternp, ttgsrotmat3np, armname)
if msc is not None:
feasibility[ttgsid] = 'True'
jnts[ttgsid] = dc.listToStr(msc)
msc_handa = robot.numikmsc(ttgsfgrcenternp_handa, ttgsrotmat3np, msc, armname)
if msc_handa is not None:
feasibility_handa[ttgsid] = 'True'
jnts_handa[ttgsid] = dc.listToStr(msc_handa)
else:
feasibility_handa[ttgsid] = 'False'
jnts_handa[ttgsid] = dc.listToStr([])
msc_worlda = robot.numikmsc(ttgsfgrcenternp_worlda, ttgsrotmat3np, msc, armname)
if msc_worlda is not None:
feasibility_worlda[ttgsid] = 'True'
jnts_worlda[ttgsid] = dc.listToStr(msc_worlda)
else:
feasibility_worlda[ttgsid] = 'False'
jnts_worlda[ttgsid] = dc.listToStr([])
else:
feasibility[ttgsid] = 'False'
feasibility_handa[ttgsid] = 'False'
feasibility_worlda[ttgsid] = 'False'
jnts[ttgsid] = dc.listToStr([])
jnts_handa[ttgsid] = dc.listToStr([])
jnts_worlda[ttgsid] = dc.listToStr([])
# insert ik table
sql = "INSERT INTO iktabletopgrips(idrobot, idarm, idtabletopgrips, feasibility, feasibility_handa, \
feasibility_worlda, jnts, jnts_handa, jnts_worlda) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s', '%s', '%s')" % \
(idrobot, idarm, ttgsid, feasibility[ttgsid], feasibility_handa[ttgsid], feasibility_worlda[ttgsid], \
jnts[ttgsid], jnts_handa[ttgsid], jnts_worlda[ttgsid])
gdb.execute(sql)
print("IK updated!")
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 updateDBwithIK(self, gdb, robot, armname = 'rgt'):
"""
:param gdb:
:param robot:
:param armname: the name of the arm used rgt or lft
:param rethandx: the distance of retract along handx direction, default 50mm
:param retworldz: the distance of retract along worldz direction, default 50mm
:param retworlda: the distance of retract along assembly/approaching direction in the world, default 50mm
:return:
author: weiwei
date: 20170111
"""
# load idarm
idarm = gdb.loadIdArm(armname)
# load retraction distances
rethandx, retworldz, retworlda, worldz = gdb.loadIKRet()
# worlda is default for the general grasps on table top
# for assembly at start and goal, worlda is computed by assembly planner
worlda = Vec3(0,0,1)
# select idrobot
idrobot = gdb.loadIdRobot(robot)
feasibility = {}
feasibility_handx = {}
feasibility_handxworldz = {}
feasibility_worlda = {}
feasibility_worldaworldz = {}
sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \
tabletopgrips.rotmat FROM tabletopgrips, tabletopplacements, freetabletopplacement, object WHERE \
tabletopgrips.idtabletopplacements = tabletopplacements.idtabletopplacements AND \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the tabletopgrips first!")
idcounter = 0
tic = time.clock()
for resultrow in result:
print idcounter*1.0/len(result)
idcounter += 1
toc = time.clock()
print toc-tic
ttgsid = int(resultrow[0])
ttgscct0 = dc.strToV3(resultrow[1])
ttgscct1 = dc.strToV3(resultrow[2])
ttgsrotmat = dc.strToMat4(resultrow[3])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + worldz*retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + worlda*retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ worldz*retworldz
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())
if robot.numik(ttgsfgrcenternp, ttgsrotmat3np, armname) is not None:
feasibility[ttgsid] = 'True'
else:
feasibility[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_handx, ttgsrotmat3np, armname) is not None:
feasibility_handx[ttgsid] = 'True'
else:
feasibility_handx[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_handxworldz, ttgsrotmat3np, armname) is not None:
feasibility_handxworldz[ttgsid] = 'True'
else:
feasibility_handxworldz[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_worlda, ttgsrotmat3np, armname) is not None:
feasibility_worlda[ttgsid] = 'True'
else:
feasibility_worlda[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np, armname) is not None:
feasibility_worldaworldz[ttgsid] = 'True'
else:
feasibility_worldaworldz[ttgsid] = 'False'
# insert ik table
sql = "INSERT IGNORE INTO ik(idrobot, idarm, idtabletopgrips, feasibility, feasibility_handx, feasibility_handxworldz, \
feasibility_worlda, feasibility_worldaworldz) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s', '%s')" % \
(idrobot, idarm, ttgsid, feasibility[ttgsid], feasibility_handx[ttgsid], feasibility_handxworldz[ttgsid], \
feasibility_worlda[ttgsid], feasibility_worldaworldz[ttgsid])
gdb.execute(sql)
def __addAssNodes(self, armname='rgt'):
iele = 0
if armname == 'lft':
iele = 1
freeairidontpp = {}
# for plot
radiusgrip = self.regghalf[iele].radiusgrip
for nid in self.regghalf[iele].graphtpp.regg.nodes():
dictind = str(
self.regghalf[iele].graphtpp.regg.node[nid]['globalgripid'])
if dictind in freeairidontpp:
freeairidontpp[dictind].append(nid)
else:
freeairidontpp[dictind] = []
# add floatingposes
freeairidonass = {}
for asspind, assprotmat4 in enumerate(
self.toass.gridsfloatingposemat4s):
retass = assprotmat4.xformVec(self.retass[iele])
for pairind, hndrotmat4pair in enumerate(
self.toass.icoassgrippairshndmat4s[asspind]):
assgid = self.toass.icoassgrippairsids[asspind][pairind][iele]
assgidfreeair = self.toass.icoassgrippairsidfreeairs[asspind][
pairind][iele]
ccts = self.toass.icoassgrippairscontacts[asspind][pairind][
iele]
hndrotmat4 = hndrotmat4pair[iele]
asspgfgrcenter = (Vec3(ccts[0][0], ccts[0][1], ccts[0][2]) +
Vec3(ccts[1][0], ccts[1][1], ccts[1][2])) / 2
asspgfgrcenter_retass = asspgfgrcenter + retass
asspgfgrcenternp = pg.v3ToNp(asspgfgrcenter)
asspgfgrcenter_retassnp = pg.v3ToNp(asspgfgrcenter_retass)
jawwidth = self.toass.icoassgrippairsjawwidths[asspind][
pairind][iele]
hndrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
objrotmat4 = self.objrotmat4s[iele] * assprotmat4
objrotmat4retass = Mat4(objrotmat4)
objrotmat4retass.setRow(3,
objrotmat4retass.getRow3(3) + retass)
self.regg.add_node('ass' + armname + str(assgid),
fgrcenter=asspgfgrcenternp,
fgrcenterretass=asspgfgrcenter_retassnp,
jawwidth=jawwidth,
hndrotmat3np=hndrotmat3np,
assposerotmat4=objrotmat4,
assposerotmat4retass=objrotmat4retass,
assposeind=asspind,
icoassgrippairsid=pairind,
globalgripid=assgidfreeair)
if str(assgidfreeair) in freeairidonass:
freeairidonass[str(assgidfreeair)].append('ass' + armname +
str(assgid))
else:
freeairidonass[str(assgidfreeair)] = []
for freeairidontppkey in freeairidontpp.keys():
try:
for edge in list(
itertools.product(freeairidontpp[freeairidontppkey],
freeairidonass[freeairidontppkey])):
self.regg.add_edge(*edge, weight=1, edgetype='asstransfer')
except:
pass
# plot pos
nfp = len(self.toass.gridsfloatingposemat4s)
xdist = 10
x = range(300, 501, xdist)
y = range(-50, 50, 100 * xdist / nfp)
for nid in self.regg.nodes():
if nid.startswith('ass'):
asspind = self.regg.node[nid]['assposeind']
assgind = self.regg.node[nid]['icoassgrippairsid']
nassg = len(self.toass.icoassgrippairshndmat4s[asspind])
xpos = x[asspind % len(x)]
ypos = y[asspind / len(x)]
xyzpos = [
radiusgrip * math.cos(2 * math.pi / nassg * assgind) +
xpos,
radiusgrip * math.sin(2 * math.pi / nassg * assgind) +
ypos, 0
]
self.gnodesplotpos[nid] = xyzpos[:2]
if nid.startswith('assrgt'):
self.gnodesplotpos[nid][
1] = self.gnodesplotpos[nid][1] - 100
elif nid.startswith('asslft'):
self.gnodesplotpos[nid][
1] = self.gnodesplotpos[nid][1] + 100
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
def __addAssNodes(self, armname = 'rgt'):
iele = 0
if armname == 'lft':
iele = 1
freeairidontpp = {}
# for plot
radiusgrip = self.regghalf[iele].radiusgrip
for nid in self.regghalf[iele].graphtpp.regg.nodes():
dictind = str(self.regghalf[iele].graphtpp.regg.node[nid]['globalgripid'])
if dictind in freeairidontpp:
freeairidontpp[dictind].append(nid)
else:
freeairidontpp[dictind] = []
# add floatingposes
freeairidonass = {}
for asspind, assprotmat4 in enumerate(self.toass.gridsfloatingposemat4s):
retass = assprotmat4.xformVec(self.retass[iele])
for pairind, hndrotmat4pair in enumerate(self.toass.icoassgrippairshndmat4s[asspind]):
assgid = self.toass.icoassgrippairsids[asspind][pairind][iele]
assgidfreeair = self.toass.icoassgrippairsidfreeairs[asspind][pairind][iele]
ccts = self.toass.icoassgrippairscontacts[asspind][pairind][iele]
hndrotmat4 = hndrotmat4pair[iele]
asspgfgrcenter = (Vec3(ccts[0][0], ccts[0][1], ccts[0][2]) + Vec3(ccts[1][0], ccts[1][1], ccts[1][2])) / 2
asspgfgrcenter_retass = asspgfgrcenter + retass
asspgfgrcenternp = pg.v3ToNp(asspgfgrcenter)
asspgfgrcenter_retassnp = pg.v3ToNp(asspgfgrcenter_retass)
jawwidth = self.toass.icoassgrippairsjawwidths[asspind][pairind][iele]
hndrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3())
objrotmat4 = self.objrotmat4s[iele]*assprotmat4
objrotmat4retass = Mat4(objrotmat4)
objrotmat4retass.setRow(3, objrotmat4retass.getRow3(3)+retass)
self.regg.add_node('ass' + armname + str(assgid), fgrcenter=asspgfgrcenternp,
fgrcenterretass=asspgfgrcenter_retassnp, jawwidth=jawwidth,
hndrotmat3np=hndrotmat3np, assposerotmat4=objrotmat4,
assposerotmat4retass=objrotmat4retass, assposeind=asspind,
icoassgrippairsid=pairind, globalgripid=assgidfreeair)
if str(assgidfreeair) in freeairidonass:
freeairidonass[str(assgidfreeair)].append('ass' + armname + str(assgid))
else:
freeairidonass[str(assgidfreeair)] = []
for freeairidontppkey in freeairidontpp.keys():
try:
for edge in list(itertools.product(freeairidontpp[freeairidontppkey], freeairidonass[freeairidontppkey])):
self.regg.add_edge(*edge, weight=1, edgetype='asstransfer')
except:
pass
# plot pos
nfp = len(self.toass.gridsfloatingposemat4s)
xdist = 10
x = range(300,501,xdist)
y = range(-50,50,100*xdist/nfp)
for nid in self.regg.nodes():
if nid.startswith('ass'):
asspind = self.regg.node[nid]['assposeind']
assgind = self.regg.node[nid]['icoassgrippairsid']
nassg = len(self.toass.icoassgrippairshndmat4s[asspind])
xpos = x[asspind % len(x)]
ypos = y[asspind/len(x)]
xyzpos = [radiusgrip * math.cos(2 * math.pi / nassg * assgind)+xpos,
radiusgrip * math.sin(2 * math.pi / nassg * assgind)+ypos, 0]
self.gnodesplotpos[nid] = xyzpos[:2]
if nid.startswith('assrgt'):
self.gnodesplotpos[nid][1] = self.gnodesplotpos[nid][1] - 100
elif nid.startswith('asslft'):
self.gnodesplotpos[nid][1] = self.gnodesplotpos[nid][1] + 100
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 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 updateDBwithIK(self, gdb, robot, armname='rgt'):
"""
:param gdb:
:param robot:
:param armname: the name of the arm used rgt or lft
:param rethandx: the distance of retract along handx direction, default 50mm
:param retworldz: the distance of retract along worldz direction, default 50mm
:param retworlda: the distance of retract along assembly/approaching direction in the world, default 50mm
:return:
author: weiwei
date: 20170111
"""
# load idarm
idarm = gdb.loadIdArm(armname)
# load retraction distances
rethandx, retworldz, retworlda, worldz = gdb.loadIKRet()
# worlda is default for the general grasps on table top
# for assembly at start and goal, worlda is computed by assembly planner
worlda = Vec3(0, 0, 1)
# select idrobot
idrobot = gdb.loadIdRobot(robot)
feasibility = {}
feasibility_handx = {}
feasibility_handxworldz = {}
feasibility_worlda = {}
feasibility_worldaworldz = {}
sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \
tabletopgrips.rotmat FROM tabletopgrips, tabletopplacements, freetabletopplacement, object WHERE \
tabletopgrips.idtabletopplacements = tabletopplacements.idtabletopplacements AND \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the tabletopgrips first!")
idcounter = 0
tic = time.clock()
for resultrow in result:
print idcounter * 1.0 / len(result)
idcounter += 1
toc = time.clock()
print toc - tic
ttgsid = int(resultrow[0])
ttgscct0 = dc.strToV3(resultrow[1])
ttgscct1 = dc.strToV3(resultrow[2])
ttgsrotmat = dc.strToMat4(resultrow[3])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx * rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + worldz * retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + worlda * retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + worldz * retworldz
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())
if robot.numik(ttgsfgrcenternp, ttgsrotmat3np,
armname) is not None:
feasibility[ttgsid] = 'True'
else:
feasibility[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_handx, ttgsrotmat3np,
armname) is not None:
feasibility_handx[ttgsid] = 'True'
else:
feasibility_handx[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_handxworldz, ttgsrotmat3np,
armname) is not None:
feasibility_handxworldz[ttgsid] = 'True'
else:
feasibility_handxworldz[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_worlda, ttgsrotmat3np,
armname) is not None:
feasibility_worlda[ttgsid] = 'True'
else:
feasibility_worlda[ttgsid] = 'False'
if robot.numik(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np,
armname) is not None:
feasibility_worldaworldz[ttgsid] = 'True'
else:
feasibility_worldaworldz[ttgsid] = 'False'
# insert ik table
sql = "INSERT IGNORE INTO ik(idrobot, idarm, idtabletopgrips, feasibility, feasibility_handx, feasibility_handxworldz, \
feasibility_worlda, feasibility_worldaworldz) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s', '%s')" % \
(idrobot, idarm, ttgsid, feasibility[ttgsid], feasibility_handx[ttgsid], feasibility_handxworldz[ttgsid], \
feasibility_worlda[ttgsid], feasibility_worldaworldz[ttgsid])
gdb.execute(sql)
def __loadGripsToBuildGraph(self, armname = "rgt"):
"""
load tabletopgrips
retraction distance are also loaded from database
:param robot: an robot defined in robotsim.hrp5 or robotsim.nextage
:param gdb: an object of the database.GraspDB class
:param idarm: value = 1 "lft" or 2 "rgt", which arm to use
:return:
author: weiwei
date: 20170112
"""
# load idarm
idarm = gdb.loadIdArm(armname)
# get the global grip ids
# and prepare the global edges
# for each globalgripid, find all its tabletopids (pertaining to placements)
globalidsedges = {}
sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan freeairgrip first!")
for ggid in result:
globalidsedges[str(ggid[0])] = []
self.globalgripids.append(ggid[0])
sql = "SELECT tabletopplacements.idtabletopplacements, angle.value, \
tabletopplacements.idfreetabletopplacement, tabletopplacements.tabletopposition, \
tabletopplacements.rotmat FROM \
tabletopplacements,freetabletopplacement,angle,object WHERE \
tabletopplacements.idangle=angle.idangle AND \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND \
object.name LIKE '%s' AND angle.value IN (0.0, 45.0, 90.0, 135.0, 180.0, 225.0, 270.0, 315.0)" \
% self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
tpsrows = np.array(result)
# nubmer of discreted rotation
self.angles = list(set(map(float, tpsrows[:,1])))
# for plotting
self.fttpsids = list(set(map(int, tpsrows[:,2])))
self.nfttps = len(self.fttpsids)
idrobot = self.gdb.loadIdRobot(self.robot)
for i, idtps in enumerate(tpsrows[:,0]):
sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \
tabletopgrips.rotmat, tabletopgrips.jawwidth, tabletopgrips.idfreeairgrip \
FROM tabletopgrips,ik,freeairgrip,hand WHERE tabletopgrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND\
tabletopgrips.idtabletopgrips=ik.idtabletopgrips AND \
tabletopgrips.idtabletopplacements = %d AND ik.idrobot=%d AND \
ik.feasibility='True' AND ik.feasibility_handx='True' AND ik.feasibility_handxworldz='True' \
AND ik.feasibility_worlda='True' AND ik.feasibility_worldaworldz='True' AND ik.idarm = %d \
AND hand.name LIKE '%s'" \
% (int(idtps), idrobot, idarm, self.handpkg.getHandName())
resultttgs = self.gdb.execute(sql)
if len(resultttgs)==0:
continue
localidedges = []
for ttgsrow in resultttgs:
ttgsid = int(ttgsrow[0])
ttgscct0 = dc.strToV3(ttgsrow[1])
ttgscct1 = dc.strToV3(ttgsrow[2])
ttgsrotmat = dc.strToMat4(ttgsrow[3])
ttgsjawwidth = float(ttgsrow[4])
ttgsidfreeair = int(ttgsrow[5])
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
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())
objrotmat4 = dc.strToMat4(tpsrows[:,4][i])
objrotmat4worlda = Mat4(objrotmat4)
objrotmat4worlda.setRow(3, objrotmat4.getRow3(3)+self.worlda*self.retworlda)
objrotmat4worldaworldz = Mat4(objrotmat4worlda)
objrotmat4worldaworldz.setRow(3, objrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node(armname+str(ttgsid), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz,
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = int(tpsrows[:,2][i]),
tabletopplacementrotmat = objrotmat4,
tabletopplacementrotmathandx = objrotmat4,
tabletopplacementrotmathandxworldz = objrotmat4,
tabletopplacementrotmatworlda = objrotmat4worlda,
tabletopplacementrotmatworldaworldz = objrotmat4worldaworldz,
angle = float(tpsrows[:,1][i]), tabletopposition = dc.strToV3(tpsrows[:,3][i]))
globalidsedges[str(ttgsidfreeair)].append(armname+str(ttgsid))
localidedges.append(armname+str(ttgsid))
# print list(itertools.combinations(ttgrows[:,0], 2))
for edge in list(itertools.combinations(localidedges, 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transit')
if len(globalidsedges) == 0:
raise ValueError("Plan tabletopgrips first!")
for globalidedgesid in globalidsedges:
for edge in list(itertools.combinations(globalidsedges[globalidedgesid], 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transfer')
# gen plot pos
# biggest circle: grips; big circle: rotation; small circle: placements
radiusplacement = 30
radiusrot = 6
radiusgrip = 1
xyplacementspos = {}
xydiscreterotspos = {}
xyzglobalgrippos = {}
for i, ttpsid in enumerate(self.fttpsids):
xydiscreterotspos[ttpsid] = {}
xyzglobalgrippos[ttpsid] = {}
xypos = [radiusplacement * math.cos(2 * math.pi / self.nfttps * i),
radiusplacement * math.sin(2 * math.pi / self.nfttps * i)]
xyplacementspos[ttpsid] = xypos
for j, anglevalue in enumerate(self.angles):
xyzglobalgrippos[ttpsid][anglevalue] = {}
xypos = [radiusrot * math.cos(anglevalue), radiusrot * math.sin(anglevalue)]
xydiscreterotspos[ttpsid][anglevalue] = \
[xyplacementspos[ttpsid][0] + xypos[0], xyplacementspos[ttpsid][1] + xypos[1]]
for k, globalgripid in enumerate(self.globalgripids):
xypos = [radiusgrip * math.cos(2 * math.pi / len(self.globalgripids) * k),
radiusgrip * math.sin(2 * math.pi / len(self.globalgripids) * k)]
xyzglobalgrippos[ttpsid][anglevalue][globalgripid] = \
[xydiscreterotspos[ttpsid][anglevalue][0] + xypos[0],
xydiscreterotspos[ttpsid][anglevalue][1] + xypos[1], 0]
for nid in self.regg.nodes():
fttpid = self.regg.node[nid]['freetabletopplacementid']
anglevalue = self.regg.node[nid]['angle']
ggid = self.regg.node[nid]['globalgripid']
tabletopposition = self.regg.node[nid]['tabletopposition']
xyzpos = map(add, xyzglobalgrippos[fttpid][anglevalue][ggid],
[tabletopposition[0], tabletopposition[1], tabletopposition[2]])
self.gnodesplotpos[nid] = xyzpos[:2]
handpkg = hrp5threenm
handpkg = rtq85nm
hrp5nmeshgen = hrp5nmesh.Hrp5NMesh(handpkg)
hrp5nmnp = hrp5nmeshgen.genmnp(hrp5nrobot)
hrp5nmnp.reparentTo(base.render)
objpos = np.array([500, -400, 305])
objrot = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]])
objrot = np.array([[0.125178158283, 0.00399381108582, 0.992126166821],
[0.98617619276, -0.109927728772, -0.123984932899],
[0.108567006886, 0.993931531906, -0.0176991540939]]).T
objrot = np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]])
objrotmat4 = pg.npToMat4(objrot)
objrotmat4 = objrotmat4 * Mat4.rotateMat(30, Vec3(1, 0, 0))
objrot = pg.mat3ToNp(objrotmat4.getUpper3())
objpos = np.array([401.67913818, -644.12841797, 0])
objrot = np.array([[1.93558640e-06, -8.36298645e-01, 5.48274219e-01],
[1.93560686e-06, -5.48274219e-01, -8.36298645e-01],
[1.00000000e+00, 2.67997166e-06, 5.57513317e-07]])
# lfthnd
# objpos = np.array([180,130,100])
# objrot = np.array([[0,0,-1],[1,0,0],[0,-1,0]])
armid = "rgt"
armjntsgoal = hrp5nrobot.numikr(objpos, objrot, armid)
if armjntsgoal is not None:
hrp5nrobot.movearmfkr(armjntsgoal, armid)
hrp5nmnp = hrp5nmeshgen.genmnp(hrp5nrobot)
# hrp5nmnp.reparentTo(base.render)
startrotmat4 = Mat4(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.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)
