def loadFreeAirGrip(self, objname, handname="rtq85"):
"""
load self.freegripid, etc. from mysqldatabase
:param handname which hand to use, rtq85 by default
:return: a list of [freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth]
author: weiwei
date: 20170112
"""
freegripid = []
freegripcontacts = []
freegripnormals = []
freegriprotmats = []
freegripjawwidth = []
# access to db
sql = "SELECT freeairgrip.idfreeairgrip, freeairgrip.contactpnt0, freeairgrip.contactpnt1, \
freeairgrip.contactnormal0, freeairgrip.contactnormal1, freeairgrip.rotmat, \
freeairgrip.jawwidth FROM freeairgrip, hand, object \
WHERE freeairgrip.idobject = object.idobject AND object.name like '%s' \
AND freeairgrip.idhand = hand.idhand AND hand.name like '%s'" % (
objname, handname)
data = self.execute(sql)
if len(data) != 0:
for i in range(len(data)):
freegripid.append(int(data[i][0]))
freegripcontacts.append(
[dc.strToV3(data[i][1]),
dc.strToV3(data[i][2])])
freegripnormals.append(
[dc.strToV3(data[i][3]),
dc.strToV3(data[i][4])])
freegriprotmats.append(dc.strToMat4(data[i][5]))
freegripjawwidth.append(float(data[i][6]))
return [
freegripid, freegripcontacts, freegripnormals, freegriprotmats,
freegripjawwidth
]
else:
return None
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 loadIKFeasibleAGPairsFromDB(self, robot):
"""
load the IK FeasibleAGPairs AG -> assgrippairs
:return:
author: weiwei
date: 20170301
"""
self.loadFromDB()
self.__loadIKFromDB(robot)
idrobot = self.gdb.loadIdRobot(robot)
idarmrgt = self.gdb.loadIdArm('rgt')
idarmlft = self.gdb.loadIdArm('lft')
self.icoassgrippairsids = []
self.icoassgrippairscontacts = []
self.icoassgrippairsnormals = []
self.icoassgrippairshndmat4s = []
self.icoassgrippairsjawwidths = []
self.icoassgrippairsidfreeairs = []
idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0)
for fpid in range(len(self.gridsfloatingposemat4s)):
sql = "SELECT assemblyx.idassemblyx FROM assemblyx WHERE assemblyx.idassembly = %d AND \
assemblyx.rotmat LIKE '%s'" % (idassembly, dc.mat4ToStr(self.gridsfloatingposemat4s[fpid]))
result = self.gdb.execute(sql)
if len(result) != 0:
idfloatingposes = result[0][0]
icoassgrippairsids = []
icoassgrippairscontacts = []
icoassgrippairsnormals = []
icoassgrippairshndmat4s = []
icoassgrippairsjawwidths = []
icoassgrippairsidfreeairs = []
sql = "SELECT assemblyxgrippairs.idassemblyxgrips0, assemblyxgrippairs.idassemblyxgrips1, \
assemblyxgrips0.contactpoint0, assemblyxgrips0.contactpoint1, assemblyxgrips0.contactnormal0, \
assemblyxgrips0.contactnormal1, assemblyxgrips0.rotmat, \
assemblyxgrips0.jawwidth, assemblyxgrips0.idfreeairgrip, \
assemblyxgrips1.contactpoint0, assemblyxgrips1.contactpoint1, assemblyxgrips1.contactnormal0, \
assemblyxgrips1.contactnormal1, assemblyxgrips1.rotmat, \
assemblyxgrips1.jawwidth, assemblyxgrips1.idfreeairgrip \
FROM assemblyxgrippairs, assemblyxgrips0, assemblyxgrips1, \
ikassemblyxgrips0, ikassemblyxgrips1 WHERE \
assemblyxgrippairs.idassemblyxgrips0 = assemblyxgrips0.idassemblyxgrips0 AND \
assemblyxgrippairs.idassemblyxgrips1 = assemblyxgrips1.idassemblyxgrips1 AND \
assemblyxgrips0.idassemblyx = %d AND assemblyxgrips1.idassemblyx = %d AND \
assemblyxgrips0.idassemblyxgrips0 = ikassemblyxgrips0.idassemblyxgrips0 AND \
ikassemblyxgrips0.feasibility like 'True' AND ikassemblyxgrips0.feasibility_assdir like 'True' AND \
ikassemblyxgrips0.idrobot = %d AND ikassemblyxgrips0.idarm = %d AND \
assemblyxgrips1.idassemblyxgrips1 = ikassemblyxgrips1.idassemblyxgrips1 AND \
ikassemblyxgrips1.feasibility like 'True' AND ikassemblyxgrips1.feasibility_assdir like 'True' AND \
ikassemblyxgrips1.idrobot = %d AND ikassemblyxgrips1.idarm = %d" % \
(idfloatingposes, idfloatingposes, idrobot, idarmrgt, idrobot, idarmlft)
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
icoassgrippairsids.append([resultrow[0], resultrow[1]])
rgtcct0 = dc.strToV3(resultrow[2])
rgtcct1 = dc.strToV3(resultrow[3])
lftcct0 = dc.strToV3(resultrow[9])
lftcct1 = dc.strToV3(resultrow[10])
icoassgrippairscontacts.append([[rgtcct0, rgtcct1], [lftcct0, lftcct1]])
rgtcctn0 = dc.strToV3(resultrow[4])
rgtcctn1 = dc.strToV3(resultrow[5])
lftcctn0 = dc.strToV3(resultrow[11])
lftcctn1 = dc.strToV3(resultrow[12])
icoassgrippairsnormals.append([[rgtcctn0, rgtcctn1], [lftcctn0, lftcctn1]])
icoassgrippairshndmat4s.append([dc.strToMat4(resultrow[6]), dc.strToMat4(resultrow[13])])
icoassgrippairsjawwidths.append([float(resultrow[7]), float(resultrow[14])])
icoassgrippairsidfreeairs.append([int(resultrow[8]), int(resultrow[15])])
self.icoassgrippairsids.append(icoassgrippairsids)
self.icoassgrippairscontacts.append(icoassgrippairscontacts)
self.icoassgrippairsnormals.append(icoassgrippairsnormals)
self.icoassgrippairshndmat4s.append(icoassgrippairshndmat4s)
self.icoassgrippairsjawwidths.append(icoassgrippairsjawwidths)
self.icoassgrippairsidfreeairs.append(icoassgrippairsidfreeairs)
def loadFromDB(self):
"""
:return:
"""
self.gridsfloatingposemat4s = []
self.icoass0gripids = []
self.icoass0gripcontacts = []
self.icoass0gripnormals = []
self.icoass0griprotmat4s = []
self.icoass0gripjawwidth = []
self.icoass0gripidfreeair = []
self.icoass1gripids = []
self.icoass1gripcontacts = []
self.icoass1gripnormals = []
self.icoass1griprotmat4s = []
self.icoass1gripjawwidth = []
self.icoass1gripidfreeair = []
idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0)
# check if rotation already exist
sql = "SELECT assemblyx.idassemblyx, assemblyx.rotmat FROM assemblyx WHERE assemblyx.idassembly = %d" % idassembly
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
poserotmat4 = dc.strToMat4(resultrow[1])
self.gridsfloatingposemat4s.append(poserotmat4)
idassemblyx = resultrow[0]
# g0
sql = "SELECT * FROM assemblyxgrips0 WHERE idassemblyx = %d" % idassemblyx
result = self.gdb.execute(sql)
if len(result) != 0:
self.icoass0gripids.append([])
self.icoass0gripcontacts.append([])
self.icoass0gripnormals.append([])
self.icoass0griprotmat4s.append([])
self.icoass0gripjawwidth.append([])
self.icoass0gripidfreeair.append([])
for resultrow in result:
self.icoass0gripids[-1].append(resultrow[0])
cct0 = dc.strToV3(resultrow[1])
cct1 = dc.strToV3(resultrow[2])
cctn0 = dc.strToV3(resultrow[3])
cctn1 = dc.strToV3(resultrow[4])
self.icoass0gripcontacts[-1].append([cct0, cct1])
self.icoass0gripnormals[-1].append([cctn0, cctn1])
self.icoass0griprotmat4s[-1].append(dc.strToMat4(resultrow[5]))
self.icoass0gripjawwidth[-1].append(float(resultrow[6]))
self.icoass0gripidfreeair[-1].append(int(resultrow[8]))
# g1
sql = "SELECT * FROM assemblyxgrips1 WHERE idassemblyx = %d" % idassemblyx
result = self.gdb.execute(sql)
if len(result) != 0:
self.icoass1gripids.append([])
self.icoass1gripcontacts.append([])
self.icoass1gripnormals.append([])
self.icoass1griprotmat4s.append([])
self.icoass1gripjawwidth.append([])
self.icoass1gripidfreeair.append([])
for resultrow in result:
self.icoass1gripids[-1].append(resultrow[0])
cct0 = dc.strToV3(resultrow[1])
cct1 = dc.strToV3(resultrow[2])
cctn0 = dc.strToV3(resultrow[3])
cctn1 = dc.strToV3(resultrow[4])
self.icoass1gripcontacts[-1].append([cct0, cct1])
self.icoass1gripnormals[-1].append([cctn0, cctn1])
self.icoass1griprotmat4s[-1].append(dc.strToMat4(resultrow[5]))
self.icoass1gripjawwidth[-1].append(float(resultrow[6]))
self.icoass1gripidfreeair[-1].append(int(resultrow[8]))
def loadIKfeasibleGPfromDB(self, robot):
"""
load the IK Feasible handover pairs
:return:
author: weiwei
date: 20170301
"""
self.loadFPGfromDB()
self.loadIKFromDB(robot)
idrobot = self.gdb.loadIdRobot(robot)
idarmrgt = self.gdb.loadIdArm('rgt')
idarmlft = self.gdb.loadIdArm('lft')
self.floatinggrippairsids = []
self.floatinggrippairshndmat4s = []
self.floatinggrippairscontacts = []
self.floatinggrippairsnormals = []
self.floatinggrippairsjawwidths = []
self.floatinggrippairsidfreeairs = []
self.floatinggrippairsjnts = []
self.floatinggrippairsjnts_handa = []
for fpid in range(len(self.gridsfloatingposemat4s)):
sql = "SELECT floatingposes.idfloatingposes FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s' AND floatingposes.rotmat LIKE '%s'" % \
(self.dbobjname, dc.mat4ToStr(self.gridsfloatingposemat4s[fpid]))
result = self.gdb.execute(sql)
floatinggrippairsids = []
floatinggrippairshndmat4s = []
floatinggrippairscontacts = []
floatinggrippairsnormals = []
floatinggrippairsjawwidths = []
floatinggrippairsidfreeairs = []
floatinggrippairsjnts = []
floatinggrippairsjnts_handa = []
if len(result) != 0:
idfloatingposes = result[0][0]
sql = "SELECT floatinggripspairs.idfloatinggrips0, floatinggripspairs.idfloatinggrips1, \
fg0.contactpoint0, fg0.contactpoint1, fg0.contactnormal0, fg0.contactnormal1, fg0.rotmat, \
fg0.jawwidth, fg0.idfreeairgrip, \
fg1.contactpoint0, fg1.contactpoint1, fg1.contactnormal0, fg1.contactnormal1, fg1.rotmat, \
fg1.jawwidth, fg1.idfreeairgrip, ikfg0.jnts, ikfg0.jnts_handa, \
ikfg1.jnts, ikfg1.jnts_handa FROM floatinggripspairs, floatinggrips fg0, floatinggrips fg1, \
ikfloatinggrips ikfg0, ikfloatinggrips ikfg1 WHERE \
floatinggripspairs.idfloatinggrips0 = fg0.idfloatinggrips AND \
floatinggripspairs.idfloatinggrips1 = fg1.idfloatinggrips AND \
fg0.idfloatingposes = %d AND fg1.idfloatingposes = %d AND \
fg0.idfloatinggrips = ikfg0.idfloatinggrips AND \
ikfg0.feasibility like 'True' AND ikfg0.feasibility_handa like 'True' AND \
ikfg0.idrobot = %d AND ikfg0.idarm = %d AND \
fg1.idfloatinggrips = ikfg1.idfloatinggrips AND ikfg1.feasibility like 'True' \
AND ikfg1.feasibility_handa like 'True' AND \
ikfg1.idrobot = %d AND ikfg1.idarm = %d" % \
(idfloatingposes, idfloatingposes, idrobot, idarmrgt, idrobot, idarmlft)
result = self.gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
floatinggrippairsids.append(
[resultrow[0], resultrow[1]])
floatinggrippairshndmat4s.append([
dc.strToMat4(resultrow[6]),
dc.strToMat4(resultrow[13])
])
rgtcct0 = dc.strToV3(resultrow[2])
rgtcct1 = dc.strToV3(resultrow[3])
lftcct0 = dc.strToV3(resultrow[9])
lftcct1 = dc.strToV3(resultrow[10])
floatinggrippairscontacts.append([[rgtcct0, rgtcct1],
[lftcct0, lftcct1]])
rgtcctn0 = dc.strToV3(resultrow[4])
rgtcctn1 = dc.strToV3(resultrow[5])
lftcctn0 = dc.strToV3(resultrow[11])
lftcctn1 = dc.strToV3(resultrow[12])
floatinggrippairsnormals.append([[rgtcctn0, rgtcctn1],
[lftcctn0, lftcctn1]])
floatinggrippairsjawwidths.append(
[float(resultrow[7]),
float(resultrow[14])])
floatinggrippairsidfreeairs.append(
[int(resultrow[8]),
int(resultrow[15])])
floatinggrippairsjnts.append([
dc.strToList(resultrow[16]),
dc.strToList(resultrow[18])
])
floatinggrippairsjnts_handa.append([
dc.strToList(resultrow[17]),
dc.strToList(resultrow[19])
])
self.floatinggrippairsids.append(floatinggrippairsids)
self.floatinggrippairshndmat4s.append(floatinggrippairshndmat4s)
self.floatinggrippairscontacts.append(floatinggrippairscontacts)
self.floatinggrippairsnormals.append(floatinggrippairsnormals)
self.floatinggrippairsjawwidths.append(floatinggrippairsjawwidths)
self.floatinggrippairsidfreeairs.append(
floatinggrippairsidfreeairs)
self.floatinggrippairsjnts.append(floatinggrippairsjnts)
self.floatinggrippairsjnts_handa.append(
floatinggrippairsjnts_handa)
def loadFPGfromDB(self):
"""
for debug purpose
load the floatingposes and their grasps from the database
:return:
author: weiwei
date: 20170227
"""
print("Loading FP and Gs to update floatinggripids...")
self.gridsfloatingposemat4s = []
self.floatinggripids = []
self.floatinggripmat4s = []
self.floatinggripcontacts = []
self.floatinggripnormals = []
self.floatinggripjawwidth = []
self.floatinggripidfreeair = []
sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
fposesresult = self.gdb.execute(sql)
if len(fposesresult) != 0:
tic = time.time()
for i, resultrow in enumerate(fposesresult):
toc = time.time()
print(
"Loading floatinggripids", " Finished: ", i, "/",
len(fposesresult), " Time past: ", "%.2f" % (toc - tic),
"s", " Expected remaining time: ",
"%.2f" % ((toc - tic) * len(fposesresult) / (i + 1e-4) -
(toc - tic)), "s")
self.gridsfloatingposemat4s.append(dc.strToMat4(resultrow[1]))
idfloatingposes = resultrow[0]
sql = "SELECT floatinggrips.idfloatinggrips, floatinggrips.contactpoint0, \
floatinggrips.contactpoint1, floatinggrips.contactnormal0, \
floatinggrips.contactnormal1, floatinggrips.rotmat, floatinggrips.jawwidth, \
floatinggrips.idfreeairgrip FROM floatinggrips,freeairgrip,hand WHERE \
floatinggrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND floatinggrips.idfloatingposes = %d AND \
hand.name = '%s'" % (idfloatingposes,
self.handpkg.getHandName())
fgresult = self.gdb.execute(sql)
if len(fgresult) != 0:
floatinggripids = []
floatinggripmat4s = []
floatinggripcontacts = []
floatinggripnormals = []
floatinggripjawwidths = []
floatinggripidfreeairs = []
for fgresultrow in fgresult:
cct0 = dc.strToV3(fgresultrow[1])
cct1 = dc.strToV3(fgresultrow[2])
cctn0 = dc.strToV3(fgresultrow[3])
cctn1 = dc.strToV3(fgresultrow[4])
floatinggripids.append(int(fgresultrow[0]))
floatinggripmat4s.append(dc.strToMat4(fgresultrow[5]))
floatinggripcontacts.append([cct0, cct1])
floatinggripnormals.append([cctn0, cctn1])
floatinggripjawwidths.append(float(fgresultrow[6]))
floatinggripidfreeairs.append(int(fgresultrow[7]))
self.floatinggripids.append(floatinggripids)
self.floatinggripmat4s.append(floatinggripmat4s)
self.floatinggripcontacts.append(floatinggripcontacts)
self.floatinggripnormals.append(floatinggripnormals)
self.floatinggripjawwidth.append(floatinggripjawwidths)
self.floatinggripidfreeair.append(floatinggripidfreeairs)
else:
print('Plan floating grips first!')
assert (False)
else:
assert ('No object found!')
def saveToDB(self, positionlist, gdb, discretesize=8):
"""
:param positionlist: a list of positions to place the object one the table
:param discretesize: the discretization of rotation angles around z axis
:return:
author: weiwei
date: 20161215, osaka
"""
# save discretiezed angle
sql = "SELECT * FROM angle"
result = gdb.execute(sql)
if len(result) == 0:
sql = "INSERT INTO angle(value) VALUES "
for i in range(discretesize):
sql += "("+str(360*i*1.0/discretesize)+"), "
sql = sql[:-2]+";"
gdb.execute(sql)
else:
print("Angles already set!")
# 1) select the tabletopplacements
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise Exception("Plan the freetabletopplacement first!")
return
result = np.asarray(result)
idfreettplist = [int(x) for x in result[:, 0]]
rotmatfreettplist = [dc.strToMat4(x) for x in result[:, 1]]
# 2) select the angle
sql = "SELECT angle.idangle,angle.value FROM angle"
result = np.asarray(gdb.execute(sql))
idanglelist = [int(x) for x in result[:, 0]]
anglevaluelist = [float(x) for x in result[:, 1]]
# 3) save tabletopplacements
sql = "SELECT idtabletopplacements FROM tabletopplacements,freetabletopplacement,object WHERE \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
# 3.1) check if already exist
if len(result) != 0:
print("Tabletopplacements already exist! ")
isredo = input("Do you want to overwrite the database? (Y/N)")
if isredo != "Y" and isredo != "y":
print("Placement planning aborted.")
return
else:
for idfreettp in idfreettplist:
sql = "DELETE FROM tabletopplacements WHERE \
tabletopplacements.idfreetabletopplacement LIKE '%s'" % idfreettp
gdb.execute(sql)
print("Old tabletopplacements have been deleted!")
# save to database
sql = "INSERT INTO tabletopplacements(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreettplist, rotmatfreettplist):
for idangle, anglevalue in zip(idanglelist, anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
rotmat4 = pg.npToMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2]+";"
gdb.execute(sql)
print("Tabletop placements saved!")
# save tabletopgrips
isttg, ttgresult = self.__getTtg(gdb)
if not isttg:
idhand = gdb.loadIdHand(self.handname)
for idfree in idfreettplist:
sql = "SELECT tabletopplacements.idtabletopplacements, \
tabletopplacements.tabletopposition, angle.value,\
freetabletopgrip.contactpoint0, freetabletopgrip.contactpoint1, \
freetabletopgrip.contactnormal0, freetabletopgrip.contactnormal1, \
freetabletopgrip.rotmat, freetabletopgrip.jawwidth, freetabletopgrip.idfreeairgrip \
FROM tabletopplacements,freetabletopplacement,freetabletopgrip,freeairgrip,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = %d AND \
freetabletopplacement.idfreetabletopplacement = %d" % (idhand, idfree)
result1 = gdb.execute(sql)
if len(result1) == 0:
# no availalbe grasps
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]]
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):
rotmat = rm.rodrigues([0, 0, 1], rotangle)
rotmat4 = pg.npToMat4(rotmat, ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat*rotmat4
sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES \
('%s', '%s', '%s', '%s', '%s', '%s', %d, %d) " % \
(dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \
dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements)
gdb.execute(sql)
else:
# TODO delete this one, placements and grasps always exist together
print("Tabletopgrips already exist!")
raise Exception("Warning! Grasps are NOT saved!")
return
print("Grasps saved!")
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!")