def saveToDB(self):
"""
save the floatingposes and their grasps to the database
:return:
author: weiwei
date: 20170221
"""
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:
for i in range(len(self.gridsfloatingposemat4s)):
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):
"""
save the floatingposes and their grasps to the database
:return:
author: weiwei
date: 20170221
"""
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:
for i in range(len(self.gridsfloatingposemat4s)):
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):
"""
: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):
"""
: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):
"""
save dropworkcellgrip
dropworkcellgrip take the position and orientation of stable object on th eworkcell into account ,
:param discretesize:
:param gdb:
:return:
author: jiayao
date: 20170816
"""
# save freetabletopgrip
idhand = gdb.loadIdHand(self.handname)
idobject = gdb.loadIdObject(self.dbobjname)
for i in range(len(self.tpsgripcontacts)):
#print self.freeairgripid[i]
sql = "INSERT INTO freegrip.dropworkcellgrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand,iddropstablepos,iddropfreegrip,idfreeairgrip) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d, %d,%d,%d)" % \
(idobject, dc.v3ToStr(self.tpsgripcontacts[i][0]), dc.v3ToStr(self.tpsgripcontacts[i][1]),
dc.v3ToStr(self.tpsgripnormals[i][0]), dc.v3ToStr(self.tpsgripnormals[i][1]),
dc.mat4ToStr(self.tpsgriprotmats[i]), str(self.tpsgripjawwidth[i]), idhand, self.dbidstablepos,self.tpsgripiddropfree[i], \
self.tpsgripidfreeair[i])
gdb.execute(sql)
print "save ok"
def saveToDB(self, gdb):
"""
save the result to mysqldatabase
:param gdb: is an object of the GraspDB class in the database package
:return:
author: jiayao
date: 20170811
"""
# save to database
gdb = db.GraspDB()
idhand = gdb.loadIdHand(self.handname)
idobject = 1
print len(self.gripcontacts)
for i in range(len(self.gripcontacts)):
sql = "INSERT INTO freegrip.dropfreegrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand,idstablepos) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d, %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, self.idstablepos)
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 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
"""
# save to database
gdb = db.GraspDB()
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT * FROM freeairgrip, object WHERE freeairgrip.idobject = object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand LIKE '%s'" % (
self.dbobjname, idhand)
result = gdb.execute(sql)
if not result:
sql = "SELECT idobject FROM object WHERE name LIKE '%s'" % self.dbobjname
returnlist = gdb.execute(sql)
if len(returnlist) != 0:
idobject = returnlist[0][0]
else:
sql = "INSERT INTO object(name) VALUES('%s')" % self.dbobjname
idobject = 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)
else:
print "Grasps already saved or duplicated filename!"
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
"""
# save to database
gdb = db.GraspDB()
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 = raw_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 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
"""
# save to database
gdb = db.GraspDB()
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT * FROM freeairgrip, object WHERE freeairgrip.idobject = object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand LIKE '%s'" % (self.dbobjname, idhand)
result = gdb.execute(sql)
if not result:
sql = "SELECT idobject FROM object WHERE name LIKE '%s'" % self.dbobjname
returnlist = gdb.execute(sql)
if len(returnlist) != 0:
idobject = returnlist[0][0]
else:
sql = "INSERT INTO object(name) VALUES('%s')" % self.dbobjname
idobject = 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)
else:
print "Grasps already saved or duplicated filename!"
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:
# 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)
else:
print "Freetabletopplacement already exist!"
# save freetabletopgrip
idhand = 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:
print "Freetabletopgrip already exist!"
def loadIKFeasibleFGPairsFromDB(self, robot):
"""
load the IK FeasibleFGPairs
: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.floatinggrippairsids = []
self.floatinggrippairshndmat4s = []
self.floatinggrippairscontacts = []
self.floatinggrippairsnormals = []
self.floatinggrippairsjawwidths = []
self.floatinggrippairsidfreeairs = []
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)
if len(result) != 0:
idfloatingposes = result[0][0]
floatinggrippairsids = []
floatinggrippairshndmat4s = []
floatinggrippairscontacts = []
floatinggrippairsnormals = []
floatinggrippairsjawwidths = []
floatinggrippairsidfreeairs = []
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 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_handx like 'True' AND \
ikfg0.idrobot = %d AND ikfg0.idarm = %d AND \
fg1.idfloatinggrips = ikfg1.idfloatinggrips AND ikfg1.feasibility like 'True' AND ikfg1.feasibility_handx 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])])
self.floatinggrippairsids.append(floatinggrippairsids)
self.floatinggrippairshndmat4s.append(floatinggrippairshndmat4s)
self.floatinggrippairscontacts.append(floatinggrippairscontacts)
self.floatinggrippairsnormals.append(floatinggrippairsnormals)
self.floatinggrippairsjawwidths.append(floatinggrippairsjawwidths)
self.floatinggrippairsidfreeairs.append(floatinggrippairsidfreeairs)
def saveToDB(self, positionlist, gdb, discretesize=8.0):
"""
: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!"
# 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)
if len(result) == 0:
# 1) select the freetabletopplacement
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 ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [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 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(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist, anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2]+";"
gdb.execute(sql)
else:
print "Tabletopplacements already exist!"
# save tabletopgrips
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT tabletopgrips.idtabletopgrips FROM tabletopgrips,freeairgrip,object WHERE \
tabletopgrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idobject=object.idobject AND object.name LIKE '%s' AND \
freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = gdb.execute(sql)
if len(result) == 0:
sql = "SELECT freetabletopplacement.idfreetabletopplacement \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
for idfree in result:
idfree = int(idfree[0])
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 grasp availalbe?
continue
# sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, \
# contactnormal1, rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES "
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 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(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:
print "Tabletopgrips already exist!"
print "Save to DB done!"
def saveToDB(self, positionlist, gdb, discretesize=4):
"""
: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!"
# save tabletopplacements
sql = "SELECT idstartgoal FROM startgoal,freetabletopplacement,object WHERE \
startgoal.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
# 1) select the freetabletopplacement
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 ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [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 to database
sql = "INSERT INTO startgoal(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist,
anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "startgoal already exist!"
print "Save to DB done!"
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:
# 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)
else:
print "Freetabletopplacement already exist!"
# save freetabletopgrip
idhand = 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:
print "Freetabletopgrip already exist!"
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, positionlist, gdb, discretesize=4.0):
"""
: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!"
# 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)
if len(result) == 0:
# 1) select the freetabletopplacement
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 ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [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 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(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist,
anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "Tabletopplacements already exist!"
# save tabletopgrips
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT tabletopgrips.idtabletopgrips FROM tabletopgrips,freeairgrip,object WHERE \
tabletopgrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idobject=object.idobject AND object.name LIKE '%s' AND \
freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = gdb.execute(sql)
if len(result) == 0:
sql = "SELECT freetabletopplacement.idfreetabletopplacement \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
for idfree in result:
idfree = int(idfree[0])
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 grasp availalbe?
continue
# sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, \
# contactnormal1, rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES "
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 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(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:
print "Tabletopgrips already exist!"
print "Save to DB done!"
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 loadIKFeasibleFGPairsFromDB(self, robot):
"""
load the IK FeasibleFGPairs
: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.floatinggrippairsids = []
self.floatinggrippairshndmat4s = []
self.floatinggrippairscontacts = []
self.floatinggrippairsnormals = []
self.floatinggrippairsjawwidths = []
self.floatinggrippairsidfreeairs = []
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)
if len(result) != 0:
idfloatingposes = result[0][0]
floatinggrippairsids = []
floatinggrippairshndmat4s = []
floatinggrippairscontacts = []
floatinggrippairsnormals = []
floatinggrippairsjawwidths = []
floatinggrippairsidfreeairs = []
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 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_handx like 'True' AND \
ikfg0.idrobot = %d AND ikfg0.idarm = %d AND \
fg1.idfloatinggrips = ikfg1.idfloatinggrips AND ikfg1.feasibility like 'True' AND ikfg1.feasibility_handx 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])])
self.floatinggrippairsids.append(floatinggrippairsids)
self.floatinggrippairshndmat4s.append(floatinggrippairshndmat4s)
self.floatinggrippairscontacts.append(floatinggrippairscontacts)
self.floatinggrippairsnormals.append(floatinggrippairsnormals)
self.floatinggrippairsjawwidths.append(floatinggrippairsjawwidths)
self.floatinggrippairsidfreeairs.append(floatinggrippairsidfreeairs)
def freegripRotMove(self):
# self.freegripid = freeairgripdata[0]
# self.freegripcontacts = freeairgripdata[1]
# self.freegripnormals = freeairgripdata[2]
# self.freegriprotmats = freeairgripdata[3]
# self.freegripjawwidth = freeairgripdata[4]
idhand = 1
idobject = 1
gdb = db.GraspDB()
sql = "SELECT dropstablepos.iddropstablepos\
FROM dropstablepos, object \
WHERE dropstablepos.idobject = object.idobject AND object.name like '%s'" % (
self.dbobjname)
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):
# rotmat = rm.rodrigues([0, 0, 1], rotangle)
rotmat = rotangle
rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
ttoppos[0], ttoppos[1], ttoppos[2], 1)
# rotmat4 = pg.cvtMat4(rotmat, ttoppos)
# rotmat4=pg.cvtMat4(rotangle, ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat * rotmat4
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)
