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)