def saveToDB(self):
"""
:return:
"""
self.icoass0gripids = []
self.icoass1gripids = []
# save to assembly table, 0-right, 1-left
idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0)
# check if rotation already exist
sql = "SELECT * FROM assemblyx WHERE assemblyx.idassembly = %d" % idassembly
result = self.gdb.execute(sql)
if len(result) == 0:
# save rotation and grasps
# if assemblyx do not exist, assemblyxgrip0/1 dont exist
for i, poserotmat4 in enumerate(self.gridsfloatingposemat4s):
sql = "INSERT INTO assemblyx(idassembly, rotmat) VALUES (%d, '%s')" % (idassembly, dc.mat4ToStr(poserotmat4))
idassemblyx = self.gdb.execute(sql)
# object 0
self.icoass0gripids.append([])
for j, hndrotmat4 in enumerate(self.icoass0griprotmat4s[i]):
strcct0 = dc.v3ToStr(self.icoass0gripcontacts[i][j][0])
strcct1 = dc.v3ToStr(self.icoass0gripcontacts[i][j][1])
strcctn0 = dc.v3ToStr(self.icoass0gripnormals[i][j][0])
strcctn1 = dc.v3ToStr(self.icoass0gripnormals[i][j][1])
strrotmat = dc.mat4ToStr(hndrotmat4)
strjawwidth = str(self.icoass0gripjawwidth[i][j])
idfreeairgrip = self.icoass0gripidfreeair[i][j]
sql = "INSERT INTO assemblyxgrips0(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idassemblyx, idfreeairgrip) VALUES ('%s', '%s', '%s', '%s', '%s', '%s', %d, \
%d)" % (strcct0, strcct1, strcctn0, strcctn1, strrotmat, strjawwidth, idassemblyx, idfreeairgrip)
idaxg = self.gdb.execute(sql)
self.icoass0gripids[-1].append(idaxg)
# object 1
self.icoass1gripids.append([])
for j, hndrotmat4 in enumerate(self.icoass1griprotmat4s[i]):
strcct0 = dc.v3ToStr(self.icoass1gripcontacts[i][j][0])
strcct1 = dc.v3ToStr(self.icoass1gripcontacts[i][j][1])
strcctn0 = dc.v3ToStr(self.icoass1gripnormals[i][j][0])
strcctn1 = dc.v3ToStr(self.icoass1gripnormals[i][j][1])
strrotmat = dc.mat4ToStr(hndrotmat4)
strjawwidth = str(self.icoass1gripjawwidth[i][j])
idfreeairgrip = self.icoass1gripidfreeair[i][j]
sql = "INSERT INTO assemblyxgrips1(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idassemblyx, idfreeairgrip) VALUES ('%s', '%s', '%s', '%s', '%s', '%s', %d, \
%d)" % (strcct0, strcct1, strcctn0, strcctn1, strrotmat, strjawwidth, idassemblyx, idfreeairgrip)
idaxg = self.gdb.execute(sql)
self.icoass1gripids[-1].append(idaxg)
def saveToDB(self, gdb):
"""
save the result to mysqldatabase
:param gdb: is an object of the GraspDB class in the database package
:return:
author: weiwei
date: 20170110
"""
idhand = gdb.loadIdHand(self.handname)
idobject = gdb.loadIdObject(self.dbobjname)
sql = "SELECT * FROM freeairgrip, object WHERE freeairgrip.idobject LIKE '%s' AND \
freeairgrip.idhand LIKE '%s'" % (idobject, idhand)
result = gdb.execute(sql)
if len(result) > 0:
print("Grasps already saved or duplicated filename!")
isredo = input("Do you want to overwrite the database? (Y/N)")
if isredo != "Y" and isredo != "y":
print("Grasp planning aborted.")
else:
sql = "DELETE FROM freeairgrip WHERE freeairgrip.idobject LIKE '%s' AND \
freeairgrip.idhand LIKE '%s'" % (idobject, idhand)
gdb.execute(sql)
print(self.gripcontacts)
for i in range(len(self.gripcontacts)):
sql = "INSERT INTO freeairgrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d)" % \
(idobject, dc.v3ToStr(self.gripcontacts[i][0]), dc.v3ToStr(self.gripcontacts[i][1]),
dc.v3ToStr(self.gripcontactnormals[i][0]), dc.v3ToStr(self.gripcontactnormals[i][1]),
dc.mat4ToStr(self.griprotmats[i]), str(self.gripjawwidth[i]), idhand)
gdb.execute(sql)
def loadIdAssembly(self, dbobj0name, rotmat0, dbobj1name, rotmat1,
assdirect1to0):
# select assembly id
# return [] if not found
idobj0 = self.loadIdObject(dbobj0name)
idobj1 = self.loadIdObject(dbobj1name)
strrotmat0 = dc.mat4ToStr(rotmat0)
strrotmat1 = dc.mat4ToStr(rotmat1)
strassdirect1to0 = dc.v3ToStr(assdirect1to0)
sql = "SELECT idassembly FROM assembly WHERE assembly.idobject0 = %d AND assembly.idobject1 = %d AND \
assembly.rotmat0 LIKE '%s' AND assembly.rotmat1 LIKE '%s' AND assembly.assdirect1to0 LIKE '%s'" % \
(idobj0, idobj1, strrotmat0, strrotmat1, strassdirect1to0)
result = self.execute(sql)
if len(result) != 0:
idassembly = result[0][0]
else:
sql = "INSERT INTO assembly(idobject0, rotmat0, idobject1, rotmat1, assdirect1to0) VALUES (%d, '%s', %d, '%s', '%s')" % \
(idobj0, strrotmat0, idobj1, strrotmat1, strassdirect1to0)
idassembly = self.execute(sql)
return idassembly
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 saveToDB(self):
"""
save freetabletopplacement
manipulation.freetabletopplacement doesn't take into account the position and orientation of the object
it is "free" in position and rotation around z axis
in contrast, each item in regrasp.tabletopplacements has different position and orientation
it is at a specific pose in the workspace
To clearly indicate the difference, "free" is attached to the front of "freetabletopplacement"
"s" is attached to the end of "tabletopplacements"
:param discretesize:
:param gdb:
:return:
author: weiwei
date: 20170111
"""
# save freetabletopplacement
sql = "SELECT * FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
print("Freetabletopplacement 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 eachresult in result:
sql = "DELETE FROM freetabletopgrip WHERE freetabletopgrip.idfreetabletopplacement LIKE '%s'" % \
eachresult[0]
self.gdb.execute(sql)
for eachresult in result:
sql = "DELETE FROM freetabletopplacement WHERE freetabletopplacement.idfreetabletopplacement LIKE '%s'" % \
eachresult[0]
self.gdb.execute(sql)
# the fretabletopplacements for the self.dbobjname is not saved
sql = "INSERT INTO freetabletopplacement(rotmat, idobject) VALUES "
for i in range(len(self.tpsmat4s)):
sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \
(dc.mat4ToStr(self.tpsmat4s[i]), self.dbobjname)
sql = sql[:-2] + ";"
self.gdb.execute(sql)
# save freetabletopgrip
idhand = self.gdb.loadIdHand(self.handname)
sql = "SELECT * FROM freetabletopgrip,freetabletopplacement,freeairgrip,object WHERE \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = self.gdb.execute(sql)
if len(result) == 0:
for i in range(len(self.tpsmat4s)):
sql = "SELECT freetabletopplacement.idfreetabletopplacement FROM freetabletopplacement,object WHERE \
freetabletopplacement.rotmat LIKE '%s' AND \
object.name LIKE '%s'" % (dc.mat4ToStr(self.tpsmat4s[i]), self.dbobjname)
result = self.gdb.execute(sql)[0]
print(result)
if len(result) != 0:
idfreetabletopplacement = result[0]
# note self.tpsgriprotmats[i] might be empty (no cd-free grasps)
if len(self.tpsgriprotmats[i]) != 0:
sql = "INSERT INTO freetabletopgrip(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreetabletopplacement, idfreeairgrip) VALUES "
for j in range(len(self.tpsgriprotmats[i])):
cct0 = self.tpsgripcontacts[i][j][0]
cct1 = self.tpsgripcontacts[i][j][1]
cctn0 = self.tpsgripnormals[i][j][0]
cctn1 = self.tpsgripnormals[i][j][1]
sql += "('%s', '%s', '%s', '%s', '%s', '%s', %d, %d), " % \
(dc.v3ToStr(cct0), dc.v3ToStr(cct1), dc.v3ToStr(cctn0), dc.v3ToStr(cctn1), \
dc.mat4ToStr(self.tpsgriprotmats[i][j]), str(self.tpsgripjawwidth[i][j]), \
idfreetabletopplacement, self.tpsgripidfreeair[i][j])
sql = sql[:-2] + ";"
self.gdb.execute(sql)
else:
# TODO delete this one, placements and grasps always exist together
print("Freetabletopgrip already exist!")
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 __saveFPGToDB(self):
"""
save the floatingposes and their grasps to the database
:return:
author: weiwei
date: 20170221
"""
print("Saving floating poses and grasps into the database...")
sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
# the gridsfloatingposes for the self.dbobjname is not saved
sql = "INSERT INTO floatingposes(rotmat, idobject) VALUES "
for i in range(len(self.gridsfloatingposemat4s)):
# print i, "/", len(self.gridsfloatingposemat4s)
sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \
(dc.mat4ToStr(self.gridsfloatingposemat4s[i]), self.dbobjname)
sql = sql[:-2] + ";"
self.gdb.execute(sql)
sql = "SELECT * FROM floatinggrips,floatingposes,object,freeairgrip,hand WHERE \
floatinggrips.idfloatingposes = floatingposes.idfloatingposes AND \
floatingposes.idobject = object.idobject AND \
object.name LIKE '%s' AND floatinggrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = hand.idhand AND hand.name LIKE '%s'" % (
self.dbobjname, self.handpkg.getHandName())
result = self.gdb.execute(sql)
if len(result) == 0:
tic = time.time()
for i in range(len(self.gridsfloatingposemat4s)):
toc = time.time()
print(
"Saving FP and Gs to DB", " Finished: ", i, "/",
len(self.gridsfloatingposemat4s), " Time past: ",
"%.2f" % (toc - tic), "s", " Expected remaining time: ",
"%.2f" % ((toc - tic) * len(self.gridsfloatingposemat4s) /
(i + 1e-4) - (toc - tic)), "s")
sql = "SELECT floatingposes.idfloatingposes FROM floatingposes,object WHERE \
floatingposes.idobject = object.idobject AND \
floatingposes.rotmat LIKE '%s' AND \
object.name LIKE '%s'" % (dc.mat4ToStr(
self.gridsfloatingposemat4s[i]), self.dbobjname)
result = self.gdb.execute(sql)[0]
if len(result) != 0:
idfloatingposes = result[0]
sql = "SELECT * FROM floatinggrips WHERE idfloatingposes = %d" % idfloatingposes
result = self.gdb.execute(sql)
if len(result) == 0:
if len(self.floatinggripmat4s[i]) != 0:
sql = "INSERT INTO floatinggrips(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfloatingposes, idfreeairgrip) VALUES "
for j in range(len(self.floatinggripmat4s[i])):
# print "gripposes", i, "/", len(self.gridsfloatingposemat4s)
# print "grips", j, "/", len(self.floatinggripmat4s[i])
cct0 = self.floatinggripcontacts[i][j][0]
cct1 = self.floatinggripcontacts[i][j][1]
cctn0 = self.floatinggripnormals[i][j][0]
cctn1 = self.floatinggripnormals[i][j][1]
sql += "('%s', '%s', '%s', '%s', '%s', '%s', %d, %d), " % \
(dc.v3ToStr(cct0), dc.v3ToStr(cct1), dc.v3ToStr(cctn0), dc.v3ToStr(cctn1), \
dc.mat4ToStr(self.floatinggripmat4s[i][j]), str(self.floatinggripjawwidth[i][j]), \
idfloatingposes, self.floatinggripidfreeair[i][j])
sql = sql[:-2] + ";"
self.gdb.execute(sql)
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!")