def __loadGripsToBuildGraph(self, armname="rgt"): """ load tabletopgrips retraction distance are also loaded from database :param robot: an robot defined in robotsim.hrp5 or robotsim.nextage :param gdb: an object of the database.GraspDB class :param idarm: value = 1 "lft" or 2 "rgt", which arm to use :return: author: weiwei date: 20170112 """ # load idarm idarm = gdb.loadIdArm(armname) # get the global grip ids # and prepare the global edges # for each globalgripid, find all its tabletopids (pertaining to placements) globalidsedges = {} sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \ object.name LIKE '%s'" % self.dbobjname result = self.gdb.execute(sql) if len(result) == 0: raise ValueError("Plan freeairgrip first!") for ggid in result: globalidsedges[str(ggid[0])] = [] self.globalgripids.append(ggid[0]) sql = "SELECT tabletopplacements.idtabletopplacements, angle.value, \ tabletopplacements.idfreetabletopplacement, tabletopplacements.tabletopposition, \ tabletopplacements.rotmat FROM \ tabletopplacements,freetabletopplacement,angle,object WHERE \ tabletopplacements.idangle=angle.idangle AND \ tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \ freetabletopplacement.idobject=object.idobject AND \ object.name LIKE '%s' AND angle.value IN (0.0, 45.0, 90.0, 135.0, 180.0, 225.0, 270.0, 315.0)" \ % self.dbobjname result = self.gdb.execute(sql) if len(result) != 0: tpsrows = np.array(result) # nubmer of discreted rotation self.angles = list(set(map(float, tpsrows[:, 1]))) # for plotting self.fttpsids = list(set(map(int, tpsrows[:, 2]))) self.nfttps = len(self.fttpsids) idrobot = self.gdb.loadIdRobot(self.robot) for i, idtps in enumerate(tpsrows[:, 0]): sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \ tabletopgrips.rotmat, tabletopgrips.jawwidth, tabletopgrips.idfreeairgrip \ FROM tabletopgrips,ik,freeairgrip,hand WHERE tabletopgrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \ freeairgrip.idhand = hand.idhand AND\ tabletopgrips.idtabletopgrips=ik.idtabletopgrips AND \ tabletopgrips.idtabletopplacements = %d AND ik.idrobot=%d AND \ ik.feasibility='True' AND ik.feasibility_handx='True' AND ik.feasibility_handxworldz='True' \ AND ik.feasibility_worlda='True' AND ik.feasibility_worldaworldz='True' AND ik.idarm = %d \ AND hand.name LIKE '%s'" \ % (int(idtps), idrobot, idarm, self.handpkg.getHandName()) resultttgs = self.gdb.execute(sql) if len(resultttgs) == 0: continue localidedges = [] for ttgsrow in resultttgs: ttgsid = int(ttgsrow[0]) ttgscct0 = dc.strToV3(ttgsrow[1]) ttgscct1 = dc.strToV3(ttgsrow[2]) ttgsrotmat = dc.strToMat4(ttgsrow[3]) ttgsjawwidth = float(ttgsrow[4]) ttgsidfreeair = int(ttgsrow[5]) ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx * self.rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz * self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda * self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + self.worldz * self.retworldz ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp( ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp( ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) objrotmat4 = dc.strToMat4(tpsrows[:, 4][i]) objrotmat4worlda = Mat4(objrotmat4) objrotmat4worlda.setRow( 3, objrotmat4.getRow3(3) + self.worlda * self.retworlda) objrotmat4worldaworldz = Mat4(objrotmat4worlda) objrotmat4worldaworldz.setRow( 3, objrotmat4worlda.getRow3(3) + self.worldz * self.retworldz) self.regg.add_node( armname + str(ttgsid), fgrcenter=ttgsfgrcenternp, fgrcenterhandx=ttgsfgrcenternp_handx, fgrcenterhandxworldz=ttgsfgrcenternp_handxworldz, fgrcenterworlda=ttgsfgrcenternp_worlda, fgrcenterworldaworldz=ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid=ttgsidfreeair, freetabletopplacementid=int(tpsrows[:, 2][i]), tabletopplacementrotmat=objrotmat4, tabletopplacementrotmathandx=objrotmat4, tabletopplacementrotmathandxworldz=objrotmat4, tabletopplacementrotmatworlda=objrotmat4worlda, tabletopplacementrotmatworldaworldz= objrotmat4worldaworldz, angle=float(tpsrows[:, 1][i]), tabletopposition=dc.strToV3(tpsrows[:, 3][i])) globalidsedges[str(ttgsidfreeair)].append(armname + str(ttgsid)) localidedges.append(armname + str(ttgsid)) # print list(itertools.combinations(ttgrows[:,0], 2)) for edge in list(itertools.combinations(localidedges, 2)): self.regg.add_edge(*edge, weight=1, edgetype='transit') if len(globalidsedges) == 0: raise ValueError("Plan tabletopgrips first!") for globalidedgesid in globalidsedges: for edge in list( itertools.combinations(globalidsedges[globalidedgesid], 2)): self.regg.add_edge(*edge, weight=1, edgetype='transfer') # gen plot pos # biggest circle: grips; big circle: rotation; small circle: placements radiusplacement = 30 radiusrot = 6 radiusgrip = 1 xyplacementspos = {} xydiscreterotspos = {} xyzglobalgrippos = {} for i, ttpsid in enumerate(self.fttpsids): xydiscreterotspos[ttpsid] = {} xyzglobalgrippos[ttpsid] = {} xypos = [ radiusplacement * math.cos(2 * math.pi / self.nfttps * i), radiusplacement * math.sin(2 * math.pi / self.nfttps * i) ] xyplacementspos[ttpsid] = xypos for j, anglevalue in enumerate(self.angles): xyzglobalgrippos[ttpsid][anglevalue] = {} xypos = [ radiusrot * math.cos(anglevalue), radiusrot * math.sin(anglevalue) ] xydiscreterotspos[ttpsid][anglevalue] = \ [xyplacementspos[ttpsid][0] + xypos[0], xyplacementspos[ttpsid][1] + xypos[1]] for k, globalgripid in enumerate(self.globalgripids): xypos = [ radiusgrip * math.cos(2 * math.pi / len(self.globalgripids) * k), radiusgrip * math.sin(2 * math.pi / len(self.globalgripids) * k) ] xyzglobalgrippos[ttpsid][anglevalue][globalgripid] = \ [xydiscreterotspos[ttpsid][anglevalue][0] + xypos[0], xydiscreterotspos[ttpsid][anglevalue][1] + xypos[1], 0] for nid in self.regg.nodes(): fttpid = self.regg.node[nid]['freetabletopplacementid'] anglevalue = self.regg.node[nid]['angle'] ggid = self.regg.node[nid]['globalgripid'] tabletopposition = self.regg.node[nid]['tabletopposition'] xyzpos = map(add, xyzglobalgrippos[fttpid][anglevalue][ggid], [ tabletopposition[0], tabletopposition[1], tabletopposition[2] ]) self.gnodesplotpos[nid] = xyzpos[:2]
def __addstartgoal(self, startrotmat4, goalrotmat4, base): """ add start and goal for the regg :param startrotmat4 and goalrotmat4: both are 4by4 panda3d matrix :return: author: weiwei date: 20161216, sapporo """ ### start # the node id of a globalgripid in startnode nodeidofglobalidinstart= {} # the startnodeids is also for quick access self.startnodeids = [] for j, rotmat in enumerate(self.freegriprotmats1): #print j, len(self.freegriprotmats) # print rotmat ttgsrotmat = rotmat * startrotmat4 # for collision detection, we move the object back to x=0,y=0 ttgsrotmatx0y0 = Mat4(startrotmat4) ttgsrotmatx0y0.setCell(3,0,0) ttgsrotmatx0y0.setCell(3,1,0) ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0 # check if the hand collide with tabletop # tmphnd = self.robothand tmphnd = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) initmat = tmphnd.getMat() initjawwidth = tmphnd.jawwidth # set jawwidth to 80 to avoid collision with surrounding obstacles # set to gripping with is unnecessary tmphnd.setJawwidth(80) tmphnd.setMat(ttgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp) result = self.bulletworld.contactTest(hndbullnode) # tmphnd.setMat(ttgsrotmat) # tmphnd.reparentTo(base.render) # if j > 3: # base.run() if not result.getNumContacts(): ttgscct0=startrotmat4.xformPoint(self.freegripcontacts1[j][0]) ttgscct1=startrotmat4.xformPoint(self.freegripcontacts1[j][1]) ttgsfgrcenter = (ttgscct0+ttgscct1)/2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx # handxworldz is not necessary for start # ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz ttgsjawwidth = self.freegripjawwidth1[j] ttgsidfreeair = self.freegripid1[j] ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) # handxworldz is not necessary for start # ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) #print "solving starting iks" ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np) ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np) ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np) ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np) if (ikc is not None) and (ikcx is not None) and (ikca is not None) and (ikcaz is not None): # note the tabletopposition here is not the contact for the intermediate states # it is the zero pos tabletopposition = startrotmat4.getRow3(3) startrotmat4worlda = Mat4(startrotmat4) startrotmat4worlda.setRow(3, startrotmat4.getRow3(3)+self.worlda*self.retworlda) startrotmat4worldaworldz = Mat4(startrotmat4worlda) startrotmat4worldaworldz.setRow(3, startrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.regg.add_node('start'+str(j), fgrcenter=ttgsfgrcenternp, fgrcenterhandx = ttgsfgrcenternp_handx, fgrcenterhandxworldz = 'na', fgrcenterworlda = ttgsfgrcenternp_worlda, fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid = ttgsidfreeair, freetabletopplacementid = 'na', tabletopplacementrotmat = startrotmat4, tabletopplacementrotmathandx = startrotmat4, tabletopplacementrotmathandxworldz = 'na', tabletopplacementrotmatworlda = startrotmat4worlda, tabletopplacementrotmatworldaworldz = startrotmat4worldaworldz, angle = 'na', tabletopposition = tabletopposition) nodeidofglobalidinstart[ttgsidfreeair]='start'+str(j) self.startnodeids.append('start'+str(j)) # tmprtq85.reparentTo(base.render) tmphnd.setMat(initmat) tmphnd.setJawwidth(initjawwidth) if len(self.startnodeids) == 0: #raise ValueError("No available starting grip!") print ("No available start grip!") return False # add start transit edge for edge in list(itertools.combinations(self.startnodeids, 2)): self.regg.add_edge(*edge, weight = 1, edgetype = 'starttransit') # add start transfer edge for reggnode, reggnodedata in self.regg.nodes(data=True): if reggnode.startswith('mid'): globalgripid = reggnodedata['globalgripid'] if globalgripid in nodeidofglobalidinstart.keys(): startnodeid = nodeidofglobalidinstart[globalgripid] self.regg.add_edge(startnodeid, reggnode, weight=1, edgetype = 'starttransfer') ### goal # the node id of a globalgripid in goalnode, for quick setting up edges nodeidofglobalidingoal= {} # the goalnodeids is also for quick access self.goalnodeids = [] for j, rotmat in enumerate(self.freegriprotmats1): #print j, len(self.freegriprotmats) ttgsrotmat = rotmat * goalrotmat4 # for collision detection, we move the object back to x=0,y=0 ttgsrotmatx0y0 = Mat4(goalrotmat4) ttgsrotmatx0y0.setCell(3,0,0) ttgsrotmatx0y0.setCell(3,1,0) ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0 # check if the hand collide with tabletop tmphnd = self.robothand initmat = tmphnd.getMat() initjawwidth = tmphnd.jawwidth tmphnd.setJawwidth(self.freegripjawwidth1[j]) tmphnd.setMat(ttgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): ttgscct0=goalrotmat4.xformPoint(self.freegripcontacts1[j][0]) ttgscct1=goalrotmat4.xformPoint(self.freegripcontacts1[j][1]) ttgsfgrcenter = (ttgscct0+ttgscct1)/2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz ttgsjawwidth = self.freegripjawwidth1[j] ttgsidfreeair = self.freegripid1[j] ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) #print "solving goal iks" ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np) ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np) ikcxz = self.robot.numikr(ttgsfgrcenternp_handxworldz, ttgsrotmat3np) ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np) ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np) if (ikc is not None) and (ikcx is not None) and (ikcxz is not None) \ and (ikca is not None) and (ikcaz is not None): # note the tabletopposition here is not the contact for the intermediate states # it is the zero pos tabletopposition = goalrotmat4.getRow3(3) goalrotmat4worlda = Mat4(goalrotmat4) goalrotmat4worlda.setRow(3, goalrotmat4.getRow3(3)+self.worlda*self.retworlda) goalrotmat4worldaworldz = Mat4(goalrotmat4worlda) goalrotmat4worldaworldz.setRow(3, goalrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.regg.add_node('goal'+str(j), fgrcenter=ttgsfgrcenternp, fgrcenterhandx = ttgsfgrcenternp_handx, fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz, fgrcenterworlda = ttgsfgrcenternp_worlda, fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid = ttgsidfreeair, freetabletopplacementid = 'na', tabletopplacementrotmat = goalrotmat4, tabletopplacementrotmathandx = goalrotmat4, tabletopplacementrotmathandxworldz = goalrotmat4, tabletopplacementrotmatworlda = goalrotmat4worlda, tabletopplacementrotmatworldaworldz = goalrotmat4worldaworldz, angleid = 'na', tabletopposition = tabletopposition) nodeidofglobalidingoal[ttgsidfreeair]='goal'+str(j) self.goalnodeids.append('goal'+str(j)) tmphnd.setMat(initmat) tmphnd.setJawwidth(initjawwidth) if len(self.goalnodeids) == 0: #raise ValueError("No available goal grip!") print ("No available goal grip!") return False # add goal transit edges for edge in list(itertools.combinations(self.goalnodeids, 2)): self.regg.add_edge(*edge, weight = 1, edgetype = 'goaltransit') # add goal transfer edge for reggnode, reggnodedata in self.regg.nodes(data=True): if reggnode.startswith('mid'): globalgripid = reggnodedata['globalgripid'] if globalgripid in nodeidofglobalidingoal.keys(): goalnodeid = nodeidofglobalidingoal[globalgripid] self.regg.add_edge(goalnodeid, reggnode, weight=1, edgetype = 'goaltransfer') # add start to goal direct edges for startnodeid in self.startnodeids: for goalnodeid in self.goalnodeids: # startnodeggid = start node global grip id startnodeggid = self.regg.node[startnodeid]['globalgripid'] goalnodeggid = self.regg.node[goalnodeid]['globalgripid'] if startnodeggid == goalnodeggid: self.regg.add_edge(startnodeid, goalnodeid, weight=1, edgetype = 'startgoaltransfer') return True
def addEnds(self, rotmat4, armname): # the node id of a globalgripid in end nodeidofglobalidinend = {} # the endnodeids is also for quick access self.endnodeids[armname] = [] for j, rotmat in enumerate(self.freegriprotmats): assgsrotmat = rotmat * rotmat4 # for collision detection, we move the object back to x=0,y=0 assgsrotmatx0y0 = Mat4(rotmat4) assgsrotmatx0y0.setCell(3, 0, 0) assgsrotmatx0y0.setCell(3, 1, 0) assgsrotmatx0y0 = rotmat * assgsrotmatx0y0 # check if the hand collide with tabletop tmphand = self.hand initmat = tmphand.getMat() initjawwidth = tmphand.jawwidth # set jawwidth to 80 to avoid collision with surrounding obstacles # set to gripping with is unnecessary # tmphand.setJawwidth(self.freegripjawwidth[j]) tmphand.setJawwidth(tmphand.jawwidthopen) tmphand.setMat(assgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): assgscct0 = rotmat4.xformPoint(self.freegripcontacts[j][0]) assgscct1 = rotmat4.xformPoint(self.freegripcontacts[j][1]) assgsfgrcenter = (assgscct0 + assgscct1) / 2 handx = assgsrotmat.getRow3(0) assgsfgrcenterhandx = assgsfgrcenter + handx * self.rethandx # handxworldz is not necessary for start # assgsfgrcenterhandxworldz = assgsfgrcenterhandx + self.worldz*self.retworldz assgsfgrcenterworlda = assgsfgrcenter + self.worlda * self.retworlda assgsfgrcenterworldaworldz = assgsfgrcenterworlda + self.worldz * self.retworldz assgsjawwidth = self.freegripjawwidth[j] assgsidfreeair = self.freegripid[j] assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter) assgsfgrcenternp_handx = pg.v3ToNp(assgsfgrcenterhandx) # handxworldz is not necessary for start # assgsfgrcenternp_handxworldz = pg.v3ToNp(assgsfgrcenterhandxworldz) assgsfgrcenternp_worlda = pg.v3ToNp(assgsfgrcenterworlda) assgsfgrcenternp_worldaworldz = pg.v3ToNp( assgsfgrcenterworldaworldz) assgsrotmat3np = pg.mat3ToNp(assgsrotmat.getUpper3()) ikc = self.robot.numikr(assgsfgrcenternp, assgsrotmat3np) ikcx = self.robot.numikr(assgsfgrcenternp_handx, assgsrotmat3np) ikca = self.robot.numikr(assgsfgrcenternp_worlda, assgsrotmat3np) ikcaz = self.robot.numikr(assgsfgrcenternp_worldaworldz, assgsrotmat3np) if (ikc is not None) and (ikcx is not None) and ( ikca is not None) and (ikcaz is not None): # note the tabletopposition here is not the contact for the intermediate states # it is the zero pos tabletopposition = rotmat4.getRow3(3) startrotmat4worlda = Mat4(rotmat4) startrotmat4worlda.setRow( 3, rotmat4.getRow3(3) + self.worlda * self.retworlda) startrotmat4worldaworldz = Mat4(startrotmat4worlda) startrotmat4worldaworldz.setRow( 3, startrotmat4worlda.getRow3(3) + self.worldz * self.retworldz) self.graphtpp.regg.add_node( 'end' + armname + str(j), fgrcenter=assgsfgrcenternp, fgrcenterhandx=assgsfgrcenternp_handx, fgrcenterhandxworldz='na', fgrcenterworlda=assgsfgrcenternp_worlda, fgrcenterworldaworldz=assgsfgrcenternp_worldaworldz, jawwidth=assgsjawwidth, hndrotmat3np=assgsrotmat3np, globalgripid=assgsidfreeair, freetabletopplacementid='na', tabletopplacementrotmat=rotmat4, tabletopplacementrotmathandx=rotmat4, tabletopplacementrotmathandxworldz='na', tabletopplacementrotmatworlda=startrotmat4worlda, tabletopplacementrotmatworldaworldz= startrotmat4worldaworldz, angle='na', tabletopposition=tabletopposition) nodeidofglobalidinend[assgsidfreeair] = 'start' + str(j) self.endnodeids[armname].append('start' + str(j)) tmphand.setMat(initmat) tmphand.setJawwidth(initjawwidth) if len(self.endnodeids[armname]) == 0: raise ValueError("No available end grip at " + armname) # add start transit edge for edge in list(itertools.combinations(self.endnodeids[armname], 2)): self.graphtpp.regg.add_edge(*edge, weight=1, edgetype='endtransit') # add start transfer edge for reggnode, reggnodedata in self.graphtpp.regg.nodes(data=True): if reggnode.startswith(self.armname): globalgripid = reggnodedata['globalgripid'] if globalgripid in nodeidofglobalidinend.keys(): endnodeid = nodeidofglobalidinend[globalgripid] self.graphtpp.regg.add_edge(endnodeid, reggnode, weight=1, edgetype='endtransfer') for nid in self.graphtpp.regg.nodes(): if nid.startswith('end'): ggid = self.graphtpp.regg.node[nid]['globalgripid'] tabletopposition = self.graphtpp.regg.node[nid][ 'tabletopposition'] xyzpos = map(add, self.__xyzglobalgrippos_startgoal[ggid], [ tabletopposition[0], tabletopposition[1], tabletopposition[2] ]) self.gnodesplotpos[nid] = xyzpos[:2]
def updateDBwithIK(self, robot): """ compute the IK feasible grasps of each pose :return: """ # load idrobot idrobot = self.gdb.loadIdRobot(robot) self.assgikfeas0 = [] self.assgikfeas0_retassdir = [] self.assgikfeas1= [] self.assgikfeas1_retassdir = [] tic = time.time() for fpid, apmat in enumerate(self.gridsfloatingposemat4s): assdir1to0 = apmat.xformVec(self.assDirect1to0) assdir0to1 = apmat.xformVec(-self.assDirect1to0) toc = time.time() print(toc-tic) if fpid != 0: print("remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic)) print(fpid, len(self.gridsfloatingposemat4s)) ### right hand # rgt = 0 armname = 'rgt' assgikfeas0 = [] assgikfeas0_retassdir = [] for i, hndrotmat4 in enumerate(self.icoass0griprotmat4s[fpid]): assgikfeas0.append('False') assgikfeas0_retassdir.append('False') assgsfgrcenter = (self.icoass0gripcontacts[fpid][i][0]+self.icoass0gripcontacts[fpid][i][1])/2 assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter) assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3()) handx = hndrotmat4.getRow3(0) # check the angle between handx and minus y minusworldy = Vec3(0,-1,0) if Vec3(handx).angleDeg(minusworldy) < 60: assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir1to0) if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None: assgikfeas0[i] = 'True' if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None: assgikfeas0_retassdir[i] = 'True' self.icoass0gripsik.append(assgikfeas0) self.icoass0gripsik_retassdir.append(assgikfeas0_retassdir) ### left hand armname = 'lft' assgikfeas1 = [] assgikfeas1_retassdir = [] for i, hndrotmat4 in enumerate(self.icoass1griprotmat4s[fpid]): assgikfeas1.append('False') assgikfeas1_retassdir.append('False') assgsfgrcenter = (self.icoass1gripcontacts[fpid][i][0]+self.icoass1gripcontacts[fpid][i][1])/2 assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter) assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3()) handx = hndrotmat4.getRow3(0) # check the angle between handx and minus y plusworldy = Vec3(0,1,0) if Vec3(handx).angleDeg(plusworldy) < 60: assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir0to1) if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None: assgikfeas1[i] = 'True' if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None: assgikfeas1_retassdir[i] = 'True' self.icoass1gripsik.append(assgikfeas1) self.icoass1gripsik_retassdir.append(assgikfeas1_retassdir) self.__saveIKtoDB(idrobot)
def __loadGripsToBuildGraph(self, armname = "rgt"): """ load tabletopgrips retraction distance are also loaded from database :param robot: an robot defined in robotsim.hrp5 or robotsim.nextage :param gdb: an object of the database.GraspDB class :param idarm: value = 1 "lft" or 2 "rgt", which arm to use :return: author: weiwei date: 20170112 """ # load idarm idarm = self.gdb.loadIdArm(armname) idhand = self.gdb.loadIdHand(self.handname) # get the global grip ids # and prepare the global edges # for each globalgripid, find all its tabletopids (pertaining to placements) globalidsedges = {} sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \ object.name LIKE '%s' AND freeairgrip.idhand = %d" % (self.dbobjname, idhand) # sql = "SELECT dropfreegrip.iddropfreegrip FROM dropfreegrip,object WHERE dropfreegrip.idobject=object.idobject AND \ # object.name LIKE '%s' AND dropfreegrip.idhand = %d" % (self.dbobjname, idhand) # sql = "SELECT dropworkcellgrip.iddropworkcellgrip FROM dropworkcellgrip,object WHERE dropworkcellgrip.idobject=object.idobject AND \ # object.name LIKE '%s' AND dropworkcellgrip.idhand = %d" % (self.dbobjname, idhand) result = self.gdb.execute(sql) if len(result) == 0: raise ValueError("Plan freeairgrip first!") for ggid in result: globalidsedges[str(ggid[0])] = [] self.globalgripids.append(ggid[0]) sql = "SELECT dropstablepos.iddropstablepos,dropstablepos.rot,dropstablepos.pos,angle_drop.value FROM \ dropstablepos,object,angle_drop WHERE \ dropstablepos.idobject=object.idobject AND \ object.name LIKE '%s' " % self.dbobjname result = self.gdb.execute(sql) if len(result) != 0: tpsrows = np.array(result) #self.angles = list([0.0]) self.angles = list(set(map(float, tpsrows[:, 3]))) # for plotting self.fttpsids = list(set(map(int, tpsrows[:, 0]))) self.nfttps = len(self.fttpsids) idrobot = self.gdb.loadIdRobot(self.robot) for i, idtps in enumerate(tpsrows[:,0]): sql = "SELECT dropworkcellgrip.iddropworkcellgrip, dropworkcellgrip.contactpnt0, dropworkcellgrip.contactpnt1, \ dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth ,dropworkcellgrip.idfreeairgrip\ FROM dropworkcellgrip,dropfreegrip,freeairgrip,ik_drop\ WHERE \ dropworkcellgrip.iddropstablepos = %d \ AND dropworkcellgrip.iddropworkcellgrip=ik_drop.iddropworkcellgrip AND ik_drop.idrobot=%d AND ik_drop.idarm = %d AND\ ik_drop.feasibility='True' AND ik_drop.feasibility_handx='True' AND ik_drop.feasibility_handxworldz='True' \ AND ik_drop.feasibility_worlda='True' AND ik_drop.feasibility_worldaworldz='True' \ AND dropworkcellgrip.idfreeairgrip = freeairgrip.idfreeairgrip \ AND dropworkcellgrip.idhand = % d AND dropworkcellgrip.iddropfreegrip = dropfreegrip.iddropfreegrip " \ % (int(idtps),idrobot, idarm, idhand) resultttgs = self.gdb.execute(sql) if len(resultttgs)==0: print "no result" continue localidedges = [] for ttgsrow in resultttgs: ttgsid = int(ttgsrow[0]) ttgscct0 = dc.strToV3(ttgsrow[1]) ttgscct1 = dc.strToV3(ttgsrow[2]) ttgsrotmat = dc.strToMat4(ttgsrow[3]) ttgsjawwidth = float(ttgsrow[4]) ttgsidfreeair = int(ttgsrow[5]) ttgsfgrcenter = (ttgscct0+ttgscct1)/2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) objrotmat4 = pg.npToMat4(np.transpose(pg.mat3ToNp(dc.strToMat3(tpsrows[:, 1][i]))), pg.v3ToNp(dc.strToV3(tpsrows[:, 2][i]))) objrotmat4worlda = Mat4(objrotmat4) objrotmat4worlda.setRow(3, objrotmat4.getRow3(3)+self.worlda*self.retworlda) objrotmat4worldaworldz = Mat4(objrotmat4worlda) objrotmat4worldaworldz.setRow(3, objrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.regg.add_node('mid' + str(ttgsid), fgrcenter=ttgsfgrcenternp, fgrcenterhandx = ttgsfgrcenternp_handx, fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz, fgrcenterworlda = ttgsfgrcenternp_worlda, fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid = ttgsidfreeair, #freetabletopplacementid = int(tpsrows[:,2][i]), freetabletopplacementid=int(tpsrows[:, 0][i]), tabletopplacementrotmat = objrotmat4, tabletopplacementrotmathandx = objrotmat4, tabletopplacementrotmathandxworldz = objrotmat4, tabletopplacementrotmatworlda = objrotmat4worlda, tabletopplacementrotmatworldaworldz = objrotmat4worldaworldz, angle = float(tpsrows[:,3][i]), tabletopposition = dc.strToV3(tpsrows[:,2][i])) print "str(ttgsidfreeair),str(ttgsid)",str(ttgsidfreeair),str(ttgsid) globalidsedges[str(ttgsidfreeair)].append('mid' + str(ttgsid)) localidedges.append('mid' + str(ttgsid)) for edge in list(itertools.combinations(localidedges, 2)): self.regg.add_edge(*edge, weight=1, edgetype = 'transit') #toc = time.clock() #print "(toc - tic2)", (toc - tic) if len(globalidsedges) == 0: raise ValueError("Plan tabletopgrips first!") #tic = time.clock() for globalidedgesid in globalidsedges: for edge in list(itertools.combinations(globalidsedges[globalidedgesid], 2)): self.regg.add_edge(*edge, weight=1, edgetype = 'transfer') #toc = time.clock() #print "(toc - tic3)", (toc - tic) else: print ('No placements planned!') assert('No placements planned!')
def savegoalik(self, goalrotmat4, goalid, base): ### goal # the node id of a globalgripid in goalnode, for quick setting up edges nodeidofglobalidingoal = {} # the goalnodeids is also for quick access self.goalnodeids = [] for j, rotmat in enumerate(self.freegriprotmats): # print j, len(self.freegriprotmats) ttgsrotmat = rotmat * goalrotmat4 # for collision detection, we move the object back to x=0,y=0 ttgsrotmatx0y0 = Mat4(goalrotmat4) ttgsrotmatx0y0.setCell(3, 0, 0) ttgsrotmatx0y0.setCell(3, 1, 0) ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0 # check if the hand collide with tabletop tmphnd = self.robothand initmat = tmphnd.getMat() initjawwidth = tmphnd.jawwidth tmphnd.setJawwidth(self.freegripjawwidth[j]) tmphnd.setMat(ttgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): ttgscct0 = goalrotmat4.xformPoint(self.freegripcontacts[j][0]) ttgscct1 = goalrotmat4.xformPoint(self.freegripcontacts[j][1]) ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx * self.rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz * self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda * self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + self.worldz * self.retworldz ttgsjawwidth = self.freegripjawwidth[j] ttgsidfreeair = self.freegripid[j] ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp( ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp( ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) # print "solving goal iks" ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np) ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np) ikcxz = self.robot.numikr(ttgsfgrcenternp_handxworldz, ttgsrotmat3np) ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np) ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np) feasibility = {} feasibility_handx = {} feasibility_handxworldz = {} feasibility_worlda = {} feasibility_worldaworldz = {} if ikc is not None: feasibility[j] = 'True' else: feasibility[j] = 'False' if ikcx is not None: feasibility_handx[j] = 'True' else: feasibility_handx[j] = 'False' if ikcxz is not None: feasibility_handxworldz[j] = 'True' else: feasibility_handxworldz[j] = 'False' if ikca is not None: feasibility_worlda[j] = 'True' else: feasibility_worlda[j] = 'False' if ikcaz is not None: feasibility_worldaworldz[j] = 'True' else: feasibility_worldaworldz[j] = 'False' sql = "INSERT IGNORE INTO ik_goal(idgoal,idfreeairgrip, feasibility, feasibility_handx, feasibility_handxworldz, \ feasibility_worlda, feasibility_worldaworldz) VALUES (%d,%d, '%s', '%s', '%s', '%s', '%s')" % \ (goalid, j, feasibility[j], feasibility_handx[j], \ feasibility_handxworldz[j], \ feasibility_worlda[j], feasibility_worldaworldz[j]) self.gdb.execute(sql)
def updateDBwithIK(self, robot): """ compute the IK feasible grasps of each pose :return: """ # load retraction distances rethandx, retworldz, retworlda, worldz = self.gdb.loadIKRet() # load idrobot idrobot = self.gdb.loadIdRobot(robot) self.floatinggripikfeas_rgt = [] self.floatinggripikfeas_handx_rgt = [] self.floatinggripikfeas_lft = [] self.floatinggripikfeas_handx_lft = [] tic = time.clock() for fpid in range(len(self.gridsfloatingposemat4s)): toc = time.clock() print toc-tic if fpid != 0: print "remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic) print fpid, len(self.gridsfloatingposemat4s) ### right hand armname = 'rgt' feasibility = [] feasibility_handx = [] for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]): feasibility.append('False') feasibility_handx.append('False') fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2 fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter) fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3()) handx = hndrotmat4.getRow3(0) # check the angle between handx and minus y minusworldy = Vec3(0,-1,0) if Vec3(handx).angleDeg(minusworldy) < 60: fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx) if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None: feasibility[i] = 'True' if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None: feasibility_handx[i] = 'True' self.floatinggripikfeas_rgt.append(feasibility) self.floatinggripikfeas_handx_rgt.append(feasibility_handx) ### left hand armname = 'lft' feasibility = [] feasibility_handx = [] for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]): feasibility.append('False') feasibility_handx.append('False') fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2 fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter) fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3()) handx = hndrotmat4.getRow3(0) # check the angle between handx and minus y plusworldy = Vec3(0,1,0) if Vec3(handx).angleDeg(plusworldy) < 60: fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx) if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None: feasibility[i] = 'True' if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None: feasibility_handx[i] = 'True' self.floatinggripikfeas_lft.append(feasibility) self.floatinggripikfeas_handx_lft.append(feasibility_handx) self.saveIKtoDB(idrobot)
def addEnds(self, rotmat4, armname): # the node id of a globalgripid in end nodeidofglobalidinend = {} # the endnodeids is also for quick access self.endnodeids[armname] = [] for j, rotmat in enumerate(self.freegriprotmats): assgsrotmat = rotmat * rotmat4 # for collision detection, we move the object back to x=0,y=0 assgsrotmatx0y0 = Mat4(rotmat4) assgsrotmatx0y0.setCell(3,0,0) assgsrotmatx0y0.setCell(3,1,0) assgsrotmatx0y0 = rotmat * assgsrotmatx0y0 # check if the hand collide with tabletop tmphand = self.hand initmat = tmphand.getMat() initjawwidth = tmphand.jawwidth # set jawwidth to 80 to avoid collision with surrounding obstacles # set to gripping with is unnecessary # tmphand.setJawwidth(self.freegripjawwidth[j]) tmphand.setJawwidth(tmphand.jawwidthopen) tmphand.setMat(assgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): assgscct0=rotmat4.xformPoint(self.freegripcontacts[j][0]) assgscct1=rotmat4.xformPoint(self.freegripcontacts[j][1]) assgsfgrcenter = (assgscct0+assgscct1)/2 handx = assgsrotmat.getRow3(0) assgsfgrcenterhandx = assgsfgrcenter + handx*self.rethandx # handxworldz is not necessary for start # assgsfgrcenterhandxworldz = assgsfgrcenterhandx + self.worldz*self.retworldz assgsfgrcenterworlda = assgsfgrcenter + self.worlda*self.retworlda assgsfgrcenterworldaworldz = assgsfgrcenterworlda+ self.worldz*self.retworldz assgsjawwidth = self.freegripjawwidth[j] assgsidfreeair = self.freegripid[j] assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter) assgsfgrcenternp_handx = pg.v3ToNp(assgsfgrcenterhandx) # handxworldz is not necessary for start # assgsfgrcenternp_handxworldz = pg.v3ToNp(assgsfgrcenterhandxworldz) assgsfgrcenternp_worlda = pg.v3ToNp(assgsfgrcenterworlda) assgsfgrcenternp_worldaworldz = pg.v3ToNp(assgsfgrcenterworldaworldz) assgsrotmat3np = pg.mat3ToNp(assgsrotmat.getUpper3()) ikc = self.robot.numikr(assgsfgrcenternp, assgsrotmat3np) ikcx = self.robot.numikr(assgsfgrcenternp_handx, assgsrotmat3np) ikca = self.robot.numikr(assgsfgrcenternp_worlda, assgsrotmat3np) ikcaz = self.robot.numikr(assgsfgrcenternp_worldaworldz, assgsrotmat3np) if (ikc is not None) and (ikcx is not None) and (ikca is not None) and (ikcaz is not None): # note the tabletopposition here is not the contact for the intermediate states # it is the zero pos tabletopposition = rotmat4.getRow3(3) startrotmat4worlda = Mat4(rotmat4) startrotmat4worlda.setRow(3, rotmat4.getRow3(3)+self.worlda*self.retworlda) startrotmat4worldaworldz = Mat4(startrotmat4worlda) startrotmat4worldaworldz.setRow(3, startrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.graphtpp.regg.add_node('end'+armname+str(j), fgrcenter=assgsfgrcenternp, fgrcenterhandx = assgsfgrcenternp_handx, fgrcenterhandxworldz = 'na', fgrcenterworlda = assgsfgrcenternp_worlda, fgrcenterworldaworldz = assgsfgrcenternp_worldaworldz, jawwidth=assgsjawwidth, hndrotmat3np=assgsrotmat3np, globalgripid = assgsidfreeair, freetabletopplacementid = 'na', tabletopplacementrotmat = rotmat4, tabletopplacementrotmathandx = rotmat4, tabletopplacementrotmathandxworldz = 'na', tabletopplacementrotmatworlda = startrotmat4worlda, tabletopplacementrotmatworldaworldz = startrotmat4worldaworldz, angle = 'na', tabletopposition = tabletopposition) nodeidofglobalidinend[assgsidfreeair]='start'+str(j) self.endnodeids[armname].append('start'+str(j)) tmphand.setMat(initmat) tmphand.setJawwidth(initjawwidth) if len(self.endnodeids[armname]) == 0: raise ValueError("No available end grip at " + armname) # add start transit edge for edge in list(itertools.combinations(self.endnodeids[armname], 2)): self.graphtpp.regg.add_edge(*edge, weight = 1, edgetype = 'endtransit') # add start transfer edge for reggnode, reggnodedata in self.graphtpp.regg.nodes(data=True): if reggnode.startswith(self.armname): globalgripid = reggnodedata['globalgripid'] if globalgripid in nodeidofglobalidinend.keys(): endnodeid = nodeidofglobalidinend[globalgripid] self.graphtpp.regg.add_edge(endnodeid, reggnode, weight=1, edgetype = 'endtransfer') for nid in self.graphtpp.regg.nodes(): if nid.startswith('end'): ggid = self.graphtpp.regg.node[nid]['globalgripid'] tabletopposition = self.graphtpp.regg.node[nid]['tabletopposition'] xyzpos = map(add, self.__xyzglobalgrippos_startgoal[ggid], [tabletopposition[0], tabletopposition[1], tabletopposition[2]]) self.gnodesplotpos[nid] = xyzpos[:2]
def __loadGripsToBuildGraph(self, armname = "rgt"): """ load tabletopgrips retraction distance are also loaded from database :param robot: an robot defined in robotsim.hrp5 or robotsim.nextage :param gdb: an object of the database.GraspDB class :param idarm: value = 1 "lft" or 2 "rgt", which arm to use :return: author: weiwei date: 20170112 """ # load idarm idarm = gdb.loadIdArm(armname) # get the global grip ids # and prepare the global edges # for each globalgripid, find all its tabletopids (pertaining to placements) globalidsedges = {} sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \ object.name LIKE '%s'" % self.dbobjname result = self.gdb.execute(sql) if len(result) == 0: raise ValueError("Plan freeairgrip first!") for ggid in result: globalidsedges[str(ggid[0])] = [] self.globalgripids.append(ggid[0]) sql = "SELECT tabletopplacements.idtabletopplacements, angle.value, \ tabletopplacements.idfreetabletopplacement, tabletopplacements.tabletopposition, \ tabletopplacements.rotmat FROM \ tabletopplacements,freetabletopplacement,angle,object WHERE \ tabletopplacements.idangle=angle.idangle AND \ tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \ freetabletopplacement.idobject=object.idobject AND \ object.name LIKE '%s' AND angle.value IN (0.0, 45.0, 90.0, 135.0, 180.0, 225.0, 270.0, 315.0)" \ % self.dbobjname result = self.gdb.execute(sql) if len(result) != 0: tpsrows = np.array(result) # nubmer of discreted rotation self.angles = list(set(map(float, tpsrows[:,1]))) # for plotting self.fttpsids = list(set(map(int, tpsrows[:,2]))) self.nfttps = len(self.fttpsids) idrobot = self.gdb.loadIdRobot(self.robot) for i, idtps in enumerate(tpsrows[:,0]): sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \ tabletopgrips.rotmat, tabletopgrips.jawwidth, tabletopgrips.idfreeairgrip \ FROM tabletopgrips,ik,freeairgrip,hand WHERE tabletopgrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \ freeairgrip.idhand = hand.idhand AND\ tabletopgrips.idtabletopgrips=ik.idtabletopgrips AND \ tabletopgrips.idtabletopplacements = %d AND ik.idrobot=%d AND \ ik.feasibility='True' AND ik.feasibility_handx='True' AND ik.feasibility_handxworldz='True' \ AND ik.feasibility_worlda='True' AND ik.feasibility_worldaworldz='True' AND ik.idarm = %d \ AND hand.name LIKE '%s'" \ % (int(idtps), idrobot, idarm, self.handpkg.getHandName()) resultttgs = self.gdb.execute(sql) if len(resultttgs)==0: continue localidedges = [] for ttgsrow in resultttgs: ttgsid = int(ttgsrow[0]) ttgscct0 = dc.strToV3(ttgsrow[1]) ttgscct1 = dc.strToV3(ttgsrow[2]) ttgsrotmat = dc.strToMat4(ttgsrow[3]) ttgsjawwidth = float(ttgsrow[4]) ttgsidfreeair = int(ttgsrow[5]) ttgsfgrcenter = (ttgscct0+ttgscct1)/2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) objrotmat4 = dc.strToMat4(tpsrows[:,4][i]) objrotmat4worlda = Mat4(objrotmat4) objrotmat4worlda.setRow(3, objrotmat4.getRow3(3)+self.worlda*self.retworlda) objrotmat4worldaworldz = Mat4(objrotmat4worlda) objrotmat4worldaworldz.setRow(3, objrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.regg.add_node(armname+str(ttgsid), fgrcenter=ttgsfgrcenternp, fgrcenterhandx = ttgsfgrcenternp_handx, fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz, fgrcenterworlda = ttgsfgrcenternp_worlda, fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid = ttgsidfreeair, freetabletopplacementid = int(tpsrows[:,2][i]), tabletopplacementrotmat = objrotmat4, tabletopplacementrotmathandx = objrotmat4, tabletopplacementrotmathandxworldz = objrotmat4, tabletopplacementrotmatworlda = objrotmat4worlda, tabletopplacementrotmatworldaworldz = objrotmat4worldaworldz, angle = float(tpsrows[:,1][i]), tabletopposition = dc.strToV3(tpsrows[:,3][i])) globalidsedges[str(ttgsidfreeair)].append(armname+str(ttgsid)) localidedges.append(armname+str(ttgsid)) # print list(itertools.combinations(ttgrows[:,0], 2)) for edge in list(itertools.combinations(localidedges, 2)): self.regg.add_edge(*edge, weight=1, edgetype = 'transit') if len(globalidsedges) == 0: raise ValueError("Plan tabletopgrips first!") for globalidedgesid in globalidsedges: for edge in list(itertools.combinations(globalidsedges[globalidedgesid], 2)): self.regg.add_edge(*edge, weight=1, edgetype = 'transfer') # gen plot pos # biggest circle: grips; big circle: rotation; small circle: placements radiusplacement = 30 radiusrot = 6 radiusgrip = 1 xyplacementspos = {} xydiscreterotspos = {} xyzglobalgrippos = {} for i, ttpsid in enumerate(self.fttpsids): xydiscreterotspos[ttpsid] = {} xyzglobalgrippos[ttpsid] = {} xypos = [radiusplacement * math.cos(2 * math.pi / self.nfttps * i), radiusplacement * math.sin(2 * math.pi / self.nfttps * i)] xyplacementspos[ttpsid] = xypos for j, anglevalue in enumerate(self.angles): xyzglobalgrippos[ttpsid][anglevalue] = {} xypos = [radiusrot * math.cos(anglevalue), radiusrot * math.sin(anglevalue)] xydiscreterotspos[ttpsid][anglevalue] = \ [xyplacementspos[ttpsid][0] + xypos[0], xyplacementspos[ttpsid][1] + xypos[1]] for k, globalgripid in enumerate(self.globalgripids): xypos = [radiusgrip * math.cos(2 * math.pi / len(self.globalgripids) * k), radiusgrip * math.sin(2 * math.pi / len(self.globalgripids) * k)] xyzglobalgrippos[ttpsid][anglevalue][globalgripid] = \ [xydiscreterotspos[ttpsid][anglevalue][0] + xypos[0], xydiscreterotspos[ttpsid][anglevalue][1] + xypos[1], 0] for nid in self.regg.nodes(): fttpid = self.regg.node[nid]['freetabletopplacementid'] anglevalue = self.regg.node[nid]['angle'] ggid = self.regg.node[nid]['globalgripid'] tabletopposition = self.regg.node[nid]['tabletopposition'] xyzpos = map(add, xyzglobalgrippos[fttpid][anglevalue][ggid], [tabletopposition[0], tabletopposition[1], tabletopposition[2]]) self.gnodesplotpos[nid] = xyzpos[:2]
def updateDBwithIK(self, gdb, robot, armname = 'rgt'): """ :param gdb: :param robot: :param armname: the name of the arm used rgt or lft :return: author: weiwei date: 20170111 """ # load idarm idarm = gdb.loadIdArm(armname) # load retraction distances rethanda, retworlda, worlda = gdb.loadIKRet() # select idrobot idrobot = gdb.loadIdRobot(robot) feasibility = {} feasibility_handa = {} feasibility_worlda = {} jnts = {} jnts_handa = {} jnts_worlda = {} isttg, ttgresult = self.__getTtg(gdb) if not isttg: raise ValueError("Plan the tabletopgrips first!") else: for resultrow in ttgresult: ttgsid = int(resultrow[0]) sql = "SELECT * FROM iktabletopgrips WHERE iktabletopgrips.idrobot = %d AND idarm = %d \ AND idtabletopgrips = %d" % (idrobot, idarm, ttgsid) result = gdb.execute(sql) if len(result) != 0: print("IK for the "+armname+" arm is already updated!") isredo = input("Do you want to overwrite the database? (Y/N)") if isredo != "Y" and isredo != "y": print("Updating IK is aborted.") return else: for resultrow in ttgresult: ttgsid = int(resultrow[0]) sql = "DELETE FROM iktabletopgrips WHERE iktabletopgrips.idrobot = %d AND idarm = %d \ AND idtabletopgrips = %d" % (idrobot, idarm, ttgsid) gdb.execute(sql) print("Old tabletopplacements are being deleted!") print("Old tabletopplacements have been deleted!") break idcounter = 0 tic = time.time() for resultrow in ttgresult: toc = time.time() print("Arm: ", armname, " Finished: ", "%.2f" % (idcounter*1.0/len(ttgresult)*100.0)+"%", " Time past: ", "%.2f" % (toc-tic), "s", " Expected remaining time: ", "%.2f" % ((toc-tic)/(idcounter*1.0/len(ttgresult)+1e-4)-(toc-tic)), "s") idcounter += 1 ttgsid = int(resultrow[0]) ttgscct0 = dc.strToV3(resultrow[1]) ttgscct1 = dc.strToV3(resultrow[2]) ttgsrotmat = dc.strToMat4(resultrow[3]) ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2.0 handa = -ttgsrotmat.getRow3(2) ttgsfgrcenterhanda = ttgsfgrcenter + handa*rethanda ttgsfgrcenterworlda = ttgsfgrcenter + worlda*retworlda ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handa = pg.v3ToNp(ttgsfgrcenterhanda) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) msc = robot.numik(ttgsfgrcenternp, ttgsrotmat3np, armname) if msc is not None: feasibility[ttgsid] = 'True' jnts[ttgsid] = dc.listToStr(msc) msc_handa = robot.numikmsc(ttgsfgrcenternp_handa, ttgsrotmat3np, msc, armname) if msc_handa is not None: feasibility_handa[ttgsid] = 'True' jnts_handa[ttgsid] = dc.listToStr(msc_handa) else: feasibility_handa[ttgsid] = 'False' jnts_handa[ttgsid] = dc.listToStr([]) msc_worlda = robot.numikmsc(ttgsfgrcenternp_worlda, ttgsrotmat3np, msc, armname) if msc_worlda is not None: feasibility_worlda[ttgsid] = 'True' jnts_worlda[ttgsid] = dc.listToStr(msc_worlda) else: feasibility_worlda[ttgsid] = 'False' jnts_worlda[ttgsid] = dc.listToStr([]) else: feasibility[ttgsid] = 'False' feasibility_handa[ttgsid] = 'False' feasibility_worlda[ttgsid] = 'False' jnts[ttgsid] = dc.listToStr([]) jnts_handa[ttgsid] = dc.listToStr([]) jnts_worlda[ttgsid] = dc.listToStr([]) # insert ik table sql = "INSERT INTO iktabletopgrips(idrobot, idarm, idtabletopgrips, feasibility, feasibility_handa, \ feasibility_worlda, jnts, jnts_handa, jnts_worlda) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s', '%s', '%s')" % \ (idrobot, idarm, ttgsid, feasibility[ttgsid], feasibility_handa[ttgsid], feasibility_worlda[ttgsid], \ jnts[ttgsid], jnts_handa[ttgsid], jnts_worlda[ttgsid]) gdb.execute(sql) print("IK updated!")
def __addAssNodes(self, armname = 'rgt'): iele = 0 if armname == 'lft': iele = 1 freeairidontpp = {} # for plot radiusgrip = self.regghalf[iele].radiusgrip for nid in self.regghalf[iele].graphtpp.regg.nodes(): dictind = str(self.regghalf[iele].graphtpp.regg.node[nid]['globalgripid']) if dictind in freeairidontpp: freeairidontpp[dictind].append(nid) else: freeairidontpp[dictind] = [] # add floatingposes freeairidonass = {} for asspind, assprotmat4 in enumerate(self.toass.gridsfloatingposemat4s): retass = assprotmat4.xformVec(self.retass[iele]) for pairind, hndrotmat4pair in enumerate(self.toass.icoassgrippairshndmat4s[asspind]): assgid = self.toass.icoassgrippairsids[asspind][pairind][iele] assgidfreeair = self.toass.icoassgrippairsidfreeairs[asspind][pairind][iele] ccts = self.toass.icoassgrippairscontacts[asspind][pairind][iele] hndrotmat4 = hndrotmat4pair[iele] asspgfgrcenter = (Vec3(ccts[0][0], ccts[0][1], ccts[0][2]) + Vec3(ccts[1][0], ccts[1][1], ccts[1][2])) / 2 asspgfgrcenter_retass = asspgfgrcenter + retass asspgfgrcenternp = pg.v3ToNp(asspgfgrcenter) asspgfgrcenter_retassnp = pg.v3ToNp(asspgfgrcenter_retass) jawwidth = self.toass.icoassgrippairsjawwidths[asspind][pairind][iele] hndrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3()) objrotmat4 = self.objrotmat4s[iele]*assprotmat4 objrotmat4retass = Mat4(objrotmat4) objrotmat4retass.setRow(3, objrotmat4retass.getRow3(3)+retass) self.regg.add_node('ass' + armname + str(assgid), fgrcenter=asspgfgrcenternp, fgrcenterretass=asspgfgrcenter_retassnp, jawwidth=jawwidth, hndrotmat3np=hndrotmat3np, assposerotmat4=objrotmat4, assposerotmat4retass=objrotmat4retass, assposeind=asspind, icoassgrippairsid=pairind, globalgripid=assgidfreeair) if str(assgidfreeair) in freeairidonass: freeairidonass[str(assgidfreeair)].append('ass' + armname + str(assgid)) else: freeairidonass[str(assgidfreeair)] = [] for freeairidontppkey in freeairidontpp.keys(): try: for edge in list(itertools.product(freeairidontpp[freeairidontppkey], freeairidonass[freeairidontppkey])): self.regg.add_edge(*edge, weight=1, edgetype='asstransfer') except: pass # plot pos nfp = len(self.toass.gridsfloatingposemat4s) xdist = 10 x = range(300,501,xdist) y = range(-50,50,100*xdist/nfp) for nid in self.regg.nodes(): if nid.startswith('ass'): asspind = self.regg.node[nid]['assposeind'] assgind = self.regg.node[nid]['icoassgrippairsid'] nassg = len(self.toass.icoassgrippairshndmat4s[asspind]) xpos = x[asspind % len(x)] ypos = y[asspind/len(x)] xyzpos = [radiusgrip * math.cos(2 * math.pi / nassg * assgind)+xpos, radiusgrip * math.sin(2 * math.pi / nassg * assgind)+ypos, 0] self.gnodesplotpos[nid] = xyzpos[:2] if nid.startswith('assrgt'): self.gnodesplotpos[nid][1] = self.gnodesplotpos[nid][1] - 100 elif nid.startswith('asslft'): self.gnodesplotpos[nid][1] = self.gnodesplotpos[nid][1] + 100
def __addstartgoal(self, startrotmat4, goalrotmat4, base): """ add start and goal for the regg :param startrotmat4 and goalrotmat4: both are 4by4 panda3d matrix :return: author: weiwei date: 20161216, sapporo """ ### start # the node id of a globalgripid in startnode nodeidofglobalidinstart= {} # the startnodeids is also for quick access self.startnodeids = [] for j, rotmat in enumerate(self.freegriprotmats): print j, len(self.freegriprotmats) # print rotmat ttgsrotmat = rotmat * startrotmat4 # for collision detection, we move the object back to x=0,y=0 ttgsrotmatx0y0 = Mat4(startrotmat4) ttgsrotmatx0y0.setCell(3,0,0) ttgsrotmatx0y0.setCell(3,1,0) ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0 # check if the hand collide with tabletop # tmphnd = self.robothand tmphnd = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) initmat = tmphnd.getMat() initjawwidth = tmphnd.jawwidth # set jawwidth to 80 to avoid collision with surrounding obstacles # set to gripping with is unnecessary tmphnd.setJawwidth(80) tmphnd.setMat(ttgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp) result = self.bulletworld.contactTest(hndbullnode) # tmphnd.setMat(ttgsrotmat) # tmphnd.reparentTo(base.render) # if j > 3: # base.run() if not result.getNumContacts(): ttgscct0=startrotmat4.xformPoint(self.freegripcontacts[j][0]) ttgscct1=startrotmat4.xformPoint(self.freegripcontacts[j][1]) ttgsfgrcenter = (ttgscct0+ttgscct1)/2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx # handxworldz is not necessary for start # ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz ttgsjawwidth = self.freegripjawwidth[j] ttgsidfreeair = self.freegripid[j] ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) # handxworldz is not necessary for start # ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) print "solving starting iks" ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np) ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np) ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np) ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np) if (ikc is not None) and (ikcx is not None) and (ikca is not None) and (ikcaz is not None): # note the tabletopposition here is not the contact for the intermediate states # it is the zero pos tabletopposition = startrotmat4.getRow3(3) startrotmat4worlda = Mat4(startrotmat4) startrotmat4worlda.setRow(3, startrotmat4.getRow3(3)+self.worlda*self.retworlda) startrotmat4worldaworldz = Mat4(startrotmat4worlda) startrotmat4worldaworldz.setRow(3, startrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.regg.add_node('start'+str(j), fgrcenter=ttgsfgrcenternp, fgrcenterhandx = ttgsfgrcenternp_handx, fgrcenterhandxworldz = 'na', fgrcenterworlda = ttgsfgrcenternp_worlda, fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid = ttgsidfreeair, freetabletopplacementid = 'na', tabletopplacementrotmat = startrotmat4, tabletopplacementrotmathandx = startrotmat4, tabletopplacementrotmathandxworldz = 'na', tabletopplacementrotmatworlda = startrotmat4worlda, tabletopplacementrotmatworldaworldz = startrotmat4worldaworldz, angle = 'na', tabletopposition = tabletopposition) nodeidofglobalidinstart[ttgsidfreeair]='start'+str(j) self.startnodeids.append('start'+str(j)) # tmprtq85.reparentTo(base.render) tmphnd.setMat(initmat) tmphnd.setJawwidth(initjawwidth) if len(self.startnodeids) == 0: raise ValueError("No available starting grip!") # add start transit edge for edge in list(itertools.combinations(self.startnodeids, 2)): self.regg.add_edge(*edge, weight = 1, edgetype = 'starttransit') # add start transfer edge for reggnode, reggnodedata in self.regg.nodes(data=True): if reggnode.startswith('mid'): globalgripid = reggnodedata['globalgripid'] if globalgripid in nodeidofglobalidinstart.keys(): startnodeid = nodeidofglobalidinstart[globalgripid] self.regg.add_edge(startnodeid, reggnode, weight=1, edgetype = 'starttransfer') ### goal # the node id of a globalgripid in goalnode, for quick setting up edges nodeidofglobalidingoal= {} # the goalnodeids is also for quick access self.goalnodeids = [] for j, rotmat in enumerate(self.freegriprotmats): print j, len(self.freegriprotmats) ttgsrotmat = rotmat * goalrotmat4 # for collision detection, we move the object back to x=0,y=0 ttgsrotmatx0y0 = Mat4(goalrotmat4) ttgsrotmatx0y0.setCell(3,0,0) ttgsrotmatx0y0.setCell(3,1,0) ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0 # check if the hand collide with tabletop tmphnd = self.robothand initmat = tmphnd.getMat() initjawwidth = tmphnd.jawwidth tmphnd.setJawwidth(self.freegripjawwidth[j]) tmphnd.setMat(ttgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): ttgscct0=goalrotmat4.xformPoint(self.freegripcontacts[j][0]) ttgscct1=goalrotmat4.xformPoint(self.freegripcontacts[j][1]) ttgsfgrcenter = (ttgscct0+ttgscct1)/2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz ttgsjawwidth = self.freegripjawwidth[j] ttgsidfreeair = self.freegripid[j] ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) print "solving goal iks" ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np) ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np) ikcxz = self.robot.numikr(ttgsfgrcenternp_handxworldz, ttgsrotmat3np) ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np) ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np) if (ikc is not None) and (ikcx is not None) and (ikcxz is not None) \ and (ikca is not None) and (ikcaz is not None): # note the tabletopposition here is not the contact for the intermediate states # it is the zero pos tabletopposition = goalrotmat4.getRow3(3) goalrotmat4worlda = Mat4(goalrotmat4) goalrotmat4worlda.setRow(3, goalrotmat4.getRow3(3)+self.worlda*self.retworlda) goalrotmat4worldaworldz = Mat4(goalrotmat4worlda) goalrotmat4worldaworldz.setRow(3, goalrotmat4worlda.getRow3(3)+self.worldz*self.retworldz) self.regg.add_node('goal'+str(j), fgrcenter=ttgsfgrcenternp, fgrcenterhandx = ttgsfgrcenternp_handx, fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz, fgrcenterworlda = ttgsfgrcenternp_worlda, fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz, jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np, globalgripid = ttgsidfreeair, freetabletopplacementid = 'na', tabletopplacementrotmat = goalrotmat4, tabletopplacementrotmathandx = goalrotmat4, tabletopplacementrotmathandxworldz = goalrotmat4, tabletopplacementrotmatworlda = goalrotmat4worlda, tabletopplacementrotmatworldaworldz = goalrotmat4worldaworldz, angleid = 'na', tabletopposition = tabletopposition) nodeidofglobalidingoal[ttgsidfreeair]='goal'+str(j) self.goalnodeids.append('goal'+str(j)) tmphnd.setMat(initmat) tmphnd.setJawwidth(initjawwidth) if len(self.goalnodeids) == 0: raise ValueError("No available goal grip!") # add goal transit edges for edge in list(itertools.combinations(self.goalnodeids, 2)): self.regg.add_edge(*edge, weight = 1, edgetype = 'goaltransit') # add goal transfer edge for reggnode, reggnodedata in self.regg.nodes(data=True): if reggnode.startswith('mid'): globalgripid = reggnodedata['globalgripid'] if globalgripid in nodeidofglobalidingoal.keys(): goalnodeid = nodeidofglobalidingoal[globalgripid] self.regg.add_edge(goalnodeid, reggnode, weight=1, edgetype = 'goaltransfer') # add start to goal direct edges for startnodeid in self.startnodeids: for goalnodeid in self.goalnodeids: # startnodeggid = start node global grip id startnodeggid = self.regg.node[startnodeid]['globalgripid'] goalnodeggid = self.regg.node[goalnodeid]['globalgripid'] if startnodeggid == goalnodeggid: self.regg.add_edge(startnodeid, goalnodeid, weight=1, edgetype = 'startgoaltransfer')
def updateDBwithIK(self, gdb, robot, armname = 'rgt'): """ :param gdb: :param robot: :param armname: the name of the arm used rgt or lft :param rethandx: the distance of retract along handx direction, default 50mm :param retworldz: the distance of retract along worldz direction, default 50mm :param retworlda: the distance of retract along assembly/approaching direction in the world, default 50mm :return: author: weiwei date: 20170111 """ # load idarm idarm = gdb.loadIdArm(armname) # load retraction distances rethandx, retworldz, retworlda, worldz = gdb.loadIKRet() # worlda is default for the general grasps on table top # for assembly at start and goal, worlda is computed by assembly planner worlda = Vec3(0,0,1) # select idrobot idrobot = gdb.loadIdRobot(robot) feasibility = {} feasibility_handx = {} feasibility_handxworldz = {} feasibility_worlda = {} feasibility_worldaworldz = {} sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \ tabletopgrips.rotmat FROM tabletopgrips, tabletopplacements, freetabletopplacement, object WHERE \ tabletopgrips.idtabletopplacements = tabletopplacements.idtabletopplacements AND \ tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \ freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: raise ValueError("Plan the tabletopgrips first!") idcounter = 0 tic = time.clock() for resultrow in result: print idcounter*1.0/len(result) idcounter += 1 toc = time.clock() print toc-tic ttgsid = int(resultrow[0]) ttgscct0 = dc.strToV3(resultrow[1]) ttgscct1 = dc.strToV3(resultrow[2]) ttgsrotmat = dc.strToMat4(resultrow[3]) ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx*rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + worldz*retworldz ttgsfgrcenterworlda = ttgsfgrcenter + worlda*retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ worldz*retworldz ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) if robot.numik(ttgsfgrcenternp, ttgsrotmat3np, armname) is not None: feasibility[ttgsid] = 'True' else: feasibility[ttgsid] = 'False' if robot.numik(ttgsfgrcenternp_handx, ttgsrotmat3np, armname) is not None: feasibility_handx[ttgsid] = 'True' else: feasibility_handx[ttgsid] = 'False' if robot.numik(ttgsfgrcenternp_handxworldz, ttgsrotmat3np, armname) is not None: feasibility_handxworldz[ttgsid] = 'True' else: feasibility_handxworldz[ttgsid] = 'False' if robot.numik(ttgsfgrcenternp_worlda, ttgsrotmat3np, armname) is not None: feasibility_worlda[ttgsid] = 'True' else: feasibility_worlda[ttgsid] = 'False' if robot.numik(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np, armname) is not None: feasibility_worldaworldz[ttgsid] = 'True' else: feasibility_worldaworldz[ttgsid] = 'False' # insert ik table sql = "INSERT IGNORE INTO ik(idrobot, idarm, idtabletopgrips, feasibility, feasibility_handx, feasibility_handxworldz, \ feasibility_worlda, feasibility_worldaworldz) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s', '%s')" % \ (idrobot, idarm, ttgsid, feasibility[ttgsid], feasibility_handx[ttgsid], feasibility_handxworldz[ttgsid], \ feasibility_worlda[ttgsid], feasibility_worldaworldz[ttgsid]) gdb.execute(sql)
def __addAssNodes(self, armname='rgt'): iele = 0 if armname == 'lft': iele = 1 freeairidontpp = {} # for plot radiusgrip = self.regghalf[iele].radiusgrip for nid in self.regghalf[iele].graphtpp.regg.nodes(): dictind = str( self.regghalf[iele].graphtpp.regg.node[nid]['globalgripid']) if dictind in freeairidontpp: freeairidontpp[dictind].append(nid) else: freeairidontpp[dictind] = [] # add floatingposes freeairidonass = {} for asspind, assprotmat4 in enumerate( self.toass.gridsfloatingposemat4s): retass = assprotmat4.xformVec(self.retass[iele]) for pairind, hndrotmat4pair in enumerate( self.toass.icoassgrippairshndmat4s[asspind]): assgid = self.toass.icoassgrippairsids[asspind][pairind][iele] assgidfreeair = self.toass.icoassgrippairsidfreeairs[asspind][ pairind][iele] ccts = self.toass.icoassgrippairscontacts[asspind][pairind][ iele] hndrotmat4 = hndrotmat4pair[iele] asspgfgrcenter = (Vec3(ccts[0][0], ccts[0][1], ccts[0][2]) + Vec3(ccts[1][0], ccts[1][1], ccts[1][2])) / 2 asspgfgrcenter_retass = asspgfgrcenter + retass asspgfgrcenternp = pg.v3ToNp(asspgfgrcenter) asspgfgrcenter_retassnp = pg.v3ToNp(asspgfgrcenter_retass) jawwidth = self.toass.icoassgrippairsjawwidths[asspind][ pairind][iele] hndrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3()) objrotmat4 = self.objrotmat4s[iele] * assprotmat4 objrotmat4retass = Mat4(objrotmat4) objrotmat4retass.setRow(3, objrotmat4retass.getRow3(3) + retass) self.regg.add_node('ass' + armname + str(assgid), fgrcenter=asspgfgrcenternp, fgrcenterretass=asspgfgrcenter_retassnp, jawwidth=jawwidth, hndrotmat3np=hndrotmat3np, assposerotmat4=objrotmat4, assposerotmat4retass=objrotmat4retass, assposeind=asspind, icoassgrippairsid=pairind, globalgripid=assgidfreeair) if str(assgidfreeair) in freeairidonass: freeairidonass[str(assgidfreeair)].append('ass' + armname + str(assgid)) else: freeairidonass[str(assgidfreeair)] = [] for freeairidontppkey in freeairidontpp.keys(): try: for edge in list( itertools.product(freeairidontpp[freeairidontppkey], freeairidonass[freeairidontppkey])): self.regg.add_edge(*edge, weight=1, edgetype='asstransfer') except: pass # plot pos nfp = len(self.toass.gridsfloatingposemat4s) xdist = 10 x = range(300, 501, xdist) y = range(-50, 50, 100 * xdist / nfp) for nid in self.regg.nodes(): if nid.startswith('ass'): asspind = self.regg.node[nid]['assposeind'] assgind = self.regg.node[nid]['icoassgrippairsid'] nassg = len(self.toass.icoassgrippairshndmat4s[asspind]) xpos = x[asspind % len(x)] ypos = y[asspind / len(x)] xyzpos = [ radiusgrip * math.cos(2 * math.pi / nassg * assgind) + xpos, radiusgrip * math.sin(2 * math.pi / nassg * assgind) + ypos, 0 ] self.gnodesplotpos[nid] = xyzpos[:2] if nid.startswith('assrgt'): self.gnodesplotpos[nid][ 1] = self.gnodesplotpos[nid][1] - 100 elif nid.startswith('asslft'): self.gnodesplotpos[nid][ 1] = self.gnodesplotpos[nid][1] + 100
def updateDBwithIK(self, gdb, robot, armname='rgt'): """ :param gdb: :param robot: :param armname: the name of the arm used rgt or lft :param rethandx: the distance of retract along handx direction, default 50mm :param retworldz: the distance of retract along worldz direction, default 50mm :param retworlda: the distance of retract along assembly/approaching direction in the world, default 50mm :return: author: weiwei date: 20170111 """ # load idarm idarm = gdb.loadIdArm(armname) # load retraction distances rethandx, retworldz, retworlda, worldz = gdb.loadIKRet() # worlda is default for the general grasps on table top # for assembly at start and goal, worlda is computed by assembly planner worlda = Vec3(0, 0, 1) # select idrobot idrobot = gdb.loadIdRobot(robot) feasibility = {} feasibility_handx = {} feasibility_handxworldz = {} feasibility_worlda = {} feasibility_worldaworldz = {} sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \ tabletopgrips.rotmat FROM tabletopgrips, tabletopplacements, freetabletopplacement, object WHERE \ tabletopgrips.idtabletopplacements = tabletopplacements.idtabletopplacements AND \ tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \ freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: raise ValueError("Plan the tabletopgrips first!") idcounter = 0 tic = time.clock() for resultrow in result: print idcounter * 1.0 / len(result) idcounter += 1 toc = time.clock() print toc - tic ttgsid = int(resultrow[0]) ttgscct0 = dc.strToV3(resultrow[1]) ttgscct1 = dc.strToV3(resultrow[2]) ttgsrotmat = dc.strToMat4(resultrow[3]) ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx * rethandx ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + worldz * retworldz ttgsfgrcenterworlda = ttgsfgrcenter + worlda * retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + worldz * retworldz ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) if robot.numik(ttgsfgrcenternp, ttgsrotmat3np, armname) is not None: feasibility[ttgsid] = 'True' else: feasibility[ttgsid] = 'False' if robot.numik(ttgsfgrcenternp_handx, ttgsrotmat3np, armname) is not None: feasibility_handx[ttgsid] = 'True' else: feasibility_handx[ttgsid] = 'False' if robot.numik(ttgsfgrcenternp_handxworldz, ttgsrotmat3np, armname) is not None: feasibility_handxworldz[ttgsid] = 'True' else: feasibility_handxworldz[ttgsid] = 'False' if robot.numik(ttgsfgrcenternp_worlda, ttgsrotmat3np, armname) is not None: feasibility_worlda[ttgsid] = 'True' else: feasibility_worlda[ttgsid] = 'False' if robot.numik(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np, armname) is not None: feasibility_worldaworldz[ttgsid] = 'True' else: feasibility_worldaworldz[ttgsid] = 'False' # insert ik table sql = "INSERT IGNORE INTO ik(idrobot, idarm, idtabletopgrips, feasibility, feasibility_handx, feasibility_handxworldz, \ feasibility_worlda, feasibility_worldaworldz) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s', '%s')" % \ (idrobot, idarm, ttgsid, feasibility[ttgsid], feasibility_handx[ttgsid], feasibility_handxworldz[ttgsid], \ feasibility_worlda[ttgsid], feasibility_worldaworldz[ttgsid]) gdb.execute(sql)
def updateDBwithIK(self, robot): """ compute the IK feasible grasps of each pose :return: """ # load idrobot idrobot = self.gdb.loadIdRobot(robot) self.assgikfeas0 = [] self.assgikfeas0_retassdir = [] self.assgikfeas1= [] self.assgikfeas1_retassdir = [] tic = time.clock() for fpid, apmat in enumerate(self.gridsfloatingposemat4s): assdir1to0 = apmat.xformVec(self.assDirect1to0) assdir0to1 = apmat.xformVec(-self.assDirect1to0) toc = time.clock() print toc-tic if fpid != 0: print "remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic) print fpid, len(self.gridsfloatingposemat4s) ### right hand # rgt = 0 armname = 'rgt' assgikfeas0 = [] assgikfeas0_retassdir = [] for i, hndrotmat4 in enumerate(self.icoass0griprotmat4s[fpid]): assgikfeas0.append('False') assgikfeas0_retassdir.append('False') assgsfgrcenter = (self.icoass0gripcontacts[fpid][i][0]+self.icoass0gripcontacts[fpid][i][1])/2 assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter) assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3()) handx = hndrotmat4.getRow3(0) # check the angle between handx and minus y minusworldy = Vec3(0,-1,0) if Vec3(handx).angleDeg(minusworldy) < 60: assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir1to0) if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None: assgikfeas0[i] = 'True' if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None: assgikfeas0_retassdir[i] = 'True' self.icoass0gripsik.append(assgikfeas0) self.icoass0gripsik_retassdir.append(assgikfeas0_retassdir) ### left hand armname = 'lft' assgikfeas1 = [] assgikfeas1_retassdir = [] for i, hndrotmat4 in enumerate(self.icoass1griprotmat4s[fpid]): assgikfeas1.append('False') assgikfeas1_retassdir.append('False') assgsfgrcenter = (self.icoass1gripcontacts[fpid][i][0]+self.icoass1gripcontacts[fpid][i][1])/2 assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter) assgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3()) handx = hndrotmat4.getRow3(0) # check the angle between handx and minus y plusworldy = Vec3(0,1,0) if Vec3(handx).angleDeg(plusworldy) < 60: assgsfgrcenternp_retassdir = pg.v3ToNp(assgsfgrcenter+assdir0to1) if robot.numik(assgsfgrcenternp, assgsrotmat3np, armname) is not None: assgikfeas1[i] = 'True' if robot.numik(assgsfgrcenternp_retassdir, assgsrotmat3np, armname) is not None: assgikfeas1_retassdir[i] = 'True' self.icoass1gripsik.append(assgikfeas1) self.icoass1gripsik_retassdir.append(assgikfeas1_retassdir) self.__saveIKtoDB(idrobot)
def updateDBwithIK(self, robot): """ compute the IK feasible grasps of each pose :return: """ # load retraction distances rethandx, retworldz, retworlda, worldz = self.gdb.loadIKRet() # load idrobot idrobot = self.gdb.loadIdRobot(robot) self.floatinggripikfeas_rgt = [] self.floatinggripikfeas_handx_rgt = [] self.floatinggripikfeas_lft = [] self.floatinggripikfeas_handx_lft = [] tic = time.clock() for fpid in range(len(self.gridsfloatingposemat4s)): toc = time.clock() print(toc-tic) if fpid != 0: print("remaining time", (toc-tic)*len(self.gridsfloatingposemat4s)/fpid-(toc-tic)) print(fpid, len(self.gridsfloatingposemat4s)) ### right hand armname = 'rgt' feasibility = [] feasibility_handx = [] for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]): feasibility.append('False') feasibility_handx.append('False') fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2 fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter) fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3()) handx = hndrotmat4.getRow3(0) # check the angle between handx and minus y minusworldy = Vec3(0,-1,0) if Vec3(handx).angleDeg(minusworldy) < 60: fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx) if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None: feasibility[i] = 'True' if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None: feasibility_handx[i] = 'True' self.floatinggripikfeas_rgt.append(feasibility) self.floatinggripikfeas_handx_rgt.append(feasibility_handx) ### left hand armname = 'lft' feasibility = [] feasibility_handx = [] for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]): feasibility.append('False') feasibility_handx.append('False') fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0]+self.floatinggripcontacts[fpid][i][1])/2 fpgsfgrcenternp = pg.v3ToNp(fpgsfgrcenter) fpgsrotmat3np = pg.mat3ToNp(hndrotmat4.getUpper3()) handx = hndrotmat4.getRow3(0) # check the angle between handx and minus y plusworldy = Vec3(0,1,0) if Vec3(handx).angleDeg(plusworldy) < 60: fpgsfgrcenternp_handx = pg.v3ToNp(fpgsfgrcenter+handx*rethandx) if robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armname) is not None: feasibility[i] = 'True' if robot.numik(fpgsfgrcenternp_handx, fpgsrotmat3np, armname) is not None: feasibility_handx[i] = 'True' self.floatinggripikfeas_lft.append(feasibility) self.floatinggripikfeas_handx_lft.append(feasibility_handx) self.saveIKtoDB(idrobot)
def savestartik(self, startrotmat4, startid, base): nodeidofglobalidinstart = {} # the startnodeids is also for quick access self.startnodeids = [] for j, rotmat in enumerate(self.freegriprotmats): # print j, len(self.freegriprotmats) # # print rotmat ttgsrotmat = rotmat * startrotmat4 # for collision detection, we move the object back to x=0,y=0 ttgsrotmatx0y0 = Mat4(startrotmat4) ttgsrotmatx0y0.setCell(3, 0, 0) ttgsrotmatx0y0.setCell(3, 1, 0) ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0 # check if the hand collide with tabletop # tmphnd = self.robothand tmphnd = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) initmat = tmphnd.getMat() initjawwidth = tmphnd.jawwidth # set jawwidth to 80 to avoid collision with surrounding obstacles # set to gripping with is unnecessary tmphnd.setJawwidth(80) tmphnd.setMat(ttgsrotmatx0y0) # add hand model to bulletworld hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): ttgscct0 = startrotmat4.xformPoint(self.freegripcontacts[j][0]) ttgscct1 = startrotmat4.xformPoint(self.freegripcontacts[j][1]) ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2 handx = ttgsrotmat.getRow3(0) ttgsfgrcenterhandx = ttgsfgrcenter + handx * self.rethandx # handxworldz is not necessary for start # ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda * self.retworlda ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + self.worldz * self.retworldz ttgsjawwidth = self.freegripjawwidth[j] ttgsidfreeair = self.freegripid[j] ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter) ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx) # handxworldz is not necessary for start # ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz) ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda) ttgsfgrcenternp_worldaworldz = pg.v3ToNp( ttgsfgrcenterworldaworldz) ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3()) # print "solving starting iks" ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np) ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np) ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np) ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np) feasibility = {} feasibility_handx = {} #feasibility_handxworldz = {} feasibility_worlda = {} feasibility_worldaworldz = {} if ikc is not None: feasibility[j] = 'True' else: feasibility[j] = 'False' if ikcx is not None: feasibility_handx[j] = 'True' else: feasibility_handx[j] = 'False' if ikca is not None: feasibility_worlda[j] = 'True' else: feasibility_worlda[j] = 'False' if ikcaz is not None: feasibility_worldaworldz[j] = 'True' else: feasibility_worldaworldz[j] = 'False' sql = "INSERT IGNORE INTO ik_start(idstart,idfreeairgrip, feasibility, feasibility_handx, \ feasibility_worlda, feasibility_worldaworldz) VALUES (%d,%d, '%s', '%s', '%s', '%s')" % \ (startid, j, feasibility[j], feasibility_handx[j],\ feasibility_worlda[j], feasibility_worldaworldz[j]) self.gdb.execute(sql)