class CollisionChecker(object): """ check the collision of a robot """ def __init__(self, robotmesh): """ set up the collision checker :param robotmesh is an object of robotsim/robot.robotmesh author: weiwei date: 20170608 """ self.bulletworld = BulletWorld() self.robotmesh = robotmesh self.counter = 0 def __isSglArmCollided(self, sglbullnodes): """ check the self-collision of a single arm :param sglbullnodes: a list of bullnodes starting from arm base to end-effector :return: author: weiwei date: 20170608 """ # collisioncheck, the near three links are not examined nlink = len(sglbullnodes) for i in range(nlink): for k in range(i+3, nlink): result = self.bulletworld.contactTestPair(sglbullnodes[i], sglbullnodes[k]) if result.getNumContacts(): return True return False def __isSglArmBdyCollided(self, sglbullnodes, bdybullnodes): """ check the self-collision of a single arm :param sglbullnodes: a list of bullnodes starting from arm base to end-effector :param bdybullnodes: a list of bullnodes for robot body :return: author: weiwei date: 20170615 """ # collisioncheck, the near three links are not examined nlinkarm = len(sglbullnodes) nlinkbdy = len(bdybullnodes) for i in range(1, nlinkarm): for k in range(0, nlinkbdy): result = self.bulletworld.contactTestPair(sglbullnodes[i], bdybullnodes[k]) if result.getNumContacts(): return True return False def __isDualArmCollided(self, sglbullnodesrgt, sglbullnodeslft): """ check the self-collision of a single arm the current implementation only check hands :param sglbullnodesrgt: a list of bullnodes starting from base to end-effector :param sglbullnodeslft: a list of bullnodes starting from base to end-effector :return: author: weiwei date: 20170608 """ # collisioncheck, the near three links are not examined nlinkrgt = len(sglbullnodesrgt) nlinklft = len(sglbullnodeslft) for i in range(nlinkrgt-1, nlinkrgt): for k in range(nlinklft-1, nlinklft): result = self.bulletworld.contactTestPair(sglbullnodesrgt[i], sglbullnodeslft[k]) if result.getNumContacts(): return True return False def isSelfCollided(self, robot): """ check the collision of a single arm :param armid: 'lft' or 'rgt' :return: author: weiwei date: 20170608 """ dualmnps = self.robotmesh.genmnp_list(robot) # single arm check sglmnps = dualmnps[0] sglbullnodesrgt = [] sglbullnodeslft = [] # armbase is not examined for sglmnp in sglmnps[1:len(sglmnps)-1]: sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen') sglbullnodesrgt.append(sglbullnode) # hand is multinp sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen') sglbullnodesrgt.append(sglbullnode) if self.__isSglArmCollided(sglbullnodesrgt): print "right arm self collision!" return True else: sglmnps = dualmnps[1] # armbase is not examined for sglmnp in sglmnps[1:len(sglmnps)-1]: sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen') sglbullnodeslft.append(sglbullnode) # hand is multinp sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen') sglbullnodeslft.append(sglbullnode) if self.__isSglArmCollided(sglbullnodeslft): print "left arm self collision!" return True # dual arm check if self.__isDualArmCollided(sglbullnodesrgt, sglbullnodeslft): print "left-right arm self collision!" return True # # arm body check bodymnps = dualmnps[2] bdybullnodes = [] if bodymnps: for bodymnp in bodymnps: bodybullnode = cd.genCollisionMeshMultiNp(bodymnp, basenodepath=None, name='autogen') bdybullnodes.append(bodybullnode) if self.__isSglArmBdyCollided(sglbullnodesrgt, bdybullnodes): print "right arm body self collision!" return True if self.__isSglArmBdyCollided(sglbullnodeslft, bdybullnodes): print "left right body arm self collision!" return True # for bullnode in sglbullnodesrgt[-4:]: # self.bulletworld.attachRigidBody(bullnode) # for bullnode in sglbullnodeslft: # self.bulletworld.attachRigidBody(bullnode) # for bullnode in bdybullnodes: # self.bulletworld.attachRigidBody(bullnode) return False def isCollided(self, robot, obstaclelist): """ simultaneously check self-collision and robot-obstacle collision this function should be faster than calling isselfcollided and isobstaclecollided one after another :param robot: :param obstaclelist: :return: author: weiwei date: 20170613 """ print self.counter if self.counter > 200: self.counter = 0 self.bulletworld = BulletWorld() gc.collect() self.counter += 1 dualmnps = self.robotmesh.genmnp_list(robot) sglbullnodesrgt = [] sglbullnodeslft = [] # rgt arm # single arm check sglmnps = dualmnps[0] # armbase is not examined for sglmnp in sglmnps[1:len(sglmnps)-1]: sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen') sglbullnodesrgt.append(sglbullnode) # hand is multinp sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen') sglbullnodesrgt.append(sglbullnode) # lft arm sglmnps = dualmnps[1] # armbase is not examined for sglmnp in sglmnps[1:len(sglmnps)-1]: sglbullnode = cd.genCollisionMeshNp(sglmnp, basenodepath=None, name='autogen') sglbullnodeslft.append(sglbullnode) # hand is multinp sglbullnode = cd.genCollisionMeshMultiNp(sglmnps[-1], basenodepath=None, name='autogen') sglbullnodeslft.append(sglbullnode) # arm obstacle check # only check hands nlinkrgt = len(sglbullnodesrgt) nlinklft = len(sglbullnodeslft) for obstaclemnp in obstaclelist: obstaclebullnode = cd.genCollisionMeshNp(obstaclemnp, basenodepath=None, name='autogen') for i in range(nlinkrgt - 1, nlinkrgt): result = self.bulletworld.contactTestPair(sglbullnodesrgt[i], obstaclebullnode) if result.getNumContacts(): print "rgtarm-obstacle collision!" return True for i in range(nlinklft - 1, nlinklft): result = self.bulletworld.contactTestPair(sglbullnodeslft[i], obstaclebullnode) if result.getNumContacts(): print "lftarm-obstacle collision!" return True # # # sgl arm check # if self.__isSglArmCollided(sglbullnodesrgt): # print "right arm self collision!" # return True # else: # if self.__isSglArmCollided(sglbullnodeslft): # print "left arm self collision!" # return True # # # dual arm check # if self.__isDualArmCollided(sglbullnodesrgt, sglbullnodeslft): # print "left-right arm self collision!" # return True # # # # arm body check # bodymnps = dualmnps[2] # bdybullnodes = [] # if bodymnps: # for bodymnp in bodymnps: # bodybullnode = cd.genCollisionMeshMultiNp(bodymnp, basenodepath=None, name='autogen') # bdybullnodes.append(bodybullnode) # if self.__isSglArmBdyCollided(sglbullnodesrgt, bdybullnodes): # print "right arm body self collision!" # return True # if self.__isSglArmBdyCollided(sglbullnodeslft, bdybullnodes): # print "left right body arm self collision!" # return True # for bullnode in sglbullnodesrgt: # self.bulletworld.attachRigidBody(bullnode) # for bullnode in sglbullnodeslft: # self.bulletworld.attachRigidBody(bullnode) # for bullnode in bdybullnodes: # self.bulletworld.attachRigidBody(bullnode) return False
class FloatingPoses(object): """ like freetabletopplacement and tableplacements manipulation.floatingposes corresponds to freegrip floatingposes doesn't take into account the position and orientation of the object it is "free" in position and rotation. No obstacles were considered. In contrast, each item in regrasp.floatingposes 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 "freegrip" "s" is attached to the end of "floatingposes" """ def __init__(self, objpath, gdb, handpkg, base): """ initialization :param objpath: path of the object :param base: for the loadFreeAirGrip --> genHandPairs functions (for collision detection) author: weiwei date: 20161215, tsukuba """ self.handpkg = handpkg self.handname = handpkg.getHandName() self.hand0 = handpkg.newHandNM(hndcolor=[1,0,0,.1]) self.hand1 = handpkg.newHandNM(hndcolor=[0,0,1,.1]) self.objtrimesh = trimesh.load_mesh(objpath) self.objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) self.dbobjname = os.path.splitext(os.path.basename(objpath))[0] # mat4list is nested list, floatingposemat4 is list (the flat version of mat4lsit), they are essentially the same self.mat4list = [] self.floatingposemat4 = [] # gridsfloatingposemat4s is self.floatingposemat4 translated to different grids self.gridsfloatingposemat4s = [] self.icos = None self.floatinggripids = [] self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] self.bulletworld = BulletWorld() self.handpairList = [] self.gdb = gdb self.__loadFreeAirGrip(base) # for IK self.floatinggripikfeas_rgt = [] self.floatinggripikfeas_handx_rgt = [] self.floatinggripikfeas_lft = [] self.floatinggripikfeas_handx_lft = [] # for ik-feasible pairs self.floatinggrippairsids = [] self.floatinggrippairshndmat4s = [] self.floatinggrippairscontacts = [] self.floatinggrippairsnormals = [] self.floatinggrippairsjawwidths = [] self.floatinggrippairsidfreeairs = [] self.__genPandaRotmat4() def __loadFreeAirGrip(self, base): """ load self.freegripids, etc. from mysqldatabase :param gdb: an object of the database.GraspDB class :return: author: weiwei date: 20170110 """ freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname, handname = self.handpkg.getHandName()) if freeairgripdata is None: raise ValueError("Plan the freeairgrip first!") self.freegripids = freeairgripdata[0] self.freegripcontacts = freeairgripdata[1] self.freegripnormals = freeairgripdata[2] self.freegriprotmats = freeairgripdata[3] self.freegripjawwidth = freeairgripdata[4] def __genPandaRotmat4(self, icolevel=1, angles=[0,45,90,135,180,225,270,315]): """ generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere :param icolevel, the default value 1 = 42vertices :param angles, 8 directions by default :return: a list of [pandarotmat4, ...] size of the inner list is size of the angles author: weiwei date: 20170221, tsukuba """ self.mat4list = [] self.icos = trimesh.creation.icosphere(icolevel) initmat4 = self.objnp.getMat() for vert in self.icos.vertices: self.mat4list.append([]) self.objnp.lookAt(vert[0], vert[1], vert[2]) ytozmat4 = Mat4.rotateMat(-90, self.objnp.getMat().getRow3(0)) newobjmat4 = self.objnp.getMat()*ytozmat4 for angle in angles: tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2)) tmppandamat4 = newobjmat4*tmppandamat4 self.mat4list[-1].append(tmppandamat4) self.objnp.setMat(initmat4) self.floatingposemat4 = [e for l in self.mat4list for e in l] def genHandPairs(self, base, loadser=False): self.handpairList = [] if loadser is False: # hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) # hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) pairidlist = list(itertools.combinations(range(len(self.freegripids)), 2)) print("len(pairidlist), /5000+1 : " + str(len(pairidlist)) + ", " + str(len(pairidlist)/5000+1)) for i in range(100,len(pairidlist),int(len(pairidlist)/5000+1)): # for i0, i1 in pairidlist: i0, i1 = pairidlist[i] print(i, len(pairidlist)) self.hand0.setMat(pandanpmat4 = self.freegriprotmats[i0]) self.hand0.setJawwidth(self.freegripjawwidth[i0]) self.hand1.setMat(pandanpmat4 = self.freegriprotmats[i1]) self.hand1.setJawwidth(self.freegripjawwidth[i1]) hndbullnodei0 = cd.genCollisionMeshMultiNp(self.hand0.handnp, base.render) hndbullnodei1 = cd.genCollisionMeshMultiNp(self.hand1.handnp, base.render) result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1) if not result.getNumContacts(): self.handpairList.append([self.freegripids[i0], self.freegripids[i1]]) pickle.dump(self.handpairList, open("tmp.pickle", mode="wb")) else: self.handpairList = pickle.load(open("tmp.pickle", mode="rb")) # print("self.handpairList = " + str(self.handpairList)) def genFPandGs(self, grids, icolevel=1, angles=[0,45,90,135,180,225,270,315]): """ genterate floating poses and their grasps, this function leverages genPandaRotmat4 and transformGrips :param icolevel :param angles see genPandaRotmat4 :return: author: weiwei date: 20170221 """ self.gridsfloatingposemat4s = [] self.__genPandaRotmat4(icolevel, angles) for gridposition in grids: for posemat4 in self.floatingposemat4: tmpposemat4 = Mat4(posemat4) tmpposemat4.setRow(3, Vec3(gridposition[0], gridposition[1], gridposition[2])) self.gridsfloatingposemat4s.append(tmpposemat4) self.transformGrips() 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 loadFromDB(self): """ load the floatingposes and their grasps from the database :return: author: weiwei date: 20170227 """ self.gridsfloatingposemat4s = [] self.floatinggripids = [] self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \ AND object.name LIKE '%s'" % self.dbobjname result = self.gdb.execute(sql) if len(result) != 0: for resultrow in result: self.gridsfloatingposemat4s.append(dc.strToMat4(resultrow[1])) idfloatingposes = resultrow[0] sql = "SELECT floatinggrips.idfloatinggrips, floatinggrips.contactpoint0, \ floatinggrips.contactpoint1, floatinggrips.contactnormal0, \ floatinggrips.contactnormal1, floatinggrips.rotmat, floatinggrips.jawwidth, \ floatinggrips.idfreeairgrip FROM floatinggrips,freeairgrip,hand WHERE \ floatinggrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \ freeairgrip.idhand = hand.idhand AND floatinggrips.idfloatingposes = %d AND \ hand.name = '%s'" % (idfloatingposes, self.handpkg.getHandName()) result = self.gdb.execute(sql) if len(result) != 0: floatinggripids = [] floatinggripmat4s = [] floatinggripcontacts = [] floatinggripnormals = [] floatinggripjawwidths = [] floatinggripidfreeairs = [] for resultrow in result: cct0 = dc.strToV3(resultrow[1]) cct1 = dc.strToV3(resultrow[2]) cctn0 = dc.strToV3(resultrow[3]) cctn1 = dc.strToV3(resultrow[4]) floatinggripids.append(int(resultrow[0])) floatinggripmat4s.append(dc.strToMat4(resultrow[5])) floatinggripcontacts.append([cct0, cct1]) floatinggripnormals.append([cctn0, cctn1]) floatinggripjawwidths.append(float(resultrow[6])) floatinggripidfreeairs.append(int(resultrow[7])) self.floatinggripids.append(floatinggripids) self.floatinggripmat4s.append(floatinggripmat4s) self.floatinggripcontacts.append(floatinggripcontacts) self.floatinggripnormals.append(floatinggripnormals) self.floatinggripjawwidth.append(floatinggripjawwidths) self.floatinggripidfreeair.append(floatinggripidfreeairs) else: print('Plan floating grips first!') assert(False) else: assert('No object found!') def transformGrips(self): """ transform the freeair grips to all rotmat4 in self.gridsfloatingposemat4s :return: author: weiwei date: 20170221, tsukuba """ self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] for icomat4 in self.gridsfloatingposemat4s: floatinggrips = self.transformGripsOnePose(icomat4) self.floatinggripmat4s.append(floatinggrips[0]) self.floatinggripcontacts.append(floatinggrips[1]) self.floatinggripnormals.append(floatinggrips[2]) self.floatinggripjawwidth.append(floatinggrips[3]) self.floatinggripidfreeair.append(floatinggrips[4]) def transformGripsOnePose(self, rotmat4): """ transform the freeair grips to one specific rotmat4 :param rotmat4: :return: [floatinggripmat4s, floatinggripcontacts, floatinggripnormals, floatinggripjawwidths, floatinggripidfreeairs] each element in the list is also a list """ floatinggripmat4s = [] floatinggripcontacts = [] floatinggripnormals = [] floatinggripjawwidths = [] floatinggripidfreeairs = [] for i, gripmat4 in enumerate(self.freegriprotmats): floatinggripmat4 = gripmat4 * rotmat4 cct0 = rotmat4.xformPoint(self.freegripcontacts[i][0]) cct1 = rotmat4.xformPoint(self.freegripcontacts[i][1]) cctn0 = rotmat4.xformPoint(self.freegripnormals[i][0]) cctn1 = rotmat4.xformPoint(self.freegripnormals[i][1]) floatinggripjawwidth = self.freegripjawwidth[i] floatinggripidfreeair = self.freegripids[i] floatinggripmat4s.append(floatinggripmat4) floatinggripcontacts.append([cct0, cct1]) floatinggripnormals.append([cctn0, cctn1]) floatinggripjawwidths.append(floatinggripjawwidth) floatinggripidfreeairs.append(floatinggripidfreeair) return [floatinggripmat4s, floatinggripcontacts, floatinggripnormals, floatinggripjawwidths, floatinggripidfreeairs] def showIcomat4s(self, nodepath): """ show the pandamat4s generated by genPandaRotmat4 :param nodepath, where np to repreantTo, usually base.render :return: author: weiwei date: 20170221, tsukuba """ for i, icomat4list in enumerate(self.mat4list): vert = self.icos.vertices*100 spos = Vec3(vert[i][0], vert[i][1], vert[i][2]) for icomat4 in icomat4list: pg.plotAxisSelf(nodepath, spos, icomat4, length=100, thickness=3) def updateDBwithFGPairs(self, loadser = False): """ compute the floatinggrippairs using freegrippairs and save the floatinggripspairs into Database :param loadser whether use serialized data for handpairlist :return: author: weiwei date: 20170301 """ print("updateDBwithFGPairs") print("VOHER len(self.handpairList) = " + str(len(self.handpairList))) if len(self.handpairList) == 0: self.genHandPairs(base, loadser) print("NACHER len(self.handpairList) = " + str(len(self.handpairList))) tic = time.clock() for fpid in range(len(self.gridsfloatingposemat4s)): print("fpid iteration: " + str(fpid)) 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)) # gen correspondence between freeairgripid and index # indfgoffa means index of floatinggrips whose freeairgripid are xxx indfgoffa = {} print("len(self.floatinggripidfreeair[" + str(fpid) + "]) = " + str(len(self.floatinggripidfreeair[fpid]))) # Hier koennte ich auch ueber alle freegriprotmats itertieren... for i in range(len(self.floatinggripidfreeair[fpid])): # print(">> self.floatinggripidfreeair[fpid] = " + str(self.floatinggripidfreeair[fpid])) indfgoffa[self.floatinggripidfreeair[fpid][i]] = i # print("indfgoffa[" + str(self.floatinggripidfreeair[fpid][i]) + "] = " + str(indfgoffa[self.floatinggripidfreeair[fpid][i]])) # handidpair_indfg is the pairs using index of floatinggrips handidpair_indfg = [] lenHL = len(self.handpairList) countttttttt = 0 for handidpair in self.handpairList: print("Process " + str(countttttttt) + "/" + str(lenHL)) countttttttt = countttttttt + 1 # print("len(handidpair_indfg) = " + str(len(handidpair_indfg))) hidp0 = handidpair[0] print("handidpair[0] = " + str(handidpair[0])) hidp1 = handidpair[1] print("handidpair[1] = " + str(handidpair[1])) print("indfgoffa[handidpair[0]] = " + str(indfgoffa[hidp0])) print("indfgoffa[handidpair[1]] = " + str(indfgoffa[hidp1])) handidpair_indfg.append([indfgoffa[handidpair[0]], indfgoffa[handidpair[1]]]) # if handidpair_indfg[0] is right, 1 is left sql = "INSERT IGNORE INTO floatinggripspairs VALUES (%d, %d)" % \ (self.floatinggripids[fpid][handidpair_indfg[-1][0]], self.floatinggripids[fpid][handidpair_indfg[-1][1]]) self.gdb.execute(sql) # if 1 is right, 0 is left sql = "INSERT IGNORE INTO floatinggripspairs VALUES (%d, %d)" % \ (self.floatinggripids[fpid][handidpair_indfg[-1][1]], self.floatinggripids[fpid][handidpair_indfg[-1][0]]) self.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) # for i,pairs in enumerate(self.floatinggrippairsids): # print(i) # print(pairs) 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 saveIKtoDB(self, idrobot): """ saveupdated IK to DB this function is separated from updateDBwithIK for illustration :return: """ for fpid in range(len(self.gridsfloatingposemat4s)): # right arm idarm = self.gdb.loadIdArm('rgt') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): sql = "INSERT IGNORE INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \ feasibility_handx) VALUES (%d, %d, %d, '%s', '%s')" \ % (idrobot, idarm, idfloatinggrips, self.floatinggripikfeas_rgt[fpid][i], self.floatinggripikfeas_handx_rgt[fpid][i]) gdb.execute(sql) # left arm idarm = self.gdb.loadIdArm('lft') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): sql = "INSERT IGNORE INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \ feasibility_handx) VALUES (%d, %d, %d, '%s', '%s')" \ % (idrobot, idarm, idfloatinggrips, self.floatinggripikfeas_lft[fpid][i], self.floatinggripikfeas_handx_lft[fpid][i]) gdb.execute(sql) def loadIKFromDB(self, robot): """ load the feasibility of IK from db :param robot: :return: author: weiwei date: 20170301 """ idrobot = self.gdb.loadIdRobot(robot) self.floatinggripikfeas_rgt = [] self.floatinggripikfeas_handx_rgt = [] self.floatinggripikfeas_lft = [] self.floatinggripikfeas_handx_lft = [] for fpid in range(len(self.gridsfloatingposemat4s)): # right arm feasibility = [] feasibility_handx = [] idarm = self.gdb.loadIdArm('rgt') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): feasibility.append('False') feasibility_handx.append('False') sql = "SELECT feasibility, feasibility_handx FROM ikfloatinggrips WHERE \ idrobot = %d AND idarm = %d and idfloatinggrips = %d" % (idrobot, idarm, idfloatinggrips) result = self.gdb.execute(sql) if len(result) != 0: feasibility[i] = result[0][0] feasibility_handx[i] = result[0][1] self.floatinggripikfeas_rgt.append(feasibility) self.floatinggripikfeas_handx_rgt.append(feasibility_handx) # left arm feasibility = [] feasibility_handx = [] idarm = self.gdb.loadIdArm('lft') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): feasibility.append('False') feasibility_handx.append('False') sql = "SELECT feasibility, feasibility_handx FROM ikfloatinggrips WHERE \ idrobot = %d AND idarm = %d and idfloatinggrips = %d" % (idrobot, idarm, idfloatinggrips) result = self.gdb.execute(sql) if len(result) != 0: feasibility[i] = result[0][0] feasibility_handx[i] = result[0][1] self.floatinggripikfeas_lft.append(feasibility) self.floatinggripikfeas_handx_lft.append(feasibility_handx) def plotOneFPandG(self, parentnp, fpid=0): """ plot the objpose and grasps at a specific rotmat4 :param fpid: the index of self.floatingposemat4 :return: author: weiwei date: 20170221, tsukuba """ objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) objnp.setMat(self.gridsfloatingposemat4s[fpid]) objnp.reparentTo(parentnp) for i, hndrotmat4 in enumerate(self.gridsfloatingposemat4s[fpid]): if i == 7: # show grasps hand0 = self.handpkg.newHandNM(hndcolor=[0, 1, 0, 1]) hand0.setMat(pandanpmat4 = hndrotmat4) hand0.setJawwidth(self.floatinggripjawwidth[fpid][i]) hand0.reparentTo(parentnp) print(self.handpairList) for handidpair in self.handpairList: if handidpair[0] == self.floatinggripidfreeair[fpid][i]: pairedilist = [i1 for i1 in range(len(self.floatinggripidfreeair[fpid])) if self.floatinggripidfreeair[fpid][i1]==handidpair[1]] print(pairedilist) i1 = pairedilist[0] # if self.floatinggripikfeas_lft[fpid][i1] == 'True': hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1]) hndrotmat4 = self.floatinggripmat4s[fpid][i1] hand1.setMat(pandanpmat4 = hndrotmat4) hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1]) hand1.reparentTo(parentnp) if handidpair[1] == self.floatinggripidfreeair[fpid][i]: pairedilist = [i1 for i1 in range(len(self.floatinggripidfreeair[fpid])) if self.floatinggripidfreeair[fpid][i1]==handidpair[0]] print(pairedilist) i1 = pairedilist[0] # if self.floatinggripikfeas_lft[fpid][i1] == 'True': hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1]) hndrotmat4 = self.floatinggripmat4s[fpid][i1] hand1.setMat(pandanpmat4 = hndrotmat4) hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1]) hand1.reparentTo(parentnp) def plotOneFPandGPairs(self, parentnp, fpid=0): """ plot the gpairss at a specific rotmat4 :param fpid: the index of self.floatingposemat4 :return: author: weiwei date: 20170301, tsukuba """ objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) objnp.setMat(self.gridsfloatingposemat4s[fpid]) objnp.setColor(Vec4(.7,0.3,0,1)) objnp.reparentTo(parentnp) print(self.floatinggrippairshndmat4s[fpid]) for i, hndrotmat4pair in enumerate(self.floatinggrippairshndmat4s[fpid]): # if i == 9: # show grasps hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .5]) hand0mat4 = Mat4(hndrotmat4pair[0]) # h0row3 = hand0mat4.getRow3(3) # h0row3[2] = h0row3[2]+i*20.0 # hand0mat4.setRow(3, h0row3[2]) hand0.setMat(pandanpmat4 = hand0mat4) hand0.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][0]) hand0.reparentTo(parentnp) hand1 = self.handpkg.newHandNM(hndcolor=[0, .0, 1, .5]) hand1mat4 = Mat4(hndrotmat4pair[1]) # h1row3 = hand1mat4.getRow3(3) # h1row3[2] = h1row3[2]+i*20.0 # hand1mat4.setRow(3, h1row3[2]) hand1.setMat(pandanpmat4 = hand1mat4) hand1.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][1]) hand1.reparentTo(parentnp)
class BCMchecker(object): """ BBCMchecker bullet box collision model checker """ def __init__(self, toggledebug=False): self.world = BulletWorld() self._toggledebug = toggledebug if self._toggledebug: bulletcolliderrender = base.render.attachNewNode( "bulletboxcollider") debugNode = BulletDebugNode('Debug') debugNode.showWireframe(True) debugNode.showConstraints(True) debugNode.showBoundingBoxes(False) debugNode.showNormals(False) self._debugNP = bulletcolliderrender.attachNewNode(debugNode) self.world.setDebugNode(self._debugNP.node()) self.worldrigidbodylist = [] self._isupdateworldadded = False def _updateworld(self, world, task): world.doPhysics(globalClock.getDt()) return task.cont def isBoxBoxCollided(self, objcm0, objcm1): """ check if two objects objcm0 as objcm1 are in collision with each other the two objects are in the form of collision model the AABB boxlist will be used type "box" is required :param objcm0: the first object :param objcm1: the second object :return: boolean value showing if the two objects are in collision author: weiwei date: 20190313 """ objcm0boxbullnode = genBulletCDBox(objcm0) objcm1boxbullnode = genBulletCDBox(objcm1) result = self.world.contactTestPair(objcm0boxbullnode, objcm1boxbullnode) if not result.getNumContacts(): return False else: return True def isBoxBoxListCollided(self, objcm0, objcmlist=[]): """ check if objcm0 is in collision with a list of collisionmodels in objcmlist each obj is in the form of a collision model :param objcm0: :param obcmjlist: a list of collision models :return: boolean value showing if the object and the list are in collision author: weiwei date: 20190313 """ objcm0boxbullnode = genBulletCDBox(objcm0) objcm1boxbullnode = genBulletCDBoxList(objcmlist) result = self.world.contactTestPair(objcm0boxbullnode, objcm1boxbullnode) if not result.getNumContacts(): return False else: return True def isBoxListBoxListCollided(self, objcm0list=[], objcm1list=[]): """ check if a list of collisionmodels in objcm0list is in collision with a list of collisionmodels in objcm1list each obj is in the form of a collision model :param objcm0list: a list of collision models :param obcmj1list: a list of collision models :return: boolean value showing if the object and the list are in collision author: weiwei date: 20190313 """ objcm0boxbullnode = genBulletCDBoxList(objcm0list) objcm1boxbullnode = genBulletCDBoxList(objcm1list) result = self.world.contactTestPair(objcm0boxbullnode, objcm1boxbullnode) if not result.getNumContacts(): return False else: return True def showBox(self, objcm): """ show the AABB collision meshes of the given objects :param objcm author: weiwei date: 20190313 :return: """ if not self._toggledebug: print( "Toggle debug on during defining the XCMchecker object to use showfunctions!" ) return if not self._isupdateworldadded: base.taskMgr.add(self._updateworld, "updateworld", extraArgs=[self.world], appendTask=True) objcmboxbullnode = genBulletCDBox(objcm) self.world.attach(objcmboxbullnode) self.worldrigidbodylist.append(objcmboxbullnode) self._debugNP.show() def showBoxList(self, objcmlist): """ show the AABB collision meshes of the given objects :param objcm0, objcm1 author: weiwei date: 20190313 :return: """ if not self._toggledebug: print( "Toggle debug on during defining the XCMchecker object to use showfunctions!" ) return if not self._isupdateworldadded: base.taskMgr.add(self._updateworld, "updateworld", extraArgs=[self.world], appendTask=True) objcmboxbullnode = genBulletCDBoxList(objcmlist) self.world.attach(objcmboxbullnode) self.worldrigidbodylist.append(objcmboxbullnode) self._debugNP.show() def unshow(self): """ unshow everything author: weiwei date: 20180621 :return: """ base.taskMgr.remove("updateworld") print(self.worldrigidbodylist) for cdnode in self.worldrigidbodylist: self.world.remove(cdnode) self.worldrigidbodylist = [] self._debugNP.hide()
class TwoObjAss(object): """ the class uses genAvailableFAGs to compute grasps for the assembly of two objects different rotations could be saved to database """ def __init__(self, base, obj0path, obj0Mat4, obj1path, obj1Mat4, assDirect1to0, gdb, handpkg): """ initiliazation obj0 will be manipulated by rgt hand, obj1 will be manipulated by lft hand # see genAvailableFAGs function for the details of parameters :param base: :param obj0path: :param obj0Mat4: :param obj1path: :param obj1Mat4: :param assDirect1to0: the assembly direction to assemble object 1 to object 0 (with length) :param gdb: :param handpkg: """ self.base = base self.obj0path = obj0path self.obj0Mat4 = obj0Mat4 self.dbobj0name = os.path.splitext(os.path.basename(obj0path))[0] self.obj1path = obj1path self.obj1Mat4 = obj1Mat4 self.dbobj1name = os.path.splitext(os.path.basename(obj1path))[0] self.assDirect1to0 = assDirect1to0 self.gdb = gdb self.handpkg = handpkg # define the free ass0grip and ass1grip # definition: objgrips: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair] self.obj0grips = [] self.obj1grips = [] # ico means they corresponds to ico rotmats self.gridsfloatingposemat4s = [] # right grips for each gridsfloatingposemat4 self.icoass0gripids = [] self.icoass0gripcontacts = [] self.icoass0gripnormals = [] self.icoass0griprotmat4s = [] self.icoass0gripjawwidth = [] self.icoass0gripidfreeair = [] # left grips for each gridsfloatingposemat4 self.icoass1gripids = [] self.icoass1gripcontacts = [] self.icoass1gripnormals = [] self.icoass1griprotmat4s = [] self.icoass1gripjawwidth = [] self.icoass1gripidfreeair = [] # ik related self.icoass0gripsik = [] self.icoass0gripsik_retassdir = [] self.icoass1gripsik = [] self.icoass1gripsik_retassdir = [] # handpairList self.handpairList = [] self.bulletworld = BulletWorld() # ik feasible pairs for each gridsfloatingposemat4 self.icoassgrippairsids = [] self.icoassgrippairscontacts = [] self.icoassgrippairsnormals = [] self.icoassgrippairshndmat4s = [] self.icoassgrippairsjawwidths = [] self.icoassgrippairsidfreeairs = [] def genXAssPandGs(self, grids, icolevel=1, angles=[0,45,90,135,180,225,270,315]): """ genterate xass poses and their grasps, this function leverages genPandaRotmat4 and transformGrips :param icolevel :param angles see genPandaRotmat4 :return: author: weiwei date: 20170221 """ self.gridsfloatingposemat4s = [] self.__genPandaRotmat4(icolevel, angles) for gridposition in grids: for posemat4 in self.floatingposemat4: tmpposemat4 = Mat4(posemat4) tmpposemat4.setRow(3, Vec3(gridposition[0], gridposition[1], gridposition[2])) self.gridsfloatingposemat4s.append(tmpposemat4) if len(self.obj0grips) == 0 or len(self.obj1grips) == 0: self.__genFreeAssGrips() self.__transformGrips() 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) # TODO: Have to delete assemblyx when grasps are deleted # TODO: write algorithms to enable insertion of grasps using idassemblyx def loadFromDB(self): """ :return: """ self.gridsfloatingposemat4s = [] self.icoass0gripids = [] self.icoass0gripcontacts = [] self.icoass0gripnormals = [] self.icoass0griprotmat4s = [] self.icoass0gripjawwidth = [] self.icoass0gripidfreeair = [] self.icoass1gripids = [] self.icoass1gripcontacts = [] self.icoass1gripnormals = [] self.icoass1griprotmat4s = [] self.icoass1gripjawwidth = [] self.icoass1gripidfreeair = [] idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0) # check if rotation already exist sql = "SELECT assemblyx.idassemblyx, assemblyx.rotmat FROM assemblyx WHERE assemblyx.idassembly = %d" % idassembly result = self.gdb.execute(sql) if len(result) != 0: for resultrow in result: poserotmat4 = dc.strToMat4(resultrow[1]) self.gridsfloatingposemat4s.append(poserotmat4) idassemblyx = resultrow[0] # g0 sql = "SELECT * FROM assemblyxgrips0 WHERE idassemblyx = %d" % idassemblyx result = self.gdb.execute(sql) if len(result) != 0: self.icoass0gripids.append([]) self.icoass0gripcontacts.append([]) self.icoass0gripnormals.append([]) self.icoass0griprotmat4s.append([]) self.icoass0gripjawwidth.append([]) self.icoass0gripidfreeair.append([]) for resultrow in result: self.icoass0gripids[-1].append(resultrow[0]) cct0 = dc.strToV3(resultrow[1]) cct1 = dc.strToV3(resultrow[2]) cctn0 = dc.strToV3(resultrow[3]) cctn1 = dc.strToV3(resultrow[4]) self.icoass0gripcontacts[-1].append([cct0, cct1]) self.icoass0gripnormals[-1].append([cctn0, cctn1]) self.icoass0griprotmat4s[-1].append(dc.strToMat4(resultrow[5])) self.icoass0gripjawwidth[-1].append(float(resultrow[6])) self.icoass0gripidfreeair[-1].append(int(resultrow[8])) # g1 sql = "SELECT * FROM assemblyxgrips1 WHERE idassemblyx = %d" % idassemblyx result = self.gdb.execute(sql) if len(result) != 0: self.icoass1gripids.append([]) self.icoass1gripcontacts.append([]) self.icoass1gripnormals.append([]) self.icoass1griprotmat4s.append([]) self.icoass1gripjawwidth.append([]) self.icoass1gripidfreeair.append([]) for resultrow in result: self.icoass1gripids[-1].append(resultrow[0]) cct0 = dc.strToV3(resultrow[1]) cct1 = dc.strToV3(resultrow[2]) cctn0 = dc.strToV3(resultrow[3]) cctn1 = dc.strToV3(resultrow[4]) self.icoass1gripcontacts[-1].append([cct0, cct1]) self.icoass1gripnormals[-1].append([cctn0, cctn1]) self.icoass1griprotmat4s[-1].append(dc.strToMat4(resultrow[5])) self.icoass1gripjawwidth[-1].append(float(resultrow[6])) self.icoass1gripidfreeair[-1].append(int(resultrow[8])) def updateDBwithHandPairs(self): """ compute the assgrippairs using assfreegrippairs and save the assgrippairs into Database :return: author: weiwei date: 20170307 """ self.__genHandPairs(base) tic = time.time() for fpid in range(len(self.gridsfloatingposemat4s)): 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)) # gen correspondence between freeairgripid and index # indagoffa means index of assgrips whose freeairgripid are xxx indagoffa0 = {} for i in range(len(self.icoass0gripidfreeair[fpid])): indagoffa0[self.icoass0gripidfreeair[fpid][i]] = i indagoffa1 = {} for i in range(len(self.icoass1gripidfreeair[fpid])): indagoffa1[self.icoass1gripidfreeair[fpid][i]] = i # handidpair_indag is the pairs using index of floatinggrips handidpair_indag = [] for handidpair in self.handpairList: handidpair_indag.append([indagoffa0[handidpair[0]], indagoffa1[handidpair[1]]]) sql = "INSERT IGNORE INTO assemblyxgrippairs VALUES (%d, %d)" % \ (self.icoass0gripids[fpid][handidpair_indag[-1][0]], self.icoass1gripids[fpid][handidpair_indag[-1][1]]) self.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.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 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 __genHandPairs(self, base): self.handpairList = [] self.__genFreeAssGrips() ass0gripcontacts, ass0gripnormals, ass0griprotmat4s, ass0gripjawwidth, ass0gripidfreeair = self.obj0grips ass1gripcontacts, ass1gripnormals, ass1griprotmat4s, ass1gripjawwidth, ass1gripidfreeair = self.obj1grips hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) i0list = range(len(ass0gripidfreeair)) i1list = range(len(ass1gripidfreeair)) pairidlist = list(itertools.product(i0list, i1list)) print(len(pairidlist)/10000+1) for i in range(0,len(pairidlist),len(pairidlist)/10000+1): # for i0, i1 in pairidlist: i0, i1 = pairidlist[i] print(i, len(pairidlist)) hand0.setMat(ass0griprotmat4s[i0]) hand0.setJawwidth(ass0gripjawwidth[i0]) hand1.setMat(ass1griprotmat4s[i1]) hand1.setJawwidth(ass1gripjawwidth[i1]) hndbullnodei0 = cd.genCollisionMeshMultiNp(hand0.handnp, base.render) hndbullnodei1 = cd.genCollisionMeshMultiNp(hand1.handnp, base.render) result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1) if not result.getNumContacts(): self.handpairList.append([ass0gripidfreeair[i0], ass1gripidfreeair[i1]]) # print ass0gripidfreeair # print self.icoass0gripidfreeair[0] # print ass1gripidfreeair # print self.icoass1gripidfreeair[0] # print self.handpairList # assert "compare" def __saveIKtoDB(self, idrobot): """ saveupdated IK to DB this function is separated from updateDBwithIK for illustration :return: """ for fpid in range(len(self.gridsfloatingposemat4s)): # right arm idarm = self.gdb.loadIdArm('rgt') for i, idicoass0grip in enumerate(self.icoass0gripids[fpid]): sql = "INSERT IGNORE INTO ikassemblyxgrips0(idrobot, idarm, idassemblyxgrips0, feasibility, \ feasibility_assdir) VALUES (%d, %d, %d, '%s', '%s')" \ % (idrobot, idarm, idicoass0grip, self.icoass0gripsik[fpid][i], self.icoass0gripsik_retassdir[fpid][i]) gdb.execute(sql) # left arm idarm = self.gdb.loadIdArm('lft') for i, idicoass1grip in enumerate(self.icoass1gripids[fpid]): sql = "INSERT IGNORE INTO ikassemblyxgrips1(idrobot, idarm, idassemblyxgrips1, feasibility, \ feasibility_assdir) VALUES (%d, %d, %d, '%s', '%s')" \ % (idrobot, idarm, idicoass1grip, self.icoass1gripsik[fpid][i], self.icoass1gripsik_retassdir[fpid][i]) gdb.execute(sql) def __loadIKFromDB(self, robot): """ load the feasibility of IK from db :param robot: :return: author: weiwei date: 20170307 """ idrobot = self.gdb.loadIdRobot(robot) self.assgikfeas0 = [] self.assgikfeas0_retassdir = [] self.assgikfeas1= [] self.assgikfeas1_retassdir = [] for fpid in range(len(self.gridsfloatingposemat4s)): # right arm assgikfeas0 = [] assgikfeas0_retassdir = [] idarm = self.gdb.loadIdArm('rgt') for i, idassgrips in enumerate(self.icoass0gripids[fpid]): assgikfeas0.append('False') assgikfeas0_retassdir.append('False') sql = "SELECT feasibility, feasibility_assdir FROM ikassemblyxgrips0 WHERE \ idrobot = %d AND idarm = %d and idassemblyxgrips0 = %d" % (idrobot, idarm, idassgrips) result = self.gdb.execute(sql) if len(result) != 0: assgikfeas0[i] = result[0][0] assgikfeas0_retassdir[i] = result[0][1] else: assert "Compute ik of the freeassemblygrips first!" self.assgikfeas0.append(assgikfeas0) self.assgikfeas0_retassdir.append(assgikfeas0_retassdir) # left arm assgikfeas1 = [] assgikfeas1_retassdir = [] idarm = self.gdb.loadIdArm('lft') for i, idassgrips in enumerate(self.icoass1gripids[fpid]): assgikfeas1.append('False') assgikfeas1_retassdir.append('False') sql = "SELECT feasibility, feasibility_assdir FROM ikassemblyxgrips1 WHERE \ idrobot = %d AND idarm = %d and idassemblyxgrips1 = %d" % (idrobot, idarm, idassgrips) result = self.gdb.execute(sql) if len(result) != 0: assgikfeas1[i] = result[0][0] assgikfeas1_retassdir[i] = result[0][1] else: assert "Compute ik of the freeassemblygrips first!" self.assgikfeas1.append(assgikfeas1) self.assgikfeas1_retassdir.append(assgikfeas1_retassdir) def __genPandaRotmat4(self, icolevel=1, angles=[0, 45, 90, 135, 180, 225, 270, 315]): """ generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere :param icolevel, the default value 1 = 42vertices :param angles, 8 directions by default :return: a list of [pandarotmat4, ...] size of the inner list is size of the angles author: weiwei date: 20170221, tsukuba (copied from regrasp/floatingposes.py) """ objnp = pg.genObjmnp(self.obj0path) mat4list = [] self.icos = trimesh.creation.icosphere(icolevel) initmat4 = objnp.getMat() for vert in self.icos.vertices: mat4list.append([]) objnp.lookAt(vert[0], vert[1], vert[2]) ytozmat4 = Mat4.rotateMat(-90, objnp.getMat().getRow3(0)) newobjmat4 = objnp.getMat() * ytozmat4 for angle in angles: tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2)) tmppandamat4 = newobjmat4 * tmppandamat4 mat4list[-1].append(tmppandamat4) objnp.setMat(initmat4) self.floatingposemat4 = [e for l in mat4list for e in l] def __genFreeAssGrips(self): self.obj0grips = [] self.obj1grips = [] self.obj0grips, self.obj1grips = \ genAvailableFAGs(self.base, self.obj0path, self.obj0Mat4, self.obj1path, self.obj1Mat4, self.gdb, self.handpkg) def __transformGripsOnePose(self, objgrips, rotmat4): """ transform the freeair grips to one specific rotmat4 :param objgrips: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair] :param rotmat4: :return: [xassgripcontacts, xassgripnormals, xassgriprotmat4s, xassgripjawwidth, xassgripidfreeair] each element in the list is also a list author: weiwei date: 20170307 """ assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair = objgrips xassgripcontacts = [] xassgripnormals = [] xassgriprotmat4s = [] xassgripjawwidth = [] xassgripidfreeair = [] for i, gripmat4 in enumerate(assgriprotmat4s): floatinggripmat4 = gripmat4 * rotmat4 cct0 = rotmat4.xformPoint(assgripcontacts[i][0]) cct1 = rotmat4.xformPoint(assgripcontacts[i][1]) cctn0 = rotmat4.xformPoint(assgripnormals[i][0]) cctn1 = rotmat4.xformPoint(assgripnormals[i][1]) xassgripcontacts.append([cct0, cct1]) xassgripnormals.append([cctn0, cctn1]) xassgriprotmat4s.append(floatinggripmat4) xassgripjawwidth.append(assgripjawwidth[i]) xassgripidfreeair.append(assgripidfreeair[i]) return [xassgripcontacts, xassgripnormals, xassgriprotmat4s, xassgripjawwidth, xassgripidfreeair] def __transformGrips(self): """ transform the freeass grips to all rotmat4 in self.gridsfloatingposemat4s :return: author: weiwei date: 20170221, tsukuba (copied from regrasp/floatingposes.py) """ self.icoass0gripcontacts = [] self.icoass0gripnormals = [] self.icoass0griprotmat4s = [] self.icoass0gripjawwidth = [] self.icoass0gripidfreeair = [] for icomat4 in self.gridsfloatingposemat4s: xassgrips = self.__transformGripsOnePose(self.obj0grips, icomat4) self.icoass0gripcontacts.append(xassgrips[0]) self.icoass0gripnormals.append(xassgrips[1]) self.icoass0griprotmat4s.append(xassgrips[2]) self.icoass0gripjawwidth.append(xassgrips[3]) self.icoass0gripidfreeair.append(xassgrips[4]) self.icoass1gripcontacts = [] self.icoass1gripnormals = [] self.icoass1griprotmat4s = [] self.icoass1gripjawwidth = [] self.icoass1gripidfreeair = [] for icomat4 in self.gridsfloatingposemat4s: xassgrips = self.__transformGripsOnePose(self.obj1grips, icomat4) self.icoass1gripcontacts.append(xassgrips[0]) self.icoass1gripnormals.append(xassgrips[1]) self.icoass1griprotmat4s.append(xassgrips[2]) self.icoass1gripjawwidth.append(xassgrips[3]) self.icoass1gripidfreeair.append(xassgrips[4])
class GameStatePlaying(GState): VIDAS = 3 #LEVEL_TIME = 100 LEVEL_TIME = 50 def start(self): self._playing_node = render.attachNewNode("playing-node") self._playing_node2d = aspect2d.attachNewNode("playing2d-node") self.keyMap = {"left":0, "right":0, "up":0, "down":0} base.accept( "escape" , sys.exit) props = WindowProperties() props.setCursorHidden(True) base.disableMouse() props.setMouseMode(WindowProperties.MRelative) base.win.requestProperties(props) base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) self._modulos = None self._paneles = None self._num_lvl = 1 self._num_lvls = 2 self.loadLevel() self.loadGUI() self.loadBkg() self._last_t = None self._last_t_space = 0 def stop(self): self._playing_node.removeNode() self._playing_node2d.removeNode() def loadLevel(self): self._level_time = self.LEVEL_TIME self.initBullet() self._player = Player(self.world) self.loadMap() self.setAI() def initBullet(self): self.world = BulletWorld() #self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setGravity(Vec3(0, 0, -1.62)) groundShape = BulletPlaneShape(Vec3(0, 0, 1), 0) groundNode = BulletRigidBodyNode('Ground') groundNode.addShape(groundShape) self.world.attachRigidBody(groundNode) def loadBkg(self): self.environ1 = loader.loadModel("../data/models/skydome") self.environ1.reparentTo(self._playing_node) self.environ1.setPos(0,0,0) self.environ1.setScale(1) self.environ2 = loader.loadModel("../data/models/skydome") self.environ2.reparentTo(self._playing_node) self.environ2.setP(180) self.environ2.setH(270) self.environ2.setScale(1) self.dirnlight1 = DirectionalLight("dirn_light1") self.dirnlight1.setColor(Vec4(1.0,1.0,1.0,1.0)) self.dirnlightnode1 = self._playing_node.attachNewNode(self.dirnlight1) self.dirnlightnode1.setHpr(0,317,0) self._playing_node.setLight(self.dirnlightnode1) self.alight = AmbientLight('alight') self.alight.setColor(VBase4(0.05, 0.05, 0.05, 1)) self.alight_node = self._playing_node.attachNewNode(self.alight) self._playing_node.setLight(self.alight_node) self.environ1 = loader.loadModel("../data/models/ground") self.environ1.reparentTo(self._playing_node) self.environ1.setPos(0,0,0) self.environ1.setScale(1) def loadGUI(self): self.vidas_imgs = list() w = 0.24 for i in range(self.VIDAS): image_warning = OnscreenImage(image = '../data/Texture/signal_warning.png', pos=(-1 + i*w, 0, 0.85), parent=self._playing_node2d) image_warning.setScale(0.1) image_warning.setTransparency(TransparencyAttrib.MAlpha) image_warning.hide() image_ok = OnscreenImage(image = '../data/Texture/signal_ok.png', pos=(-1 + i*w, 0, 0.85), parent=self._playing_node2d) image_ok.setScale(0.1) image_ok.setTransparency(TransparencyAttrib.MAlpha) image_ok.show() self.vidas_imgs.append((image_ok, image_warning)) self._level_time_O = OnscreenText(text = '', pos = (0, 0.85), scale = 0.14, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0), parent=self._playing_node2d) def loadMap(self): if (self._modulos is not None): for m in self._modulos: m.remove() if (self._paneles is not None): for p in self._paneles: p.remove() self._tp = TiledParser("map"+str(self._num_lvl)) self._modulos, self._paneles = self._tp.load_models(self.world, self._playing_node) def setAI(self): taskMgr.add(self.update, 'Update') def update(self, task): if (task.frame > 1): self.world.doPhysics(globalClock.getDt()) self._level_time -= globalClock.getDt() self._level_time_O.setText(str(int(self._level_time))) for panel in self._paneles: contact = self.world.contactTestPair(self._player.getRBNode(), panel.getRBNode()) if contact.getNumContacts() > 0: panel.manipulate() brokens = 0 for panel in self._paneles: if panel.isBroken(): brokens += 1 #print brokens for i, vida_imgs in enumerate(self.vidas_imgs): if i < len(self.vidas_imgs) - brokens: vida_imgs[0].show() vida_imgs[1].hide() else: vida_imgs[0].hide() vida_imgs[1].show() if brokens > self.VIDAS: self.gameOver() return task.done if self._level_time <= 0: self._num_lvl += 1 if self._num_lvl <= self._num_lvls: self.nextLevel() else: self.theEnd() return task.done return task.cont def gameOver(self): taskMgr.remove('player-task') taskMgr.remove('panel-task') taskMgr.remove('panel-sound-task') self._mission_text_O = OnscreenText(text = 'Game Over', pos = (0, 0), scale = 0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0)) taskMgr.add(self.gameOverTransition, 'game-over-transition') #self.loadLevel() print "Game Over" def gameOverTransition(self, task): base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) if task.time > 3.0: self._mission_text_O.hide() props = WindowProperties() props.setCursorHidden(False) base.win.requestProperties(props) self._state_context.changeState(gameStateMenu.GameStateMenu(self._state_context)) print "MENU" return task.done return task.cont def nextLevel(self): taskMgr.remove('player-task') taskMgr.remove('panel-task') taskMgr.remove('panel-sound-task') self._mission_text_O = OnscreenText(text = 'Mission\nComplete', pos = (0, 0), scale = 0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0)) taskMgr.add(self.nextLevelTransition, 'next-Level-transition') print "Mission Complete" def nextLevelTransition(self, task): base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) if task.time > 3.0: print "Next Level" self._mission_text_O.hide() self.loadLevel() return task.done return task.cont def theEnd(self): taskMgr.remove('player-task') taskMgr.remove('panel-task') taskMgr.remove('panel-sound-task') self._mission_text_O = OnscreenText(text = '. The End .', pos = (0, 0), scale = 0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0)) taskMgr.add(self.theEndTransition, 'theEnd-transition') #self.loadLevel() print "Mission Complete" def theEndTransition(self, task): base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) if task.time > 3.0: self._mission_text_O.hide() props = WindowProperties() props.setCursorHidden(False) base.win.requestProperties(props) self._state_context.changeState(gameStateMenu.GameStateMenu(self._state_context)) print "The End" return task.done return task.cont
class FloatingPoses(object): """ like freetabletopplacement and tableplacements manipulation.floatingposes corresponds to freegrip floatingposes doesn't take into account the position and orientation of the object it is "free" in position and rotation. No obstacles were considered. In contrast, each item in regrasp.floatingposes 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 "freegrip" "s" is attached to the end of "floatingposes" """ def __init__(self, objpath, gdb, handpkg, base): """ initialization :param objpath: path of the object :param base: for the loadFreeAirGrip --> genHandPairs functions (for collision detection) author: weiwei date: 20161215, tsukuba """ self.handpkg = handpkg self.handname = handpkg.getHandName() self.hand0 = handpkg.newHandNM(hndcolor=[1,0,0,.1]) self.hand1 = handpkg.newHandNM(hndcolor=[0,0,1,.1]) self.objtrimesh = trimesh.load_mesh(objpath) self.objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) self.dbobjname = os.path.splitext(os.path.basename(objpath))[0] # mat4list is nested list, floatingposemat4 is list (the flat version of mat4lsit), they are essentially the same self.mat4list = [] self.floatingposemat4 = [] # gridsfloatingposemat4s is self.floatingposemat4 translated to different grids self.gridsfloatingposemat4s = [] self.icos = None self.floatinggripids = [] self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] self.bulletworld = BulletWorld() self.handpairList = [] self.gdb = gdb self.__loadFreeAirGrip(base) # for IK self.floatinggripikfeas_rgt = [] self.floatinggripikfeas_handx_rgt = [] self.floatinggripikfeas_lft = [] self.floatinggripikfeas_handx_lft = [] # for ik-feasible pairs self.floatinggrippairsids = [] self.floatinggrippairshndmat4s = [] self.floatinggrippairscontacts = [] self.floatinggrippairsnormals = [] self.floatinggrippairsjawwidths = [] self.floatinggrippairsidfreeairs = [] self.__genPandaRotmat4() def __loadFreeAirGrip(self, base): """ load self.freegripids, etc. from mysqldatabase :param gdb: an object of the database.GraspDB class :return: author: weiwei date: 20170110 """ freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname, handname = self.handpkg.getHandName()) if freeairgripdata is None: raise ValueError("Plan the freeairgrip first!") self.freegripids = freeairgripdata[0] self.freegripcontacts = freeairgripdata[1] self.freegripnormals = freeairgripdata[2] self.freegriprotmats = freeairgripdata[3] self.freegripjawwidth = freeairgripdata[4] def __genPandaRotmat4(self, icolevel=1, angles=[0,45,90,135,180,225,270,315]): """ generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere :param icolevel, the default value 1 = 42vertices :param angles, 8 directions by default :return: a list of [pandarotmat4, ...] size of the inner list is size of the angles author: weiwei date: 20170221, tsukuba """ self.mat4list = [] self.icos = trimesh.creation.icosphere(icolevel) initmat4 = self.objnp.getMat() for vert in self.icos.vertices: self.mat4list.append([]) self.objnp.lookAt(vert[0], vert[1], vert[2]) ytozmat4 = Mat4.rotateMat(-90, self.objnp.getMat().getRow3(0)) newobjmat4 = self.objnp.getMat()*ytozmat4 for angle in angles: tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2)) tmppandamat4 = newobjmat4*tmppandamat4 self.mat4list[-1].append(tmppandamat4) self.objnp.setMat(initmat4) self.floatingposemat4 = [e for l in self.mat4list for e in l] def genHandPairs(self, base, loadser=False): self.handpairList = [] if loadser is False: # hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) # hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) pairidlist = list(itertools.combinations(range(len(self.freegripids)), 2)) print len(pairidlist)/5000+1 for i in range(100,len(pairidlist),len(pairidlist)/5000+1): # for i0, i1 in pairidlist: i0, i1 = pairidlist[i] print i, len(pairidlist) self.hand0.setMat(pandanpmat4 = self.freegriprotmats[i0]) self.hand0.setJawwidth(self.freegripjawwidth[i0]) self.hand1.setMat(pandanpmat4 = self.freegriprotmats[i1]) self.hand1.setJawwidth(self.freegripjawwidth[i1]) hndbullnodei0 = cd.genCollisionMeshMultiNp(self.hand0.handnp, base.render) hndbullnodei1 = cd.genCollisionMeshMultiNp(self.hand1.handnp, base.render) result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1) if not result.getNumContacts(): self.handpairList.append([self.freegripids[i0], self.freegripids[i1]]) pickle.dump(self.handpairList, open("tmp.pickle", mode="wb")) else: self.handpairList = pickle.load(open("tmp.pickle", mode="rb")) def genFPandGs(self, grids, icolevel=1, angles=[0,45,90,135,180,225,270,315]): """ genterate floating poses and their grasps, this function leverages genPandaRotmat4 and transformGrips :param icolevel :param angles see genPandaRotmat4 :return: author: weiwei date: 20170221 """ self.gridsfloatingposemat4s = [] self.__genPandaRotmat4(icolevel, angles) for gridposition in grids: for posemat4 in self.floatingposemat4: tmpposemat4 = Mat4(posemat4) tmpposemat4.setRow(3, Vec3(gridposition[0], gridposition[1], gridposition[2])) self.gridsfloatingposemat4s.append(tmpposemat4) self.transformGrips() 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 loadFromDB(self): """ load the floatingposes and their grasps from the database :return: author: weiwei date: 20170227 """ self.gridsfloatingposemat4s = [] self.floatinggripids = [] self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \ AND object.name LIKE '%s'" % self.dbobjname result = self.gdb.execute(sql) if len(result) != 0: for resultrow in result: self.gridsfloatingposemat4s.append(dc.strToMat4(resultrow[1])) idfloatingposes = resultrow[0] sql = "SELECT floatinggrips.idfloatinggrips, floatinggrips.contactpoint0, \ floatinggrips.contactpoint1, floatinggrips.contactnormal0, \ floatinggrips.contactnormal1, floatinggrips.rotmat, floatinggrips.jawwidth, \ floatinggrips.idfreeairgrip FROM floatinggrips,freeairgrip,hand WHERE \ floatinggrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \ freeairgrip.idhand = hand.idhand AND floatinggrips.idfloatingposes = %d AND \ hand.name = '%s'" % (idfloatingposes, self.handpkg.getHandName()) result = self.gdb.execute(sql) if len(result) != 0: floatinggripids = [] floatinggripmat4s = [] floatinggripcontacts = [] floatinggripnormals = [] floatinggripjawwidths = [] floatinggripidfreeairs = [] for resultrow in result: cct0 = dc.strToV3(resultrow[1]) cct1 = dc.strToV3(resultrow[2]) cctn0 = dc.strToV3(resultrow[3]) cctn1 = dc.strToV3(resultrow[4]) floatinggripids.append(int(resultrow[0])) floatinggripmat4s.append(dc.strToMat4(resultrow[5])) floatinggripcontacts.append([cct0, cct1]) floatinggripnormals.append([cctn0, cctn1]) floatinggripjawwidths.append(float(resultrow[6])) floatinggripidfreeairs.append(int(resultrow[7])) self.floatinggripids.append(floatinggripids) self.floatinggripmat4s.append(floatinggripmat4s) self.floatinggripcontacts.append(floatinggripcontacts) self.floatinggripnormals.append(floatinggripnormals) self.floatinggripjawwidth.append(floatinggripjawwidths) self.floatinggripidfreeair.append(floatinggripidfreeairs) else: print 'Plan floating grips first!' assert(False) else: assert('No object found!') def transformGrips(self): """ transform the freeair grips to all rotmat4 in self.gridsfloatingposemat4s :return: author: weiwei date: 20170221, tsukuba """ self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] for icomat4 in self.gridsfloatingposemat4s: floatinggrips = self.transformGripsOnePose(icomat4) self.floatinggripmat4s.append(floatinggrips[0]) self.floatinggripcontacts.append(floatinggrips[1]) self.floatinggripnormals.append(floatinggrips[2]) self.floatinggripjawwidth.append(floatinggrips[3]) self.floatinggripidfreeair.append(floatinggrips[4]) def transformGripsOnePose(self, rotmat4): """ transform the freeair grips to one specific rotmat4 :param rotmat4: :return: [floatinggripmat4s, floatinggripcontacts, floatinggripnormals, floatinggripjawwidths, floatinggripidfreeairs] each element in the list is also a list """ floatinggripmat4s = [] floatinggripcontacts = [] floatinggripnormals = [] floatinggripjawwidths = [] floatinggripidfreeairs = [] for i, gripmat4 in enumerate(self.freegriprotmats): floatinggripmat4 = gripmat4 * rotmat4 cct0 = rotmat4.xformPoint(self.freegripcontacts[i][0]) cct1 = rotmat4.xformPoint(self.freegripcontacts[i][1]) cctn0 = rotmat4.xformPoint(self.freegripnormals[i][0]) cctn1 = rotmat4.xformPoint(self.freegripnormals[i][1]) floatinggripjawwidth = self.freegripjawwidth[i] floatinggripidfreeair = self.freegripids[i] floatinggripmat4s.append(floatinggripmat4) floatinggripcontacts.append([cct0, cct1]) floatinggripnormals.append([cctn0, cctn1]) floatinggripjawwidths.append(floatinggripjawwidth) floatinggripidfreeairs.append(floatinggripidfreeair) return [floatinggripmat4s, floatinggripcontacts, floatinggripnormals, floatinggripjawwidths, floatinggripidfreeairs] def showIcomat4s(self, nodepath): """ show the pandamat4s generated by genPandaRotmat4 :param nodepath, where np to repreantTo, usually base.render :return: author: weiwei date: 20170221, tsukuba """ for i, icomat4list in enumerate(self.mat4list): vert = self.icos.vertices*100 spos = Vec3(vert[i][0], vert[i][1], vert[i][2]) for icomat4 in icomat4list: pg.plotAxisSelf(nodepath, spos, icomat4, length=100, thickness=3) def updateDBwithFGPairs(self, loadser = False): """ compute the floatinggrippairs using freegrippairs and save the floatinggripspairs into Database :param loadser whether use serialized data for handpairlist :return: author: weiwei date: 20170301 """ if len(self.handpairList) == 0: self.genHandPairs(base, loadser) 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) # gen correspondence between freeairgripid and index # indfgoffa means index of floatinggrips whose freeairgripid are xxx indfgoffa = {} for i in range(len(self.floatinggripidfreeair[fpid])): indfgoffa[self.floatinggripidfreeair[fpid][i]] = i # handidpair_indfg is the pairs using index of floatinggrips handidpair_indfg = [] for handidpair in self.handpairList: handidpair_indfg.append([indfgoffa[handidpair[0]], indfgoffa[handidpair[1]]]) # if handidpair_indfg[0] is right, 1 is left sql = "INSERT IGNORE INTO floatinggripspairs VALUES (%d, %d)" % \ (self.floatinggripids[fpid][handidpair_indfg[-1][0]], self.floatinggripids[fpid][handidpair_indfg[-1][1]]) self.gdb.execute(sql) # if 1 is right, 0 is left sql = "INSERT IGNORE INTO floatinggripspairs VALUES (%d, %d)" % \ (self.floatinggripids[fpid][handidpair_indfg[-1][1]], self.floatinggripids[fpid][handidpair_indfg[-1][0]]) self.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) # for i,pairs in enumerate(self.floatinggrippairsids): # print i # print pairs 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 saveIKtoDB(self, idrobot): """ saveupdated IK to DB this function is separated from updateDBwithIK for illustration :return: """ for fpid in range(len(self.gridsfloatingposemat4s)): # right arm idarm = self.gdb.loadIdArm('rgt') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): sql = "INSERT IGNORE INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \ feasibility_handx) VALUES (%d, %d, %d, '%s', '%s')" \ % (idrobot, idarm, idfloatinggrips, self.floatinggripikfeas_rgt[fpid][i], self.floatinggripikfeas_handx_rgt[fpid][i]) gdb.execute(sql) # left arm idarm = self.gdb.loadIdArm('lft') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): sql = "INSERT IGNORE INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \ feasibility_handx) VALUES (%d, %d, %d, '%s', '%s')" \ % (idrobot, idarm, idfloatinggrips, self.floatinggripikfeas_lft[fpid][i], self.floatinggripikfeas_handx_lft[fpid][i]) gdb.execute(sql) def loadIKFromDB(self, robot): """ load the feasibility of IK from db :param robot: :return: author: weiwei date: 20170301 """ idrobot = self.gdb.loadIdRobot(robot) self.floatinggripikfeas_rgt = [] self.floatinggripikfeas_handx_rgt = [] self.floatinggripikfeas_lft = [] self.floatinggripikfeas_handx_lft = [] for fpid in range(len(self.gridsfloatingposemat4s)): # right arm feasibility = [] feasibility_handx = [] idarm = self.gdb.loadIdArm('rgt') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): feasibility.append('False') feasibility_handx.append('False') sql = "SELECT feasibility, feasibility_handx FROM ikfloatinggrips WHERE \ idrobot = %d AND idarm = %d and idfloatinggrips = %d" % (idrobot, idarm, idfloatinggrips) result = self.gdb.execute(sql) if len(result) != 0: feasibility[i] = result[0][0] feasibility_handx[i] = result[0][1] self.floatinggripikfeas_rgt.append(feasibility) self.floatinggripikfeas_handx_rgt.append(feasibility_handx) # left arm feasibility = [] feasibility_handx = [] idarm = self.gdb.loadIdArm('lft') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): feasibility.append('False') feasibility_handx.append('False') sql = "SELECT feasibility, feasibility_handx FROM ikfloatinggrips WHERE \ idrobot = %d AND idarm = %d and idfloatinggrips = %d" % (idrobot, idarm, idfloatinggrips) result = self.gdb.execute(sql) if len(result) != 0: feasibility[i] = result[0][0] feasibility_handx[i] = result[0][1] self.floatinggripikfeas_lft.append(feasibility) self.floatinggripikfeas_handx_lft.append(feasibility_handx) def plotOneFPandG(self, parentnp, fpid=0): """ plot the objpose and grasps at a specific rotmat4 :param fpid: the index of self.floatingposemat4 :return: author: weiwei date: 20170221, tsukuba """ objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) objnp.setMat(self.gridsfloatingposemat4s[fpid]) objnp.reparentTo(parentnp) for i, hndrotmat4 in enumerate(self.gridsfloatingposemat4s[fpid]): if i == 7: # show grasps hand0 = self.handpkg.newHandNM(hndcolor=[0, 1, 0, 1]) hand0.setMat(pandanpmat4 = hndrotmat4) hand0.setJawwidth(self.floatinggripjawwidth[fpid][i]) hand0.reparentTo(parentnp) print self.handpairList for handidpair in self.handpairList: if handidpair[0] == self.floatinggripidfreeair[fpid][i]: pairedilist = [i1 for i1 in range(len(self.floatinggripidfreeair[fpid])) if self.floatinggripidfreeair[fpid][i1]==handidpair[1]] print pairedilist i1 = pairedilist[0] # if self.floatinggripikfeas_lft[fpid][i1] == 'True': hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1]) hndrotmat4 = self.floatinggripmat4s[fpid][i1] hand1.setMat(pandanpmat4 = hndrotmat4) hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1]) hand1.reparentTo(parentnp) if handidpair[1] == self.floatinggripidfreeair[fpid][i]: pairedilist = [i1 for i1 in range(len(self.floatinggripidfreeair[fpid])) if self.floatinggripidfreeair[fpid][i1]==handidpair[0]] print pairedilist i1 = pairedilist[0] # if self.floatinggripikfeas_lft[fpid][i1] == 'True': hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1]) hndrotmat4 = self.floatinggripmat4s[fpid][i1] hand1.setMat(pandanpmat4 = hndrotmat4) hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1]) hand1.reparentTo(parentnp) def plotOneFPandGPairs(self, parentnp, fpid=0): """ plot the gpairss at a specific rotmat4 :param fpid: the index of self.floatingposemat4 :return: author: weiwei date: 20170301, tsukuba """ objnp = pg.packpandanp(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) objnp.setMat(self.gridsfloatingposemat4s[fpid]) objnp.setColor(Vec4(.7,0.3,0,1)) objnp.reparentTo(parentnp) print self.floatinggrippairshndmat4s[fpid] for i, hndrotmat4pair in enumerate(self.floatinggrippairshndmat4s[fpid]): # if i == 9: # show grasps hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .5]) hand0mat4 = Mat4(hndrotmat4pair[0]) # h0row3 = hand0mat4.getRow3(3) # h0row3[2] = h0row3[2]+i*20.0 # hand0mat4.setRow(3, h0row3[2]) hand0.setMat(pandanpmat4 = hand0mat4) hand0.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][0]) hand0.reparentTo(parentnp) hand1 = self.handpkg.newHandNM(hndcolor=[0, .0, 1, .5]) hand1mat4 = Mat4(hndrotmat4pair[1]) # h1row3 = hand1mat4.getRow3(3) # h1row3[2] = h1row3[2]+i*20.0 # hand1mat4.setRow(3, h1row3[2]) hand1.setMat(pandanpmat4 = hand1mat4) hand1.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][1]) hand1.reparentTo(parentnp)
class TwoObjAss(object): """ the class uses genAvailableFAGs to compute grasps for the assembly of two objects different rotations could be saved to database """ def __init__(self, base, obj0path, obj0Mat4, obj1path, obj1Mat4, assDirect1to0, gdb, handpkg): """ initiliazation obj0 will be manipulated by rgt hand, obj1 will be manipulated by lft hand # see genAvailableFAGs function for the details of parameters :param base: :param obj0path: :param obj0Mat4: :param obj1path: :param obj1Mat4: :param assDirect1to0: the assembly direction to assemble object 1 to object 0 (with length) :param gdb: :param handpkg: """ self.base = base self.obj0path = obj0path self.obj0Mat4 = obj0Mat4 self.dbobj0name = os.path.splitext(os.path.basename(obj0path))[0] self.obj1path = obj1path self.obj1Mat4 = obj1Mat4 self.dbobj1name = os.path.splitext(os.path.basename(obj1path))[0] self.assDirect1to0 = assDirect1to0 self.gdb = gdb self.handpkg = handpkg # define the free ass0grip and ass1grip # definition: objgrips: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair] self.obj0grips = [] self.obj1grips = [] # ico means they corresponds to ico rotmats self.gridsfloatingposemat4s = [] # right grips for each gridsfloatingposemat4 self.icoass0gripids = [] self.icoass0gripcontacts = [] self.icoass0gripnormals = [] self.icoass0griprotmat4s = [] self.icoass0gripjawwidth = [] self.icoass0gripidfreeair = [] # left grips for each gridsfloatingposemat4 self.icoass1gripids = [] self.icoass1gripcontacts = [] self.icoass1gripnormals = [] self.icoass1griprotmat4s = [] self.icoass1gripjawwidth = [] self.icoass1gripidfreeair = [] # ik related self.icoass0gripsik = [] self.icoass0gripsik_retassdir = [] self.icoass1gripsik = [] self.icoass1gripsik_retassdir = [] # handpairList self.handpairList = [] self.bulletworld = BulletWorld() # ik feasible pairs for each gridsfloatingposemat4 self.icoassgrippairsids = [] self.icoassgrippairscontacts = [] self.icoassgrippairsnormals = [] self.icoassgrippairshndmat4s = [] self.icoassgrippairsjawwidths = [] self.icoassgrippairsidfreeairs = [] def genXAssPandGs(self, grids, icolevel=1, angles=[0,45,90,135,180,225,270,315]): """ genterate xass poses and their grasps, this function leverages genPandaRotmat4 and transformGrips :param icolevel :param angles see genPandaRotmat4 :return: author: weiwei date: 20170221 """ self.gridsfloatingposemat4s = [] self.__genPandaRotmat4(icolevel, angles) for gridposition in grids: for posemat4 in self.floatingposemat4: tmpposemat4 = Mat4(posemat4) tmpposemat4.setRow(3, Vec3(gridposition[0], gridposition[1], gridposition[2])) self.gridsfloatingposemat4s.append(tmpposemat4) if len(self.obj0grips) == 0 or len(self.obj1grips) == 0: self.__genFreeAssGrips() self.__transformGrips() 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) # TODO: Have to delete assemblyx when grasps are deleted # TODO: write algorithms to enable insertion of grasps using idassemblyx def loadFromDB(self): """ :return: """ self.gridsfloatingposemat4s = [] self.icoass0gripids = [] self.icoass0gripcontacts = [] self.icoass0gripnormals = [] self.icoass0griprotmat4s = [] self.icoass0gripjawwidth = [] self.icoass0gripidfreeair = [] self.icoass1gripids = [] self.icoass1gripcontacts = [] self.icoass1gripnormals = [] self.icoass1griprotmat4s = [] self.icoass1gripjawwidth = [] self.icoass1gripidfreeair = [] idassembly = self.gdb.loadIdAssembly(self.dbobj0name, self.obj0Mat4, self.dbobj1name, self.obj1Mat4, self.assDirect1to0) # check if rotation already exist sql = "SELECT assemblyx.idassemblyx, assemblyx.rotmat FROM assemblyx WHERE assemblyx.idassembly = %d" % idassembly result = self.gdb.execute(sql) if len(result) != 0: for resultrow in result: poserotmat4 = dc.strToMat4(resultrow[1]) self.gridsfloatingposemat4s.append(poserotmat4) idassemblyx = resultrow[0] # g0 sql = "SELECT * FROM assemblyxgrips0 WHERE idassemblyx = %d" % idassemblyx result = self.gdb.execute(sql) if len(result) != 0: self.icoass0gripids.append([]) self.icoass0gripcontacts.append([]) self.icoass0gripnormals.append([]) self.icoass0griprotmat4s.append([]) self.icoass0gripjawwidth.append([]) self.icoass0gripidfreeair.append([]) for resultrow in result: self.icoass0gripids[-1].append(resultrow[0]) cct0 = dc.strToV3(resultrow[1]) cct1 = dc.strToV3(resultrow[2]) cctn0 = dc.strToV3(resultrow[3]) cctn1 = dc.strToV3(resultrow[4]) self.icoass0gripcontacts[-1].append([cct0, cct1]) self.icoass0gripnormals[-1].append([cctn0, cctn1]) self.icoass0griprotmat4s[-1].append(dc.strToMat4(resultrow[5])) self.icoass0gripjawwidth[-1].append(float(resultrow[6])) self.icoass0gripidfreeair[-1].append(int(resultrow[8])) # g1 sql = "SELECT * FROM assemblyxgrips1 WHERE idassemblyx = %d" % idassemblyx result = self.gdb.execute(sql) if len(result) != 0: self.icoass1gripids.append([]) self.icoass1gripcontacts.append([]) self.icoass1gripnormals.append([]) self.icoass1griprotmat4s.append([]) self.icoass1gripjawwidth.append([]) self.icoass1gripidfreeair.append([]) for resultrow in result: self.icoass1gripids[-1].append(resultrow[0]) cct0 = dc.strToV3(resultrow[1]) cct1 = dc.strToV3(resultrow[2]) cctn0 = dc.strToV3(resultrow[3]) cctn1 = dc.strToV3(resultrow[4]) self.icoass1gripcontacts[-1].append([cct0, cct1]) self.icoass1gripnormals[-1].append([cctn0, cctn1]) self.icoass1griprotmat4s[-1].append(dc.strToMat4(resultrow[5])) self.icoass1gripjawwidth[-1].append(float(resultrow[6])) self.icoass1gripidfreeair[-1].append(int(resultrow[8])) def updateDBwithHandPairs(self): """ compute the assgrippairs using assfreegrippairs and save the assgrippairs into Database :return: author: weiwei date: 20170307 """ self.__genHandPairs(base) 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) # gen correspondence between freeairgripid and index # indagoffa means index of assgrips whose freeairgripid are xxx indagoffa0 = {} for i in range(len(self.icoass0gripidfreeair[fpid])): indagoffa0[self.icoass0gripidfreeair[fpid][i]] = i indagoffa1 = {} for i in range(len(self.icoass1gripidfreeair[fpid])): indagoffa1[self.icoass1gripidfreeair[fpid][i]] = i # handidpair_indag is the pairs using index of floatinggrips handidpair_indag = [] for handidpair in self.handpairList: handidpair_indag.append([indagoffa0[handidpair[0]], indagoffa1[handidpair[1]]]) sql = "INSERT IGNORE INTO assemblyxgrippairs VALUES (%d, %d)" % \ (self.icoass0gripids[fpid][handidpair_indag[-1][0]], self.icoass1gripids[fpid][handidpair_indag[-1][1]]) self.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 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 __genHandPairs(self, base): self.handpairList = [] self.__genFreeAssGrips() ass0gripcontacts, ass0gripnormals, ass0griprotmat4s, ass0gripjawwidth, ass0gripidfreeair = self.obj0grips ass1gripcontacts, ass1gripnormals, ass1griprotmat4s, ass1gripjawwidth, ass1gripidfreeair = self.obj1grips hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) i0list = range(len(ass0gripidfreeair)) i1list = range(len(ass1gripidfreeair)) pairidlist = list(itertools.product(i0list, i1list)) print len(pairidlist)/10000+1 for i in range(0,len(pairidlist),len(pairidlist)/10000+1): # for i0, i1 in pairidlist: i0, i1 = pairidlist[i] print i, len(pairidlist) hand0.setMat(ass0griprotmat4s[i0]) hand0.setJawwidth(ass0gripjawwidth[i0]) hand1.setMat(ass1griprotmat4s[i1]) hand1.setJawwidth(ass1gripjawwidth[i1]) hndbullnodei0 = cd.genCollisionMeshMultiNp(hand0.handnp, base.render) hndbullnodei1 = cd.genCollisionMeshMultiNp(hand1.handnp, base.render) result = self.bulletworld.contactTestPair(hndbullnodei0, hndbullnodei1) if not result.getNumContacts(): self.handpairList.append([ass0gripidfreeair[i0], ass1gripidfreeair[i1]]) # print ass0gripidfreeair # print self.icoass0gripidfreeair[0] # print ass1gripidfreeair # print self.icoass1gripidfreeair[0] # print self.handpairList # assert "compare" def __saveIKtoDB(self, idrobot): """ saveupdated IK to DB this function is separated from updateDBwithIK for illustration :return: """ for fpid in range(len(self.gridsfloatingposemat4s)): # right arm idarm = self.gdb.loadIdArm('rgt') for i, idicoass0grip in enumerate(self.icoass0gripids[fpid]): sql = "INSERT IGNORE INTO ikassemblyxgrips0(idrobot, idarm, idassemblyxgrips0, feasibility, \ feasibility_assdir) VALUES (%d, %d, %d, '%s', '%s')" \ % (idrobot, idarm, idicoass0grip, self.icoass0gripsik[fpid][i], self.icoass0gripsik_retassdir[fpid][i]) gdb.execute(sql) # left arm idarm = self.gdb.loadIdArm('lft') for i, idicoass1grip in enumerate(self.icoass1gripids[fpid]): sql = "INSERT IGNORE INTO ikassemblyxgrips1(idrobot, idarm, idassemblyxgrips1, feasibility, \ feasibility_assdir) VALUES (%d, %d, %d, '%s', '%s')" \ % (idrobot, idarm, idicoass1grip, self.icoass1gripsik[fpid][i], self.icoass1gripsik_retassdir[fpid][i]) gdb.execute(sql) def __loadIKFromDB(self, robot): """ load the feasibility of IK from db :param robot: :return: author: weiwei date: 20170307 """ idrobot = self.gdb.loadIdRobot(robot) self.assgikfeas0 = [] self.assgikfeas0_retassdir = [] self.assgikfeas1= [] self.assgikfeas1_retassdir = [] for fpid in range(len(self.gridsfloatingposemat4s)): # right arm assgikfeas0 = [] assgikfeas0_retassdir = [] idarm = self.gdb.loadIdArm('rgt') for i, idassgrips in enumerate(self.icoass0gripids[fpid]): assgikfeas0.append('False') assgikfeas0_retassdir.append('False') sql = "SELECT feasibility, feasibility_assdir FROM ikassemblyxgrips0 WHERE \ idrobot = %d AND idarm = %d and idassemblyxgrips0 = %d" % (idrobot, idarm, idassgrips) result = self.gdb.execute(sql) if len(result) != 0: assgikfeas0[i] = result[0][0] assgikfeas0_retassdir[i] = result[0][1] else: assert "Compute ik of the freeassemblygrips first!" self.assgikfeas0.append(assgikfeas0) self.assgikfeas0_retassdir.append(assgikfeas0_retassdir) # left arm assgikfeas1 = [] assgikfeas1_retassdir = [] idarm = self.gdb.loadIdArm('lft') for i, idassgrips in enumerate(self.icoass1gripids[fpid]): assgikfeas1.append('False') assgikfeas1_retassdir.append('False') sql = "SELECT feasibility, feasibility_assdir FROM ikassemblyxgrips1 WHERE \ idrobot = %d AND idarm = %d and idassemblyxgrips1 = %d" % (idrobot, idarm, idassgrips) result = self.gdb.execute(sql) if len(result) != 0: assgikfeas1[i] = result[0][0] assgikfeas1_retassdir[i] = result[0][1] else: assert "Compute ik of the freeassemblygrips first!" self.assgikfeas1.append(assgikfeas1) self.assgikfeas1_retassdir.append(assgikfeas1_retassdir) def __genPandaRotmat4(self, icolevel=1, angles=[0, 45, 90, 135, 180, 225, 270, 315]): """ generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere :param icolevel, the default value 1 = 42vertices :param angles, 8 directions by default :return: a list of [pandarotmat4, ...] size of the inner list is size of the angles author: weiwei date: 20170221, tsukuba (copied from regrasp/floatingposes.py) """ objnp = pg.genObjmnp(self.obj0path) mat4list = [] self.icos = trimesh.creation.icosphere(icolevel) initmat4 = objnp.getMat() for vert in self.icos.vertices: mat4list.append([]) objnp.lookAt(vert[0], vert[1], vert[2]) ytozmat4 = Mat4.rotateMat(-90, objnp.getMat().getRow3(0)) newobjmat4 = objnp.getMat() * ytozmat4 for angle in angles: tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2)) tmppandamat4 = newobjmat4 * tmppandamat4 mat4list[-1].append(tmppandamat4) objnp.setMat(initmat4) self.floatingposemat4 = [e for l in mat4list for e in l] def __genFreeAssGrips(self): self.obj0grips = [] self.obj1grips = [] self.obj0grips, self.obj1grips = \ genAvailableFAGs(self.base, self.obj0path, self.obj0Mat4, self.obj1path, self.obj1Mat4, self.gdb, self.handpkg) def __transformGripsOnePose(self, objgrips, rotmat4): """ transform the freeair grips to one specific rotmat4 :param objgrips: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair] :param rotmat4: :return: [xassgripcontacts, xassgripnormals, xassgriprotmat4s, xassgripjawwidth, xassgripidfreeair] each element in the list is also a list author: weiwei date: 20170307 """ assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair = objgrips xassgripcontacts = [] xassgripnormals = [] xassgriprotmat4s = [] xassgripjawwidth = [] xassgripidfreeair = [] for i, gripmat4 in enumerate(assgriprotmat4s): floatinggripmat4 = gripmat4 * rotmat4 cct0 = rotmat4.xformPoint(assgripcontacts[i][0]) cct1 = rotmat4.xformPoint(assgripcontacts[i][1]) cctn0 = rotmat4.xformPoint(assgripnormals[i][0]) cctn1 = rotmat4.xformPoint(assgripnormals[i][1]) xassgripcontacts.append([cct0, cct1]) xassgripnormals.append([cctn0, cctn1]) xassgriprotmat4s.append(floatinggripmat4) xassgripjawwidth.append(assgripjawwidth[i]) xassgripidfreeair.append(assgripidfreeair[i]) return [xassgripcontacts, xassgripnormals, xassgriprotmat4s, xassgripjawwidth, xassgripidfreeair] def __transformGrips(self): """ transform the freeass grips to all rotmat4 in self.gridsfloatingposemat4s :return: author: weiwei date: 20170221, tsukuba (copied from regrasp/floatingposes.py) """ self.icoass0gripcontacts = [] self.icoass0gripnormals = [] self.icoass0griprotmat4s = [] self.icoass0gripjawwidth = [] self.icoass0gripidfreeair = [] for icomat4 in self.gridsfloatingposemat4s: xassgrips = self.__transformGripsOnePose(self.obj0grips, icomat4) self.icoass0gripcontacts.append(xassgrips[0]) self.icoass0gripnormals.append(xassgrips[1]) self.icoass0griprotmat4s.append(xassgrips[2]) self.icoass0gripjawwidth.append(xassgrips[3]) self.icoass0gripidfreeair.append(xassgrips[4]) self.icoass1gripcontacts = [] self.icoass1gripnormals = [] self.icoass1griprotmat4s = [] self.icoass1gripjawwidth = [] self.icoass1gripidfreeair = [] for icomat4 in self.gridsfloatingposemat4s: xassgrips = self.__transformGripsOnePose(self.obj1grips, icomat4) self.icoass1gripcontacts.append(xassgrips[0]) self.icoass1gripnormals.append(xassgrips[1]) self.icoass1griprotmat4s.append(xassgrips[2]) self.icoass1gripjawwidth.append(xassgrips[3]) self.icoass1gripidfreeair.append(xassgrips[4])
class FloatingPoses(object): """ like freetabletopplacement and tableplacements manipulation.floatingposes corresponds to freegrip floatingposes doesn't take into account the position and orientation of the object it is "free" in position and rotation. No obstacles were considered. In contrast, each item in regrasp.floatingposes 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 "freegrip" "s" is attached to the end of "floatingposes" meanings of abbreviations: FP = floating pose, G = grasps, GP = grasping pairs author: weiwei date: 2016, 2019 """ def __init__(self, objpath, gdb, handpkg, base): """ initialization :param objpath: path of the object :param base: for the loadFreeAirGrip --> genHandPairs functions (for collision detection) author: weiwei date: 20161215, tsukuba """ self.__base = base self.handpkg = handpkg self.handname = handpkg.getHandName() self.hand0 = handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) self.hand1 = handpkg.newHandNM(hndcolor=[0, 0, 1, .1]) self.objtrimesh = trimesh.load_mesh(objpath) self.objnp = self.__base.pg.packpandanp_fn( self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) self.dbobjname = os.path.splitext(os.path.basename(objpath))[0] # mat4list is nested list, floatingposemat4 is list (the flat version of mat4lsit), they are essentially the same self.mat4list = [] self.floatingposemat4 = [] # gridsfloatingposemat4s is self.floatingposemat4 translated to different grids self.gridsfloatingposemat4s = [] self.icos = None self.floatinggripids = [] self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] self.bulletworld = BulletWorld() self.handpairList = [] self.gdb = gdb self.__loadFreeAirGrip(base) # for IK self.feasibility_rgt = [] self.feasibility_handa_rgt = [] self.feasibility_lft = [] self.feasibility_handa_lft = [] self.jnts_rgt = [] self.jnts_handa_rgt = [] self.jnts_lft = [] self.jnts_handa_lft = [] # for ik-feasible pairs self.floatinggrippairsids = [] self.floatinggrippairshndmat4s = [] self.floatinggrippairscontacts = [] self.floatinggrippairsnormals = [] self.floatinggrippairsjawwidths = [] self.floatinggrippairsidfreeairs = [] self.floatinggrippairsjnts = [] self.floatinggrippairsjnts_handa = [] def __loadFreeAirGrip(self, base): """ load self.freegripids, etc. from mysqldatabase :param gdb: an object of the database.GraspDB class :return: author: weiwei date: 20170110 """ freeairgripdata = self.gdb.loadFreeAirGrip( self.dbobjname, handname=self.handpkg.getHandName()) if freeairgripdata is None: raise ValueError("Plan the freeairgrip first!") self.freegripids = freeairgripdata[0] self.freegripcontacts = freeairgripdata[1] self.freegripnormals = freeairgripdata[2] self.freegriprotmats = freeairgripdata[3] self.freegripjawwidth = freeairgripdata[4] def __genPandaRotmat4(self, icolevel=1, angles=[0, 45, 90, 135, 180, 225, 270, 315]): """ generate panda3d rotmat4 using icospheres and rotationaangle each origin-vertex vector of the icosphere :param icolevel, the default value 1 = 42vertices :param angles, 8 directions by default :return: a list of [pandarotmat4, ...] size of the inner list is size of the angles author: weiwei date: 20170221, tsukuba """ self.mat4list = [] self.icos = trimesh.creation.icosphere(icolevel) initmat4 = self.objnp.getMat() for vert in self.icos.vertices: self.mat4list.append([]) self.objnp.lookAt(vert[0], vert[1], vert[2]) ytozmat4 = Mat4.rotateMat(-90, self.objnp.getMat().getRow3(0)) newobjmat4 = self.objnp.getMat() * ytozmat4 for angle in angles: tmppandamat4 = Mat4.rotateMat(angle, newobjmat4.getRow3(2)) tmppandamat4 = newobjmat4 * tmppandamat4 self.mat4list[-1].append(tmppandamat4) self.objnp.setMat(initmat4) self.floatingposemat4 = [e for l in self.mat4list for e in l] print(len(self.floatingposemat4)) def __checkFPexistance(self): """ check if the floating pose, grasps, and the handover pairs already exist if exist, return True else return False :return: boolean value indicating the existance of floating poses, grasps, and the handover pairs author: weiwei date: 20190318 """ 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: print("Floating poses already saved!") isredo = input("Do you want to overwrite the database? (Y/N)") if isredo != "Y" and isredo != "y": print("Floating pose planning aborted.") return True else: print("Deleting existing floating poses...") sql = "DELETE FROM floatingposes USING floatingposes, object WHERE floatingposes.idobject = object.idobject AND \ object.name LIKE '%s'" % self.dbobjname self.gdb.execute(sql) return False def __saveFPGToDB(self): """ save the floatingposes and their grasps to the database :return: author: weiwei date: 20170221 """ print("Saving floating poses and grasps into the database...") sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \ AND object.name LIKE '%s'" % self.dbobjname result = self.gdb.execute(sql) if len(result) == 0: # the gridsfloatingposes for the self.dbobjname is not saved sql = "INSERT INTO floatingposes(rotmat, idobject) VALUES " for i in range(len(self.gridsfloatingposemat4s)): # print i, "/", len(self.gridsfloatingposemat4s) sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \ (dc.mat4ToStr(self.gridsfloatingposemat4s[i]), self.dbobjname) sql = sql[:-2] + ";" self.gdb.execute(sql) sql = "SELECT * FROM floatinggrips,floatingposes,object,freeairgrip,hand WHERE \ floatinggrips.idfloatingposes = floatingposes.idfloatingposes AND \ floatingposes.idobject = object.idobject AND \ object.name LIKE '%s' AND floatinggrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \ freeairgrip.idhand = hand.idhand AND hand.name LIKE '%s'" % ( self.dbobjname, self.handpkg.getHandName()) result = self.gdb.execute(sql) if len(result) == 0: tic = time.time() for i in range(len(self.gridsfloatingposemat4s)): toc = time.time() print( "Saving FP and Gs to DB", " Finished: ", i, "/", len(self.gridsfloatingposemat4s), " Time past: ", "%.2f" % (toc - tic), "s", " Expected remaining time: ", "%.2f" % ((toc - tic) * len(self.gridsfloatingposemat4s) / (i + 1e-4) - (toc - tic)), "s") sql = "SELECT floatingposes.idfloatingposes FROM floatingposes,object WHERE \ floatingposes.idobject = object.idobject AND \ floatingposes.rotmat LIKE '%s' AND \ object.name LIKE '%s'" % (dc.mat4ToStr( self.gridsfloatingposemat4s[i]), self.dbobjname) result = self.gdb.execute(sql)[0] if len(result) != 0: idfloatingposes = result[0] sql = "SELECT * FROM floatinggrips WHERE idfloatingposes = %d" % idfloatingposes result = self.gdb.execute(sql) if len(result) == 0: if len(self.floatinggripmat4s[i]) != 0: sql = "INSERT INTO floatinggrips(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \ rotmat, jawwidth, idfloatingposes, idfreeairgrip) VALUES " for j in range(len(self.floatinggripmat4s[i])): # print "gripposes", i, "/", len(self.gridsfloatingposemat4s) # print "grips", j, "/", len(self.floatinggripmat4s[i]) cct0 = self.floatinggripcontacts[i][j][0] cct1 = self.floatinggripcontacts[i][j][1] cctn0 = self.floatinggripnormals[i][j][0] cctn1 = self.floatinggripnormals[i][j][1] sql += "('%s', '%s', '%s', '%s', '%s', '%s', %d, %d), " % \ (dc.v3ToStr(cct0), dc.v3ToStr(cct1), dc.v3ToStr(cctn0), dc.v3ToStr(cctn1), \ dc.mat4ToStr(self.floatinggripmat4s[i][j]), str(self.floatinggripjawwidth[i][j]), \ idfloatingposes, self.floatinggripidfreeair[i][j]) sql = sql[:-2] + ";" self.gdb.execute(sql) def __genHandPairs(self, base, loadser=False): self.handpairList = [] if loadser is False: # hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) # hand1 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1]) pairidlist = list( itertools.combinations(range(len(self.freegripids)), 2)) total = len(pairidlist) / (len(pairidlist) / 5000) + 1 tic = time.time() for i in range(100, len(pairidlist), int(len(pairidlist) / 5000.0 + 1)): toc = time.time() print( "Generating grasp pairs", " Finished: ", "%.2f" % (i / int(len(pairidlist) / 5000.0 + 1) / total * 100), "%", " Time past: ", "%.2f" % (toc - tic), "s", " Expected remaining time: ", "%.2f" % ((toc - tic) * int(len(pairidlist) / 5000.0 + 1) * total / (i + 1e-4) - (toc - tic)), "s") i0, i1 = pairidlist[i] self.hand0.setMat(pandanpmat4=self.freegriprotmats[i0]) # self.hand0.setMat(npmat4=self.freegriprotmats[i0]) self.hand0.setJawwidth(self.freegripjawwidth[i0]) self.hand1.setMat(pandanpmat4=self.freegriprotmats[i1]) # self.hand1.setMat(npmat4=self.freegriprotmats[i1]) self.hand1.setJawwidth(self.freegripjawwidth[i1]) hndbullnodei0 = cd.genBulletCDMeshMultiNp( self.hand0.handnp, base.render) hndbullnodei1 = cd.genBulletCDMeshMultiNp( self.hand1.handnp, base.render) result = self.bulletworld.contactTestPair( hndbullnodei0, hndbullnodei1) if not result.getNumContacts(): # check the open state self.hand0.setJawwidth(self.hand0.jawwidthopen) self.hand1.setJawwidth(self.hand1.jawwidthopen) hndbullnodei0 = cd.genBulletCDMeshMultiNp( self.hand0.handnp, base.render) hndbullnodei1 = cd.genBulletCDMeshMultiNp( self.hand1.handnp, base.render) result = self.bulletworld.contactTestPair( hndbullnodei0, hndbullnodei1) if not result.getNumContacts(): self.handpairList.append( [self.freegripids[i0], self.freegripids[i1]]) json.dump(self.handpairList, open("tmp.json", mode="w")) else: self.handpairList = json.load(open("tmp.json", mode="r")) def __transformGrips(self): """ transform the freeair grips to all rotmat4 in self.gridsfloatingposemat4s :return: author: weiwei date: 20170221, tsukuba """ self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] for icomat4 in self.gridsfloatingposemat4s: floatinggrips = self.transformGripsOnePose(icomat4) self.floatinggripmat4s.append(floatinggrips[0]) self.floatinggripcontacts.append(floatinggrips[1]) self.floatinggripnormals.append(floatinggrips[2]) self.floatinggripjawwidth.append(floatinggrips[3]) self.floatinggripidfreeair.append(floatinggrips[4]) def __updateDBwithGP(self, loadser=False): """ compute the floatinggrippairs using freegrippairs and save the floatinggripspairs into Database :param loadser whether use serialized data for handpairlist :return: author: weiwei date: 20170301 """ print( "Start updating the floatingpose/grasps DB with floating grasp pairs!" ) if len(self.handpairList) == 0: self.__genHandPairs(base, loadser) tic = time.time() for fpid in range(len(self.gridsfloatingposemat4s)): toc = time.time() print( "Saving grasp pairs", " Finished: ", fpid, "/", len(self.gridsfloatingposemat4s), " Time past: ", "%.2f" % (toc - tic), "s", " Expected remaining time: ", "%.2f" % ((toc - tic) * len(self.gridsfloatingposemat4s) / (fpid + 1e-4) - (toc - tic)), "s") # gen correspondence between freeairgripid and index # indfgoffa means index of floatinggrips whose freeairgripid are xxx indfgoffa = {} for i in range(len(self.floatinggripidfreeair[fpid])): indfgoffa[self.floatinggripidfreeair[fpid][i]] = i # handidpair_indfg is the pairs using index of floatinggrips handidpair_indfg = [] sql = "INSERT INTO floatinggripspairs VALUES " for handidpair in self.handpairList: handidpair_indfg.append( [indfgoffa[handidpair[0]], indfgoffa[handidpair[1]]]) # if handidpair_indfg[0] is right, 1 is left sql = sql + "(%d, %d)" % \ (self.floatinggripids[fpid][handidpair_indfg[-1][0]], self.floatinggripids[fpid][handidpair_indfg[-1][1]]) # if 1 is right, 0 is left sql = sql + ", (%d, %d), " % \ (self.floatinggripids[fpid][handidpair_indfg[-1][1]], self.floatinggripids[fpid][handidpair_indfg[-1][0]]) # self.gdb.execute(sql) # # if 1 is right, 0 is left # sql = "INSERT INTO floatinggripspairs VALUES (%d, %d)" % \ # (self.floatinggripids[fpid][handidpair_indfg[-1][1]], self.floatinggripids[fpid][handidpair_indfg[-1][0]]) sql = sql[:-2] self.gdb.execute(sql) def __checkIKexistance(self, idrobot): """ check if the ik has been computed the existance is confirmed if the ik of the first idfloatinggrips has been updated :param idrobot: :return: boolean value indicating the existence of ik, True for existence, False for non-existence author: weiwei date: 20190318 """ # bexist = False # for fpid in range(len(self.gridsfloatingposemat4s)): # print(fpid, len(self.gridsfloatingposemat4s)) # # right arm # idarm = self.gdb.loadIdArm('rgt') # for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): # sql = "SELECT * FROM ikfloatinggrips WHERE idrobot LIKE '%s' AND idarm LIKE '%s' \ # AND idfloatinggrips LIKE '%s'" % (idrobot, idarm, idfloatinggrips) # result = self.gdb.execute(sql) # if(len(result)>0): # bexist = True # break # if bexist: # break # # left arm # idarm = self.gdb.loadIdArm('lft') # for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): # sql = "SELECT * FROM ikfloatinggrips WHERE idrobot LIKE '%s' AND idarm LIKE '%s' \ # AND idfloatinggrips LIKE '%s'" % (idrobot, idarm, idfloatinggrips) # result = self.gdb.execute(sql) # if(len(result)>0): # bexist = True # break # if bexist: # break # # if bexist: # print("IK is already updated!") # isredo = input("Do you want to overwrite the database? (Y/N)") # if isredo != "Y" and isredo != "y": # print("Updating IK aborted.") # return True # else: # for fpid in range(len(self.gridsfloatingposemat4s)): # # right arm # idarm = self.gdb.loadIdArm('rgt') # for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): # sql = "DELETE FROM ikfloatinggrips WHERE idrobot LIKE '%s' AND idarm LIKE '%s' \ # AND idfloatinggrips LIKE '%s'" % (idrobot, idarm, idfloatinggrips) # self.gdb.execute(sql) # # left arm # idarm = self.gdb.loadIdArm('lft') # for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): # sql = "DELETE FROM ikfloatinggrips WHERE idrobot LIKE '%s' AND idarm LIKE '%s' \ # AND idfloatinggrips LIKE '%s'" % (idrobot, idarm, idfloatinggrips) # self.gdb.execute(sql) # return False # else: # return False sql = "SELECT * FROM ikfloatinggrips WHERE idrobot LIKE '%s'" % ( idrobot) result = self.gdb.execute(sql) if (len(result) > 0): print("IK is already updated!") isredo = input("Do you want to overwrite the database? (Y/N)") if isredo != "Y" and isredo != "y": print("Updating IK aborted.") return True else: sql = "DELETE FROM ikfloatinggrips WHERE idrobot LIKE '%s'" % ( idrobot) self.gdb.execute(sql) return False else: return False def __saveIKtoDB(self, idrobot): """ saveupdated IK to DB this function is separated from updateDBwithIK for illustration :return: author: weiwei date: 20190316, toyonaka """ for fpid in range(len(self.gridsfloatingposemat4s)): # right arm idarm = self.gdb.loadIdArm('rgt') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): sql = "INSERT INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \ feasibility_handa, jnts, jnts_handa) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s')" \ % (idrobot, idarm, idfloatinggrips, self.feasibility_rgt[fpid][i], self.feasibility_handa_rgt[fpid][i], self.jnts_rgt[fpid][i], self.jnts_handa_rgt[fpid][i]) self.gdb.execute(sql) # left arm idarm = self.gdb.loadIdArm('lft') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): sql = "INSERT INTO ikfloatinggrips(idrobot, idarm, idfloatinggrips, feasibility, \ feasibility_handa, jnts, jnts_handa) VALUES (%d, %d, %d, '%s', '%s', '%s', '%s')" \ % (idrobot, idarm, idfloatinggrips, self.feasibility_lft[fpid][i], self.feasibility_handa_lft[fpid][i], self.jnts_lft[fpid][i], self.jnts_handa_lft[fpid][i]) self.gdb.execute(sql) def genFPGPandSaveToDB(self, grids, icolevel=1, angles=[0, 45, 90, 135, 180, 225, 270, 315]): """ genterate floating poses and their grasps and the floatinggrippairs, this function leverages genPandaRotmat4 and transformGrips :param icolevel :param angles see genPandaRotmat4 :return: author: weiwei date: 20170221 """ if self.__checkFPexistance(): pass else: self.gridsfloatingposemat4s = [] self.__genPandaRotmat4(icolevel, angles) for gridposition in grids: for posemat4 in self.floatingposemat4: tmpposemat4 = Mat4(posemat4) tmpposemat4.setRow( 3, Vec3(gridposition[0], gridposition[1], gridposition[2])) self.gridsfloatingposemat4s.append(tmpposemat4) self.__transformGrips() print("Saving the floatingposes and grasps to database") self.__saveFPGToDB() print("Updating the databse with floating grasppairs") self.loadFPGfromDB() # update floatinggripids self.__updateDBwithGP() def updateDBwithIK(self, robot): """ compute the IK feasible grasps of each pose :return: author: weiwei date: 20190316 """ # load retraction distances rethanda, retworlda, worldz = self.gdb.loadIKRet() # load idrobot idrobot = self.gdb.loadIdRobot(robot) if self.__checkIKexistance(idrobot): pass else: self.feasibility_rgt = [] self.feasibility_handa_rgt = [] self.feasibility_lft = [] self.feasibility_handa_lft = [] self.jnts_rgt = [] self.jnts_handa_rgt = [] self.jnts_lft = [] self.jnts_handa_lft = [] tic = time.time() for fpid in range(len(self.gridsfloatingposemat4s)): toc = time.time() print( "Updating IK", " Finished: ", fpid, "/", len(self.gridsfloatingposemat4s), " Time past: ", "%.2f" % (toc - tic), "s", " Expected remaining time: ", "%.2f" % ((toc - tic) * len(self.gridsfloatingposemat4s) / (fpid + 1e-4) - (toc - tic)), "s") ### right hand armid = 'rgt' feasibility = [] feasibility_handa = [] jnts = [] jnts_handa = [] for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]): feasibility.append('False') feasibility_handa.append('False') jnts.append([]) jnts_handa.append([]) fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0] + self.floatinggripcontacts[fpid][i][1]) / 2 fpgsfgrcenternp = self.__base.pg.v3ToNp(fpgsfgrcenter) fpgsrotmat3np = self.__base.pg.mat3ToNp( hndrotmat4.getUpper3()) handa = -hndrotmat4.getRow3(2) # check the angle between handa and minus y minusworldy = Vec3(0, -1, 0) if Vec3(handa).angleDeg(minusworldy) < 60: msc = robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armid) if msc is not None: feasibility[-1] = 'True' jnts[-1] = dc.listToStr(msc) fpgsfgrcenternp_handa = self.__base.pg.v3ToNp( fpgsfgrcenter + handa * rethanda) msc_handa = robot.numikmsc(fpgsfgrcenternp_handa, fpgsrotmat3np, msc, armid) if msc_handa is not None: feasibility_handa[-1] = 'True' jnts_handa[-1] = dc.listToStr(msc_handa) self.feasibility_rgt.append(feasibility) self.feasibility_handa_rgt.append(feasibility_handa) self.jnts_rgt.append(jnts) self.jnts_handa_rgt.append(jnts_handa) ### left hand armid = 'lft' feasibility = [] feasibility_handa = [] jnts = [] jnts_handa = [] for i, hndrotmat4 in enumerate(self.floatinggripmat4s[fpid]): feasibility.append('False') feasibility_handa.append('False') jnts.append([]) jnts_handa.append([]) fpgsfgrcenter = (self.floatinggripcontacts[fpid][i][0] + self.floatinggripcontacts[fpid][i][1]) / 2 fpgsfgrcenternp = self.__base.pg.v3ToNp(fpgsfgrcenter) fpgsrotmat3np = self.__base.pg.mat3ToNp( hndrotmat4.getUpper3()) handa = -hndrotmat4.getRow3(2) # check the angle between handa and minus y plusworldy = Vec3(0, 1, 0) if Vec3(handa).angleDeg(plusworldy) < 60: msc = robot.numik(fpgsfgrcenternp, fpgsrotmat3np, armid) if msc is not None: feasibility[-1] = 'True' jnts[-1] = dc.listToStr(msc) fpgsfgrcenternp_handa = self.__base.pg.v3ToNp( fpgsfgrcenter + handa * rethanda) msc_handa = robot.numikmsc(fpgsfgrcenternp_handa, fpgsrotmat3np, msc, armid) if msc_handa is not None: feasibility_handa[-1] = 'True' jnts_handa[-1] = dc.listToStr(msc_handa) self.feasibility_lft.append(feasibility) self.feasibility_handa_lft.append(feasibility_handa) self.jnts_lft.append(jnts) self.jnts_handa_lft.append(jnts_handa) self.__saveIKtoDB(idrobot) print("IK is updated!") def loadFPGfromDB(self): """ for debug purpose load the floatingposes and their grasps from the database :return: author: weiwei date: 20170227 """ print("Loading FP and Gs to update floatinggripids...") self.gridsfloatingposemat4s = [] self.floatinggripids = [] self.floatinggripmat4s = [] self.floatinggripcontacts = [] self.floatinggripnormals = [] self.floatinggripjawwidth = [] self.floatinggripidfreeair = [] sql = "SELECT * FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \ AND object.name LIKE '%s'" % self.dbobjname fposesresult = self.gdb.execute(sql) if len(fposesresult) != 0: tic = time.time() for i, resultrow in enumerate(fposesresult): toc = time.time() print( "Loading floatinggripids", " Finished: ", i, "/", len(fposesresult), " Time past: ", "%.2f" % (toc - tic), "s", " Expected remaining time: ", "%.2f" % ((toc - tic) * len(fposesresult) / (i + 1e-4) - (toc - tic)), "s") self.gridsfloatingposemat4s.append(dc.strToMat4(resultrow[1])) idfloatingposes = resultrow[0] sql = "SELECT floatinggrips.idfloatinggrips, floatinggrips.contactpoint0, \ floatinggrips.contactpoint1, floatinggrips.contactnormal0, \ floatinggrips.contactnormal1, floatinggrips.rotmat, floatinggrips.jawwidth, \ floatinggrips.idfreeairgrip FROM floatinggrips,freeairgrip,hand WHERE \ floatinggrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \ freeairgrip.idhand = hand.idhand AND floatinggrips.idfloatingposes = %d AND \ hand.name = '%s'" % (idfloatingposes, self.handpkg.getHandName()) fgresult = self.gdb.execute(sql) if len(fgresult) != 0: floatinggripids = [] floatinggripmat4s = [] floatinggripcontacts = [] floatinggripnormals = [] floatinggripjawwidths = [] floatinggripidfreeairs = [] for fgresultrow in fgresult: cct0 = dc.strToV3(fgresultrow[1]) cct1 = dc.strToV3(fgresultrow[2]) cctn0 = dc.strToV3(fgresultrow[3]) cctn1 = dc.strToV3(fgresultrow[4]) floatinggripids.append(int(fgresultrow[0])) floatinggripmat4s.append(dc.strToMat4(fgresultrow[5])) floatinggripcontacts.append([cct0, cct1]) floatinggripnormals.append([cctn0, cctn1]) floatinggripjawwidths.append(float(fgresultrow[6])) floatinggripidfreeairs.append(int(fgresultrow[7])) self.floatinggripids.append(floatinggripids) self.floatinggripmat4s.append(floatinggripmat4s) self.floatinggripcontacts.append(floatinggripcontacts) self.floatinggripnormals.append(floatinggripnormals) self.floatinggripjawwidth.append(floatinggripjawwidths) self.floatinggripidfreeair.append(floatinggripidfreeairs) else: print('Plan floating grips first!') assert (False) else: assert ('No object found!') def transformGripsOnePose(self, rotmat4): """ transform the freeair grips to one specific rotmat4 :param rotmat4: :return: [floatinggripmat4s, floatinggripcontacts, floatinggripnormals, floatinggripjawwidths, floatinggripidfreeairs] each element in the list is also a list """ floatinggripmat4s = [] floatinggripcontacts = [] floatinggripnormals = [] floatinggripjawwidths = [] floatinggripidfreeairs = [] for i, gripmat4 in enumerate(self.freegriprotmats): floatinggripmat4 = gripmat4 * rotmat4 cct0 = rotmat4.xformPoint(self.freegripcontacts[i][0]) cct1 = rotmat4.xformPoint(self.freegripcontacts[i][1]) cctn0 = rotmat4.xformPoint(self.freegripnormals[i][0]) cctn1 = rotmat4.xformPoint(self.freegripnormals[i][1]) floatinggripjawwidth = self.freegripjawwidth[i] floatinggripidfreeair = self.freegripids[i] floatinggripmat4s.append(floatinggripmat4) floatinggripcontacts.append([cct0, cct1]) floatinggripnormals.append([cctn0, cctn1]) floatinggripjawwidths.append(floatinggripjawwidth) floatinggripidfreeairs.append(floatinggripidfreeair) return [ floatinggripmat4s, floatinggripcontacts, floatinggripnormals, floatinggripjawwidths, floatinggripidfreeairs ] def showIcomat4s(self, nodepath): """ show the pandamat4s generated by genPandaRotmat4 :param nodepath, where np to repreantTo, usually base.render :return: author: weiwei date: 20170221, tsukuba """ for i, icomat4list in enumerate(self.mat4list): vert = self.icos.vertices * 100 spos = Vec3(vert[i][0], vert[i][1], vert[i][2]) for icomat4 in icomat4list: self.__base.pg.plotAxis(nodepath, spos, icomat4, length=100, thickness=3) def loadIKfeasibleGPfromDB(self, robot): """ load the IK Feasible handover pairs :return: author: weiwei date: 20170301 """ self.loadFPGfromDB() self.loadIKFromDB(robot) idrobot = self.gdb.loadIdRobot(robot) idarmrgt = self.gdb.loadIdArm('rgt') idarmlft = self.gdb.loadIdArm('lft') self.floatinggrippairsids = [] self.floatinggrippairshndmat4s = [] self.floatinggrippairscontacts = [] self.floatinggrippairsnormals = [] self.floatinggrippairsjawwidths = [] self.floatinggrippairsidfreeairs = [] self.floatinggrippairsjnts = [] self.floatinggrippairsjnts_handa = [] for fpid in range(len(self.gridsfloatingposemat4s)): sql = "SELECT floatingposes.idfloatingposes FROM floatingposes, object WHERE floatingposes.idobject = object.idobject \ AND object.name LIKE '%s' AND floatingposes.rotmat LIKE '%s'" % \ (self.dbobjname, dc.mat4ToStr(self.gridsfloatingposemat4s[fpid])) result = self.gdb.execute(sql) floatinggrippairsids = [] floatinggrippairshndmat4s = [] floatinggrippairscontacts = [] floatinggrippairsnormals = [] floatinggrippairsjawwidths = [] floatinggrippairsidfreeairs = [] floatinggrippairsjnts = [] floatinggrippairsjnts_handa = [] if len(result) != 0: idfloatingposes = result[0][0] sql = "SELECT floatinggripspairs.idfloatinggrips0, floatinggripspairs.idfloatinggrips1, \ fg0.contactpoint0, fg0.contactpoint1, fg0.contactnormal0, fg0.contactnormal1, fg0.rotmat, \ fg0.jawwidth, fg0.idfreeairgrip, \ fg1.contactpoint0, fg1.contactpoint1, fg1.contactnormal0, fg1.contactnormal1, fg1.rotmat, \ fg1.jawwidth, fg1.idfreeairgrip, ikfg0.jnts, ikfg0.jnts_handa, \ ikfg1.jnts, ikfg1.jnts_handa FROM floatinggripspairs, floatinggrips fg0, floatinggrips fg1, \ ikfloatinggrips ikfg0, ikfloatinggrips ikfg1 WHERE \ floatinggripspairs.idfloatinggrips0 = fg0.idfloatinggrips AND \ floatinggripspairs.idfloatinggrips1 = fg1.idfloatinggrips AND \ fg0.idfloatingposes = %d AND fg1.idfloatingposes = %d AND \ fg0.idfloatinggrips = ikfg0.idfloatinggrips AND \ ikfg0.feasibility like 'True' AND ikfg0.feasibility_handa like 'True' AND \ ikfg0.idrobot = %d AND ikfg0.idarm = %d AND \ fg1.idfloatinggrips = ikfg1.idfloatinggrips AND ikfg1.feasibility like 'True' \ AND ikfg1.feasibility_handa like 'True' AND \ ikfg1.idrobot = %d AND ikfg1.idarm = %d" % \ (idfloatingposes, idfloatingposes, idrobot, idarmrgt, idrobot, idarmlft) result = self.gdb.execute(sql) if len(result) != 0: for resultrow in result: floatinggrippairsids.append( [resultrow[0], resultrow[1]]) floatinggrippairshndmat4s.append([ dc.strToMat4(resultrow[6]), dc.strToMat4(resultrow[13]) ]) rgtcct0 = dc.strToV3(resultrow[2]) rgtcct1 = dc.strToV3(resultrow[3]) lftcct0 = dc.strToV3(resultrow[9]) lftcct1 = dc.strToV3(resultrow[10]) floatinggrippairscontacts.append([[rgtcct0, rgtcct1], [lftcct0, lftcct1]]) rgtcctn0 = dc.strToV3(resultrow[4]) rgtcctn1 = dc.strToV3(resultrow[5]) lftcctn0 = dc.strToV3(resultrow[11]) lftcctn1 = dc.strToV3(resultrow[12]) floatinggrippairsnormals.append([[rgtcctn0, rgtcctn1], [lftcctn0, lftcctn1]]) floatinggrippairsjawwidths.append( [float(resultrow[7]), float(resultrow[14])]) floatinggrippairsidfreeairs.append( [int(resultrow[8]), int(resultrow[15])]) floatinggrippairsjnts.append([ dc.strToList(resultrow[16]), dc.strToList(resultrow[18]) ]) floatinggrippairsjnts_handa.append([ dc.strToList(resultrow[17]), dc.strToList(resultrow[19]) ]) self.floatinggrippairsids.append(floatinggrippairsids) self.floatinggrippairshndmat4s.append(floatinggrippairshndmat4s) self.floatinggrippairscontacts.append(floatinggrippairscontacts) self.floatinggrippairsnormals.append(floatinggrippairsnormals) self.floatinggrippairsjawwidths.append(floatinggrippairsjawwidths) self.floatinggrippairsidfreeairs.append( floatinggrippairsidfreeairs) self.floatinggrippairsjnts.append(floatinggrippairsjnts) self.floatinggrippairsjnts_handa.append( floatinggrippairsjnts_handa) def loadIKFromDB(self, robot): """ load the feasibility of IK from db :param robot: :return: author: weiwei date: 20170301 """ idrobot = self.gdb.loadIdRobot(robot) self.feasibility_rgt = [] self.feasibility_handa_rgt = [] self.feasibility_lft = [] self.feasibility_handa_lft = [] for fpid in range(len(self.gridsfloatingposemat4s)): # right arm feasibility = [] feasibility_handa = [] idarm = self.gdb.loadIdArm('rgt') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): feasibility.append('False') feasibility_handa.append('False') sql = "SELECT feasibility, feasibility_handa FROM ikfloatinggrips WHERE \ idrobot = %d AND idarm = %d and idfloatinggrips = %d" % ( idrobot, idarm, idfloatinggrips) result = self.gdb.execute(sql) if len(result) != 0: feasibility[i] = result[0][0] feasibility_handa[i] = result[0][1] self.feasibility_rgt.append(feasibility) self.feasibility_handa_rgt.append(feasibility_handa) # left arm feasibility = [] feasibility_handa = [] idarm = self.gdb.loadIdArm('lft') for i, idfloatinggrips in enumerate(self.floatinggripids[fpid]): feasibility.append('False') feasibility_handa.append('False') sql = "SELECT feasibility, feasibility_handa FROM ikfloatinggrips WHERE \ idrobot = %d AND idarm = %d and idfloatinggrips = %d" % ( idrobot, idarm, idfloatinggrips) result = self.gdb.execute(sql) if len(result) != 0: feasibility[i] = result[0][0] feasibility_handa[i] = result[0][1] self.feasibility_lft.append(feasibility) self.feasibility_handa_lft.append(feasibility_handa) def plotOneFPandG(self, parentnp, fpid=0): """ plot the objpose and grasps at a specific rotmat4 :param fpid: the index of self.floatingposemat4 :return: author: weiwei date: 20170221, tsukuba """ objnp = self.__base.pg.packpandanp_fn(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) objnp.setMat(self.gridsfloatingposemat4s[fpid]) objnp.reparentTo(parentnp) for i, hndrotmat4 in enumerate(self.gridsfloatingposemat4s[fpid]): if i == 7: # show grasps hand0 = self.handpkg.newHandNM(hndcolor=[0, 1, 0, 1]) hand0.setMat(pandanpmat4=hndrotmat4) hand0.setJawwidth(self.floatinggripjawwidth[fpid][i]) hand0.reparentTo(parentnp) print(self.handpairList) for handidpair in self.handpairList: if handidpair[0] == self.floatinggripidfreeair[fpid][i]: pairedilist = [ i1 for i1 in range( len(self.floatinggripidfreeair[fpid])) if self.floatinggripidfreeair[fpid][i1] == handidpair[1] ] print(pairedilist) i1 = pairedilist[0] # if self.feasibility_lft[fpid][i1] == 'True': hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1]) hndrotmat4 = self.floatinggripmat4s[fpid][i1] hand1.setMat(pandanpmat4=hndrotmat4) hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1]) hand1.reparentTo(parentnp) if handidpair[1] == self.floatinggripidfreeair[fpid][i]: pairedilist = [ i1 for i1 in range( len(self.floatinggripidfreeair[fpid])) if self.floatinggripidfreeair[fpid][i1] == handidpair[0] ] print(pairedilist) i1 = pairedilist[0] # if self.feasibility_lft[fpid][i1] == 'True': hand1 = self.handpkg.newHandNM(hndcolor=[0, 1, 1, 1]) hndrotmat4 = self.floatinggripmat4s[fpid][i1] hand1.setMat(pandanpmat4=hndrotmat4) hand1.setJawwidth(self.floatinggripjawwidth[fpid][i1]) hand1.reparentTo(parentnp) def plotOneFPandGPairs(self, parentnp, fpid=0): """ plot the gpairss at a specific rotmat4 :param fpid: the index of self.floatingposemat4 :return: author: weiwei date: 20170301, tsukuba """ objnp = self.__base.pg.packpandanp_fn(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces) objnp.setMat(self.gridsfloatingposemat4s[fpid]) objnp.setColor(Vec4(.7, 0.3, 0, 1)) objnp.reparentTo(parentnp) print(self.floatinggrippairshndmat4s[fpid]) for i, hndrotmat4pair in enumerate( self.floatinggrippairshndmat4s[fpid]): # if i == 9: # show grasps hand0 = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .5]) hand0mat4 = Mat4(hndrotmat4pair[0]) # h0row3 = hand0mat4.getRow3(3) # h0row3[2] = h0row3[2]+i*20.0 # hand0mat4.setRow(3, h0row3[2]) hand0.setMat(pandanpmat4=hand0mat4) hand0.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][0]) hand0.reparentTo(parentnp) hand1 = self.handpkg.newHandNM(hndcolor=[0, .0, 1, .5]) hand1mat4 = Mat4(hndrotmat4pair[1]) # h1row3 = hand1mat4.getRow3(3) # h1row3[2] = h1row3[2]+i*20.0 # hand1mat4.setRow(3, h1row3[2]) hand1.setMat(pandanpmat4=hand1mat4) hand1.setJawwidth(self.floatinggrippairsjawwidths[fpid][i][1]) hand1.reparentTo(parentnp)
class EccoGame(ShowBase): def __init__(self): ShowBase.__init__(self) base.setBackgroundColor(0, 0, 0) game_title = "ECCO" titleN = TextNode('game-title') font = loader.loadFont("font/Caveman.ttf") titleN.setFont(font) titleN.setText(game_title) titleN.setTextColor(1, 1, 1, 1) titleN.setSlant(0.1) titleN.setShadow(0.05) titleN.setShadowColor(0, 0, 200, 1) titleN.setFrameColor(0, 0, 255, 1) titleN.setFrameLineWidth(5.0) textNodePath = self.aspect2d.attachNewNode(titleN) textNodePath.setPos(-0.4, 1.5, 0.5) textNodePath.setScale(0.2) self.level1Button = DirectButton(text=("Level 1"), scale=.1, pos=(0, 0, 0.2), command=self.level1) self.level2Button = DirectButton(text=("Level 2"), scale=.1, pos=(0, 0, 0), command=self.level2) def level1(self): titleNp = self.aspect2d.find('game-title') titleNp.removeNode() self.level1Button.destroy() self.level2Button.destroy() self.sizescale = 0.6 self.setupWorld() self.setupSky() self.setupFloor() self.setupCharacter() self.inst1 = addInstructions(0.95, "[ESC]: Quit") self.inst2 = addInstructions(0.90, "[Left key]: Turn Ecco Left") self.inst3 = addInstructions(0.85, "[Right key]: Turn Ecco Right") self.inst4 = addInstructions(0.80, "[Up key]: Jump Ecco") inputState.watchWithModifiers('esc', 'escape') inputState.watchWithModifiers('w', 'w') inputState.watchWithModifiers('arrow_left', 'arrow_left') inputState.watchWithModifiers('arrow_right', 'arrow_right') inputState.watchWithModifiers('pause', 'p') inputState.watchWithModifiers('space', 'space') inputState.watchWithModifiers('arrow_up', 'arrow_up') inputState.watchWithModifiers('cam-left', 'z') inputState.watchWithModifiers('cam-right', 'x') inputState.watchWithModifiers('cam-forward', 'c') inputState.watchWithModifiers('cam-backward', 'v') taskMgr.add(self.update, "update") # Game state variables self.isMoving = False # display framerate self.setFrameRateMeter(True) # Set up the camera self.disableMouse() self.camera.setPos(self.characterNP.getX(), self.characterNP.getY() - 30, 4) self.setupSound() # coins variables self.coinsCollected = 0 self.dictOfCoins = {} self.coins = [] # Set up Coins as Collectables self.setupCoins() # Set up Obstacles self.setupObstacles() # Setup Level Display self.setupLevelDisplay() self.counter = 0 def setupLevelDisplay(self): LEVEL_1 = "Level 1" levelDisplay(LEVEL_1) levelN = TextNode('level-display') levelN.setText(LEVEL_1) font = loader.loadFont("font/Caveman.ttf") levelN.setFont(font) levelN.setTextColor(1, 1, 1, 1) levelN.setSlant(0.1) levelN.setShadow(0.05) levelN.setShadowColor(255, 0, 0, 1) textNodePath = self.aspect2d.attachNewNode(levelN) textNodePath.setPos(-0.45, 0, 0) textNodePath.setScale(0.2) def update(self, task): dt = globalClock.getDt() self.pos = self.characterNP.getPos() self.counter = self.counter + 1 if self.counter == 150: levelNp = self.aspect2d.find('level-display') levelNp.removeNode() self.setUpCamera() self.processInput(dt) self.processContacts() self.coinScoreDisplay() self.checkIfEccoDied() self.world.doPhysics(dt, 10, 1 / 230.0) return task.cont def setupWorld(self): # create bullet world self.debugNP = self.render.attachNewNode(BulletDebugNode('Debug')) #self.debugNP.show() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) def setupSky(self): self.milkyWayNp = render.attachNewNode('milkyway') self.milkyWay_2Np = render.attachNewNode('milkyway_2') self.marsNp = render.attachNewNode('mars') self.sunNp = render.attachNewNode('sun') # Load the model for the sky # self.sky = loader.loadModel("models/sky/solar_sky_sphere") self.sky = loader.loadModel("models/sky/solar_sky_sphere") # Load the texture for the sky. self.sky_tex = loader.loadTexture("models/sky/stars_1k_tex.jpg") # Set the sky texture to the sky model self.sky.setTexture(self.sky_tex, 1) # Parent the sky model to the render node so that the sky is rendered self.sky.reparentTo(self.render) # Scale the size of the sky. self.sky.setScale(15000) x = 0.005 y = 1700.0 z = 0.0 self.sky.setPos(x, y, 0) # #milkyway 1 self.milkyWay = loader.loadModel("models/sky/planet_sphere") self.milkWay_tex = loader.loadTexture("models/sky/milkyway_tex.jpg") self.milkyWay.setTexture(self.milkWay_tex, 1) self.milkyWay.reparentTo(self.milkyWayNp) self.milkyWay.setScale(200) self.milkyWay.setPos(x + 2000, y + 10000, z - 500) # milkyway 2 self.milkyWay_2 = loader.loadModel("models/sky/planet_sphere") self.milkWay_2_tex = loader.loadTexture("models/sky/milkyway_2_tex.jpg") self.milkyWay_2.setTexture(self.milkWay_2_tex, 1) self.milkyWay_2.reparentTo(self.milkyWay_2Np) self.milkyWay_2.setScale(400) self.milkyWay_2.setPos(x - 3000, y + 10000, z + 500) # sun self.sun = loader.loadModel("models/sky/planet_sphere") self.sun_tex = loader.loadTexture("models/sky/sun_2_tex.jpg") self.sun.setTexture(self.sun_tex, 1) self.sun.reparentTo(self.sunNp) self.sun.setScale(600) self.sun.setPos(x + 1000, y + 10000, z + 1000) # # Load Mars self.mars = loader.loadModel("models/sky/planet_sphere") self.mars_tex = loader.loadTexture("models/sky/mars_1k_tex.jpg") self.mars.setTexture(self.mars_tex, 1) self.mars.reparentTo(self.marsNp) self.mars.setScale(200) self.mars.setPos(x + 3000, y + 10000, z + 500) def setupSound(self): # Set up sound mySound = base.loader.loadSfx("sounds/Farm Morning.ogg") mySound.play() mySound.setVolume(3.0) mySound.setLoop(True) footsteps = base.loader.loadSfx("sounds/Footsteps_on_Cement-Tim_Fryer.wav") footsteps.play() footsteps.setVolume(0.8) footsteps.setLoop(True) self.jumpSound = base.loader.loadSfx("sounds/Jump-SoundBible.com-1007297584.wav") self.jumpSound.setVolume(0.2) self.collectSound = base.loader.loadSfx("sounds/pin_dropping-Brian_Rocca-2084700791.wav") self.gameOverSound = base.loader.loadSfx("sounds/Bike Horn-SoundBible.com-602544869.wav") self.levelCompleteSound = base.loader.loadSfx("sounds/Ta Da-SoundBible.com-1884170640.wav") def setupFloor(self): size = Vec3(7.5, 3000, 1.81818) shape = BulletBoxShape(size * 0.55) # shape = BulletPlaneShape(Vec3(0, 0, 1), 0) node = BulletRigidBodyNode('Box-Floor') node.addShape(shape) node.setMass(0) stairNP = self.render.attachNewNode(node) stairNP.setPos(0, 0, 0) stairNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(stairNP.node()) modelNP = loader.loadModel('models/box.egg') modelNP.reparentTo(stairNP) modelNP.setPos(-size.x / 2.0, -size.y / 2.0, -size.z / 2.0) modelNP.setScale(size) def setupCharacter(self): # Character h = 1.75 w = 0.4 shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.character = BulletCharacterControllerNode(shape, 0.4, 'Player') self.character.setGravity(35) self.characterNP = self.render.attachNewNode(self.character) self.characterNP.setPos(0, 10, 5) self.characterNP.setCollideMask(BitMask32.allOn()) self.world.attachCharacter(self.character) self.ecco = Actor('ralph-models/ralph.egg.pz', { 'run': 'ralph-models/ralph-run.egg', 'jump': 'ralph/ralph-run.egg', 'damage': 'models/lack-damage.egg'}) self.ecco.reparentTo(self.characterNP) self.ecco.setScale(0.7048) self.ecco.setH(180) def setUpCamera(self): # If the camera is too far from ecco, move it closer. # If the camera is too close to ecco, move it farther. camvec = self.characterNP.getPos() - self.camera.getPos() camvec.setZ(0.0) camdist = camvec.length() camvec.normalize() if camdist > 10.0: self.camera.setPos(self.camera.getPos() + camvec * (camdist - 40)) camdist = 10.0 if camdist < 5.0: self.camera.setPos(self.camera.getPos() - camvec * (35 - camdist)) camdist = 5.0 def processInput(self, dt): speed = Vec3(0, 0, 0) if inputState.isSet('esc'): sys.exit() if inputState.isSet('w'): speed.setY(35.0) if inputState.isSet('arrow_left'): speed.setX(-35.0) if inputState.isSet('arrow_right'): speed.setX(35.0) if inputState.isSet('space'): self.jump() self.jumpSound.play() if inputState.isSet('arrow_up'): self.jump() self.jumpSound.play() if inputState.isSet('cam-left'): self.camera.setX(self.camera, -20 * dt) if inputState.isSet('cam-right'): self.camera.setX(self.camera, +20 * dt) if inputState.isSet('cam-forward'): self.camera.setY(self.camera, -200 * dt) if inputState.isSet('cam-backward'): self.camera.setY(self.camera, +200 * dt) # Make Ecco run if self.isMoving is False: self.ecco.loop("run") self.isMoving = True if self.pos.getY() > 1450.0: speed.setY(0.0) else: speed.setY(40.0) self.character.setLinearMovement(speed, True) def jump(self): self.character.setMaxJumpHeight(3.0) self.character.setJumpSpeed(25.0) self.character.doJump() def setupCoins(self): # display coins = 0 textN = TextNode('coin-score') textN.setText(str("Coins: " + str(self.coinsCollected))) textN.setSlant(0.1) textNodePath = self.aspect2d.attachNewNode(textN) textNodePath.setPos(0, 0.95, 0.9) textNodePath.setScale(0.08) randNum = random.sample(range(0, 1500, 200), 6) # coins for i in range(6): randX = random.uniform(-3.0, 3.2) randY = float(randNum[i]) shape = BulletSphereShape(0.3) coinNode = BulletGhostNode('Coin-' + str(i)) coinNode.addShape(shape) np = self.render.attachNewNode(coinNode) np.setCollideMask(BitMask32.allOff()) np.setPos(randX, randY, 2) # Adding sphere model sphereNp = loader.loadModel('models/smiley.egg') sphereNp_tex = loader.loadTexture("models/sky/coin_2_tex.jpg") sphereNp.setTexture(sphereNp_tex, 1) sphereNp.reparentTo(np) sphereNp.setScale(0.45) sphereNp.hprInterval(2.5, Vec3(360, 0, 0)).loop() self.world.attachGhost(coinNode) self.coins.append(coinNode) print "node name:" + str(coinNode.getName()) def processContacts(self): for coin in self.coins: self.testWithSingleBody(coin) self.coinsCollected = len(self.dictOfCoins) def testWithSingleBody(self, secondNode): contactResult = self.world.contactTestPair(self.character, secondNode) if contactResult.getNumContacts() > 0: self.collectSound.play() for contact in contactResult.getContacts(): cp = contact.getManifoldPoint() node0 = contact.getNode0() node1 = contact.getNode1() self.dictOfCoins[node1.getName()] = 1 np = self.render.find(node1.getName()) np.node().removeAllChildren() self.world.removeGhost(np.node()) def setupObstacles(self): # Obstacle origin = Point3(2, 0, 0) size = Vec3(2, 2.75, 1.5) shape = BulletBoxShape(size * 0.55) randNum1 = random.sample(range(0, 1500, 300), 3) randNum2 = random.sample(range(0, 1500, 500), 3) for i in range(2): randX = random.uniform(-3.5, 3.5) randY = float(randNum1[i]) pos = origin + size * i ObstacleNP = self.render.attachNewNode(BulletRigidBodyNode('Obstacle%i' % i)) ObstacleNP.node().addShape(shape) ObstacleNP.node().setMass(1.0) ObstacleNP.setPos(randX, randY, 3) ObstacleNP.setCollideMask(BitMask32.allOn()) modelNP = loader.loadModel('models/box.egg') modelNP_tex = loader.loadTexture("models/sky/milkyway_tex.jpg") modelNP.setTexture(modelNP_tex, 1) modelNP.reparentTo(ObstacleNP) # modelNP.setPos(0, 0, 0) modelNP.setPos(-size.x / 2.0, -size.y / 2.0, -size.z / 2.0) modelNP.setScale(size) self.world.attachRigidBody(ObstacleNP.node()) size_2 = Vec3(3, 2.75, 1.5) shape2 = BulletBoxShape(size_2 * 0.55) for i in range(2): randX = random.uniform(-3.5, 3.5) randY = float(randNum2[i]) pos = origin + size_2 * i pos.setY(0) ObstacleNP = self.render.attachNewNode(BulletRigidBodyNode('ObstacleSmall%i' % i)) ObstacleNP.node().addShape(shape2) ObstacleNP.node().setMass(1.0) ObstacleNP.setPos(randX, randY, 2) ObstacleNP.setCollideMask(BitMask32.allOn()) modelNP = loader.loadModel('models/box.egg') modelNP_tex = loader.loadTexture("models/sky/moon_1k_tex.jpg") modelNP.setTexture(modelNP_tex, 1) modelNP.reparentTo(ObstacleNP) # modelNP.setPos(0, 0, 0) modelNP.setPos(-size_2.x / 2.0, -size_2.y / 2.0, -size_2.z / 2.0) modelNP.setScale(size_2) self.world.attachRigidBody(ObstacleNP.node()) def checkIfEccoDied(self): print "position" + str(self.pos.getY()) if self.pos.getZ() > -50.0 and self.pos.getZ() < 0.0: title = "Game Over" levelCompleteN = TextNode('ecco-died') font = loader.loadFont("font/Caveman.ttf") levelCompleteN.setFont(font) levelCompleteN.setText(title) levelCompleteN.setTextColor(1, 1, 1, 1) levelCompleteN.setSlant(0.1) levelCompleteN.setShadow(0.03) levelCompleteN.setShadowColor(0, 0, 200, 1) # levelN.setFrameAsMargin(0, 0, 0, 0) levelCompleteN.setFrameColor(200, 0, 0, 1) levelCompleteN.setFrameLineWidth(5.0) # textNp.node().setGlyphShift(1.0) textNodePath = self.aspect2d.attachNewNode(levelCompleteN) textNodePath.setPos(-0.9, 1.5, 0.5) textNodePath.setScale(0.2) if self.pos.getZ() < -49.0: self.gameOverSound.play() elif self.pos.getZ() < -50.0: if self.gameOverSound.status() != self.gameOverSound.PLAYING: sys.exit(1) elif self.pos.getY() > 1300.0: title = "Level 1 \n Complete" levelCompleteN = TextNode('level-complete') font = loader.loadFont("font/Caveman.ttf") levelCompleteN.setFont(font) levelCompleteN.setText(title) levelCompleteN.setTextColor(1, 1, 1, 1) levelCompleteN.setSlant(0.1) levelCompleteN.setShadow(0.03) levelCompleteN.setShadowColor(0, 0, 200, 1) # levelN.setFrameAsMargin(0, 0, 0, 0) levelCompleteN.setFrameColor(200, 0, 0, 1) levelCompleteN.setFrameLineWidth(5.0) # textNp.node().setGlyphShift(1.0) textNodePath = self.aspect2d.attachNewNode(levelCompleteN) textNodePath.setPos(-0.6, 1.5, 0.5) textNodePath.setScale(0.2) if self.levelCompleteSound.status() != self.levelCompleteSound.PLAYING: self.levelCompleteSound.play() else: pass def coinScoreDisplay(self): textNp = self.aspect2d.find('coin-score') textNp.node().clearText() textNp.node().setText(str("Coins: " + str(self.coinsCollected))) #Level 2 def level2(self): titleNp2 = self.aspect2d.find('game-title') titleNp2.removeNode() self.level1Button.destroy() self.level2Button.destroy() self.sizescale2 = 0.6 self.setupWorld2() self.setupSky2() self.setupFloor2() self.setupCharacter2() # self.title = addTitle(" ") self.inst12 = addInstructions(0.95, "[ESC]: Quit") self.inst22 = addInstructions(0.90, "[Left key]: Turn Ecco Left") self.inst32 = addInstructions(0.85, "[Right key]: Turn Ecco Right") self.inst42 = addInstructions(0.80, "[Up key]: Jump Ecco") inputState.watchWithModifiers('esc', 'escape') inputState.watchWithModifiers('w', 'w') inputState.watchWithModifiers('arrow_left', 'arrow_left') inputState.watchWithModifiers('arrow_right', 'arrow_right') inputState.watchWithModifiers('pause', 'p') inputState.watchWithModifiers('space', 'space') inputState.watchWithModifiers('arrow_up', 'arrow_up') inputState.watchWithModifiers('cam-left', 'z') inputState.watchWithModifiers('cam-right', 'x') inputState.watchWithModifiers('cam-forward', 'c') inputState.watchWithModifiers('cam-backward', 'v') taskMgr.add(self.update2, "update") # Game state variables self.isMoving2 = False # display framerate self.setFrameRateMeter(True) # Set up the camera self.disableMouse() self.camera.setPos(self.characterNP2.getX(), self.characterNP2.getY() - 30, 4) self.setupSound2() # coins variables self.coinsCollected2 = 0 self.dictOfCoins2 = {} self.coins2 = [] # Set up Coins as Collectables self.setupCoins2() # Set up Floaters with coins self.setupFloaters2() # Set up Obstacles self.setupObstacles2() # Setup Level Display self.setupLevelDisplay2() self.counter2 = 0 def setupLevelDisplay2(self): LEVEL_2 = "Level 2" levelDisplay(LEVEL_2) levelN = TextNode('level-display') levelN.setText(LEVEL_2) # www.webpagepublicity.com font = loader.loadFont("font/Caveman.ttf") levelN.setFont(font) levelN.setTextColor(1, 1, 1, 1) levelN.setSlant(0.1) levelN.setShadow(0.05) levelN.setShadowColor(255, 0, 0, 1) # levelN.setFrameAsMargin(0, 0, 0, 0) # levelN.setFrameColor(0, 0, 255, 1) # levelN.setFrameLineWidth(5.0) # # textNp.node().setGlyphShift(1.0) textNodePath = self.aspect2d.attachNewNode(levelN) textNodePath.setPos(-0.45, 0, 0) textNodePath.setScale(0.2) def update2(self, task): dt = globalClock.getDt() self.pos2 = self.characterNP2.getPos() self.counter2 = self.counter2 + 1 if self.counter2 == 150: levelNp = self.aspect2d.find('level-display') levelNp.removeNode() self.setUpCamera2() self.processInput2(dt) self.processContacts2() self.coinScoreDisplay2() self.checkIfEccoDied2() self.world2.doPhysics(dt, 10, 1 / 230.0) return task.cont def setupWorld2(self): # create bullet world self.debugNP = self.render.attachNewNode(BulletDebugNode('Debug')) #self.debugNP.show() self.world2 = BulletWorld() self.world2.setGravity(Vec3(0, 0, -9.81)) self.world2.setDebugNode(self.debugNP.node()) def setupSky2(self): self.milkyWayNp = render.attachNewNode('milkyway') self.milkyWay_2Np = render.attachNewNode('milkyway_2') self.marsNp = render.attachNewNode('mars') self.sunNp = render.attachNewNode('sun') # Load the model for the sky # self.sky = loader.loadModel("models/sky/solar_sky_sphere") self.sky = loader.loadModel("models/sky/solar_sky_sphere") # Load the texture for the sky. self.sky_tex = loader.loadTexture("models/sky/stars_1k_tex.jpg") # Set the sky texture to the sky model self.sky.setTexture(self.sky_tex, 1) # Parent the sky model to the render node so that the sky is rendered self.sky.reparentTo(self.render) # Scale the size of the sky. self.sky.setScale(15000) x = 0.005 y = 1700.0 z = 0.0 self.sky.setPos(x, y, 0) # #milkyway 1 self.milkyWay = loader.loadModel("models/sky/planet_sphere") self.milkWay_tex = loader.loadTexture("models/sky/milkyway_tex.jpg") self.milkyWay.setTexture(self.milkWay_tex, 1) self.milkyWay.reparentTo(self.milkyWayNp) self.milkyWay.setScale(200) self.milkyWay.setPos(x + 2000, y + 10000, z - 500) # milkyway 2 self.milkyWay_2 = loader.loadModel("models/sky/planet_sphere") self.milkWay_2_tex = loader.loadTexture("models/sky/milkyway_2_tex.jpg") self.milkyWay_2.setTexture(self.milkWay_2_tex, 1) self.milkyWay_2.reparentTo(self.milkyWay_2Np) self.milkyWay_2.setScale(400) self.milkyWay_2.setPos(x - 3000, y + 10000, z + 500) # sun self.sun = loader.loadModel("models/sky/planet_sphere") self.sun_tex = loader.loadTexture("models/sky/sun_2_tex.jpg") self.sun.setTexture(self.sun_tex, 1) self.sun.reparentTo(self.sunNp) self.sun.setScale(600) self.sun.setPos(x + 1000, y + 10000, z + 1000) # # Load Mars self.mars = loader.loadModel("models/sky/planet_sphere") self.mars_tex = loader.loadTexture("models/sky/mars_1k_tex.jpg") self.mars.setTexture(self.mars_tex, 1) self.mars.reparentTo(self.marsNp) self.mars.setScale(200) self.mars.setPos(x + 3000, y + 10000, z + 500) def setUpCamera2(self): # If the camera is too far from ecco, move it closer. # If the camera is too close to ecco, move it farther. camvec = self.characterNP2.getPos() - self.camera.getPos() camvec.setZ(0.0) camdist = camvec.length() camvec.normalize() if camdist > 10.0: self.camera.setPos(self.camera.getPos() + camvec * (camdist - 40)) camdist = 10.0 if camdist < 5.0: self.camera.setPos(self.camera.getPos() - camvec * (35 - camdist)) camdist = 5.0 def setupSound2(self): # Set up sound mySound = base.loader.loadSfx("sounds/Farm Morning.ogg") mySound.play() mySound.setVolume(3.0) mySound.setLoop(True) footsteps = base.loader.loadSfx("sounds/Footsteps_on_Cement-Tim_Fryer.wav") footsteps.play() footsteps.setVolume(0.8) footsteps.setLoop(True) self.jumpSound2 = base.loader.loadSfx("sounds/Jump-SoundBible.com-1007297584.wav") self.jumpSound2.setVolume(0.2) self.collectSound2 = base.loader.loadSfx("sounds/pin_dropping-Brian_Rocca-2084700791.wav") self.gameOverSound2 = base.loader.loadSfx("sounds/Bike Horn-SoundBible.com-602544869.wav") self.levelCompleteSound2 = base.loader.loadSfx("sounds/Ta Da-SoundBible.com-1884170640.wav") def setupFloor2(self): size = Vec3(7.5, 3000, 1.81818) shape = BulletBoxShape(size * 0.55) # shape = BulletPlaneShape(Vec3(0, 0, 1), 0) node = BulletRigidBodyNode('Box-Floor') node.addShape(shape) node.setMass(0) stairNP = self.render.attachNewNode(node) stairNP.setPos(0, 0, 0) stairNP.setCollideMask(BitMask32.allOn()) self.world2.attachRigidBody(stairNP.node()) modelNP = loader.loadModel('models/box.egg') modelNP.reparentTo(stairNP) modelNP.setPos(-size.x / 2.0, -size.y / 2.0, -size.z / 2.0) modelNP.setScale(size) def setupCharacter2(self): # Character h = 1.75 w = 0.4 shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.character2 = BulletCharacterControllerNode(shape, 0.4, 'Player') self.character2.setGravity(35) self.characterNP2 = self.render.attachNewNode(self.character2) self.characterNP2.setPos(0, 10, 5) self.characterNP2.setCollideMask(BitMask32.allOn()) self.world2.attachCharacter(self.character2) self.ecco2 = Actor('ralph-models/ralph.egg.pz', { 'run': 'ralph-models/ralph-run.egg', 'jump': 'ralph/ralph-run.egg', 'damage': 'models/lack-damage.egg'}) self.ecco2.reparentTo(self.characterNP2) self.ecco2.setScale(0.7048) self.ecco2.setH(180) def processInput2(self, dt): speed = Vec3(0, 0, 0) if inputState.isSet('esc'): sys.exit() if inputState.isSet('w'): speed.setY(35.0) if inputState.isSet('arrow_left'): speed.setX(-35.0) if inputState.isSet('arrow_right'): speed.setX(35.0) if inputState.isSet('space'): self.jump2() self.jumpSound2.play() if inputState.isSet('arrow_up'): self.jump2() self.jumpSound2.play() if inputState.isSet('cam-left'): self.camera.setX(self.camera, -20 * dt) if inputState.isSet('cam-right'): self.camera.setX(self.camera, +20 * dt) if inputState.isSet('cam-forward'): self.camera.setY(self.camera, -200 * dt) if inputState.isSet('cam-backward'): self.camera.setY(self.camera, +200 * dt) # Make Ecco run if self.isMoving2 is False: self.ecco2.loop("run") self.isMoving2 = True if self.pos2.getY() > 1450.0: speed.setY(0.0) else: speed.setY(40.0) self.character2.setLinearMovement(speed, True) def jump2(self): self.character2.setMaxJumpHeight(3.0) self.character2.setJumpSpeed(25.0) self.character2.doJump() def setupCoins2(self): # display coins = 0 textN = TextNode('coin-score') textN.setText(str("Coins: " + str(self.coinsCollected2))) textN.setSlant(0.1) textNodePath = self.aspect2d.attachNewNode(textN) textNodePath.setPos(0, 0.95, 0.9) textNodePath.setScale(0.08) randNum = random.sample(range(0, 1500, 200), 6) # coins for i in range(6): randX = random.uniform(-3.0, 3.2) randY = float(randNum[i]) shape = BulletSphereShape(0.3) coinNode = BulletGhostNode('Coin-' + str(i)) coinNode.addShape(shape) np = self.render.attachNewNode(coinNode) np.setCollideMask(BitMask32.allOff()) np.setPos(randX, randY, 2) # Adding sphere model sphereNp = loader.loadModel('models/smiley.egg') sphereNp_tex = loader.loadTexture("models/sky/coin_2_tex.jpg") sphereNp.setTexture(sphereNp_tex, 1) sphereNp.reparentTo(np) sphereNp.setScale(0.45) sphereNp.hprInterval(2.5, Vec3(360, 0, 0)).loop() self.world2.attachGhost(coinNode) self.coins2.append(coinNode) print "node name:" + str(coinNode.getName()) def setupObstacles2(self): # Obstacle origin = Point3(2, 0, 0) size = Vec3(2, 2.75, 1.5) shape = BulletBoxShape(size * 0.55) randNum1 = random.sample(range(0, 1500, 300), 3) randNum2 = random.sample(range(0, 1500, 500), 3) for i in range(2): randX = random.uniform(-3.5, 3.5) randY = float(randNum1[i]) pos = origin + size * i ObstacleNP = self.render.attachNewNode(BulletRigidBodyNode('Obstacle%i' % i)) ObstacleNP.node().addShape(shape) ObstacleNP.node().setMass(1.0) ObstacleNP.setPos(randX, randY, 3) ObstacleNP.setCollideMask(BitMask32.allOn()) modelNP = loader.loadModel('models/box.egg') modelNP_tex = loader.loadTexture("models/sky/milkyway_tex.jpg") modelNP.setTexture(modelNP_tex, 1) modelNP.reparentTo(ObstacleNP) # modelNP.setPos(0, 0, 0) modelNP.setPos(-size.x / 2.0, -size.y / 2.0, -size.z / 2.0) modelNP.setScale(size) self.world2.attachRigidBody(ObstacleNP.node()) size_2 = Vec3(3, 2.75, 1.5) shape2 = BulletBoxShape(size_2 * 0.55) for i in range(2): randX = random.uniform(-3.5, 3.5) randY = float(randNum2[i]) pos = origin + size_2 * i pos.setY(0) ObstacleNP = self.render.attachNewNode(BulletRigidBodyNode('ObstacleSmall%i' % i)) ObstacleNP.node().addShape(shape2) ObstacleNP.node().setMass(1.0) ObstacleNP.setPos(randX, randY, 2) ObstacleNP.setCollideMask(BitMask32.allOn()) modelNP = loader.loadModel('models/box.egg') modelNP_tex = loader.loadTexture("models/sky/moon_1k_tex.jpg") modelNP.setTexture(modelNP_tex, 1) modelNP.reparentTo(ObstacleNP) # modelNP.setPos(0, 0, 0) modelNP.setPos(-size_2.x / 2.0, -size_2.y / 2.0, -size_2.z / 2.0) modelNP.setScale(size_2) self.world2.attachRigidBody(ObstacleNP.node()) def setupFloaters2(self): size = Vec3(3.5, 5.5, 0.3) randNum = random.sample(range(10, 1500, 500), 3) for i in range(3): randX = random.randrange(-2, 3, 10) randY = float(randNum[i]) # randY = random.randint(1000, 1500) shape = BulletBoxShape(size * 0.55) node = BulletRigidBodyNode('Floater') node.setMass(0) node.addShape(shape) np = self.render.attachNewNode(node) # np.setPos(9, 30, 3) np.setPos(randX, randY, 6) np.setR(0) self.world2.attachRigidBody(node) dummyNp = self.render.attachNewNode('milkyway') dummyNp.setPos(randX, randY, 6) modelNP = loader.loadModel('models/box.egg') modelNP_tex = loader.loadTexture("models/sky/moon_tex.jpg") modelNP.setTexture(modelNP_tex, 1) modelNP.reparentTo(dummyNp) modelNP.setPos(-1, 0, -1) modelNP.setPos(-size.x / 2.0, -size.y / 2.0, -size.z / 2.0) modelNP.setScale(size) dummyNp.hprInterval(2.5, Vec3(360, 0, 0)).loop() # Put A Coin On the Floater shape = BulletSphereShape(0.75) coinNode = BulletGhostNode('FloaterCoin-' + str(i)) coinNode.addShape(shape) np = self.render.attachNewNode(coinNode) np.setCollideMask(BitMask32.allOff()) # np.setPos(randX, randY, 2) np.setPos(randX, randY, 7.0) # Adding sphere model sphereNp = loader.loadModel('models/smiley.egg') sphereNp_tex = loader.loadTexture("models/sky/coin_2_tex.jpg") sphereNp.setTexture(sphereNp_tex, 1) sphereNp.reparentTo(np) sphereNp.setScale(0.85) sphereNp.hprInterval(1.5, Vec3(360, 0, 0)).loop() self.world2.attachGhost(coinNode) self.coins2.append(coinNode) print "node name:" + str(coinNode.getName()) def processContacts2(self): for coin in self.coins2: self.testWithSingleBody2(coin) self.coinsCollected2 = len(self.dictOfCoins2) def testWithSingleBody2(self, secondNode): contactResult = self.world2.contactTestPair(self.character2, secondNode) if contactResult.getNumContacts() > 0: self.collectSound2.play() for contact in contactResult.getContacts(): cp = contact.getManifoldPoint() node0 = contact.getNode0() node1 = contact.getNode1() self.dictOfCoins2[node1.getName()] = 1 np = self.render.find(node1.getName()) np.node().removeAllChildren() self.world2.removeGhost(np.node()) def checkIfEccoDied2(self): print "position" + str(self.pos2.getY()) if self.pos2.getZ() > -50.0 and self.pos2.getZ() < 0.0: title = "Game Over" levelCompleteN = TextNode('ecco-died') font = loader.loadFont("font/Caveman.ttf") levelCompleteN.setFont(font) levelCompleteN.setText(title) levelCompleteN.setTextColor(1, 1, 1, 1) levelCompleteN.setSlant(0.1) levelCompleteN.setShadow(0.03) levelCompleteN.setShadowColor(0, 0, 200, 1) # levelN.setFrameAsMargin(0, 0, 0, 0) levelCompleteN.setFrameColor(200, 0, 0, 1) levelCompleteN.setFrameLineWidth(5.0) # textNp.node().setGlyphShift(1.0) textNodePath = self.aspect2d.attachNewNode(levelCompleteN) textNodePath.setPos(-0.9, 1.5, 0.5) textNodePath.setScale(0.2) if self.pos2.getZ() < -49.0: self.gameOverSound2.play() elif self.pos2.getZ() < -50.0: if self.gameOverSound2.status() != self.gameOverSound2.PLAYING: sys.exit(1) elif self.pos2.getY() > 1300.0: title = "Level 2 \n Complete" levelCompleteN = TextNode('level-complete') font = loader.loadFont("font/Caveman.ttf") levelCompleteN.setFont(font) levelCompleteN.setText(title) levelCompleteN.setTextColor(1, 1, 1, 1) levelCompleteN.setSlant(0.1) levelCompleteN.setShadow(0.03) levelCompleteN.setShadowColor(0, 0, 200, 1) # levelN.setFrameAsMargin(0, 0, 0, 0) levelCompleteN.setFrameColor(200, 0, 0, 1) levelCompleteN.setFrameLineWidth(5.0) # textNp.node().setGlyphShift(1.0) textNodePath = self.aspect2d.attachNewNode(levelCompleteN) textNodePath.setPos(-0.6, 1.5, 0.5) textNodePath.setScale(0.2) if self.levelCompleteSound2.status() != self.levelCompleteSound2.PLAYING: self.levelCompleteSound2.play() else: pass def coinScoreDisplay2(self): textNp = self.aspect2d.find('coin-score') textNp.node().clearText() textNp.node().setText(str("Coins: " + str(self.coinsCollected2)))
class GameStatePlaying(GState): VIDAS = 3 #LEVEL_TIME = 100 LEVEL_TIME = 50 def start(self): self._playing_node = render.attachNewNode("playing-node") self._playing_node2d = aspect2d.attachNewNode("playing2d-node") self.keyMap = {"left": 0, "right": 0, "up": 0, "down": 0} base.accept("escape", sys.exit) props = WindowProperties() props.setCursorHidden(True) base.disableMouse() props.setMouseMode(WindowProperties.MRelative) base.win.requestProperties(props) base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) self._modulos = None self._paneles = None self._num_lvl = 1 self._num_lvls = 2 self.loadLevel() self.loadGUI() self.loadBkg() self._last_t = None self._last_t_space = 0 def stop(self): self._playing_node.removeNode() self._playing_node2d.removeNode() def loadLevel(self): self._level_time = self.LEVEL_TIME self.initBullet() self._player = Player(self.world) self.loadMap() self.setAI() def initBullet(self): self.world = BulletWorld() #self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setGravity(Vec3(0, 0, -1.62)) groundShape = BulletPlaneShape(Vec3(0, 0, 1), 0) groundNode = BulletRigidBodyNode('Ground') groundNode.addShape(groundShape) self.world.attachRigidBody(groundNode) def loadBkg(self): self.environ1 = loader.loadModel("../data/models/skydome") self.environ1.reparentTo(self._playing_node) self.environ1.setPos(0, 0, 0) self.environ1.setScale(1) self.environ2 = loader.loadModel("../data/models/skydome") self.environ2.reparentTo(self._playing_node) self.environ2.setP(180) self.environ2.setH(270) self.environ2.setScale(1) self.dirnlight1 = DirectionalLight("dirn_light1") self.dirnlight1.setColor(Vec4(1.0, 1.0, 1.0, 1.0)) self.dirnlightnode1 = self._playing_node.attachNewNode(self.dirnlight1) self.dirnlightnode1.setHpr(0, 317, 0) self._playing_node.setLight(self.dirnlightnode1) self.alight = AmbientLight('alight') self.alight.setColor(VBase4(0.05, 0.05, 0.05, 1)) self.alight_node = self._playing_node.attachNewNode(self.alight) self._playing_node.setLight(self.alight_node) self.environ1 = loader.loadModel("../data/models/ground") self.environ1.reparentTo(self._playing_node) self.environ1.setPos(0, 0, 0) self.environ1.setScale(1) def loadGUI(self): self.vidas_imgs = list() w = 0.24 for i in range(self.VIDAS): image_warning = OnscreenImage( image='../data/Texture/signal_warning.png', pos=(-1 + i * w, 0, 0.85), parent=self._playing_node2d) image_warning.setScale(0.1) image_warning.setTransparency(TransparencyAttrib.MAlpha) image_warning.hide() image_ok = OnscreenImage(image='../data/Texture/signal_ok.png', pos=(-1 + i * w, 0, 0.85), parent=self._playing_node2d) image_ok.setScale(0.1) image_ok.setTransparency(TransparencyAttrib.MAlpha) image_ok.show() self.vidas_imgs.append((image_ok, image_warning)) self._level_time_O = OnscreenText(text='', pos=(0, 0.85), scale=0.14, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0), parent=self._playing_node2d) def loadMap(self): if (self._modulos is not None): for m in self._modulos: m.remove() if (self._paneles is not None): for p in self._paneles: p.remove() self._tp = TiledParser("map" + str(self._num_lvl)) self._modulos, self._paneles = self._tp.load_models( self.world, self._playing_node) def setAI(self): taskMgr.add(self.update, 'Update') def update(self, task): if (task.frame > 1): self.world.doPhysics(globalClock.getDt()) self._level_time -= globalClock.getDt() self._level_time_O.setText(str(int(self._level_time))) for panel in self._paneles: contact = self.world.contactTestPair(self._player.getRBNode(), panel.getRBNode()) if contact.getNumContacts() > 0: panel.manipulate() brokens = 0 for panel in self._paneles: if panel.isBroken(): brokens += 1 #print brokens for i, vida_imgs in enumerate(self.vidas_imgs): if i < len(self.vidas_imgs) - brokens: vida_imgs[0].show() vida_imgs[1].hide() else: vida_imgs[0].hide() vida_imgs[1].show() if brokens > self.VIDAS: self.gameOver() return task.done if self._level_time <= 0: self._num_lvl += 1 if self._num_lvl <= self._num_lvls: self.nextLevel() else: self.theEnd() return task.done return task.cont def gameOver(self): taskMgr.remove('player-task') taskMgr.remove('panel-task') taskMgr.remove('panel-sound-task') self._mission_text_O = OnscreenText(text='Game Over', pos=(0, 0), scale=0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0)) taskMgr.add(self.gameOverTransition, 'game-over-transition') #self.loadLevel() print "Game Over" def gameOverTransition(self, task): base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) if task.time > 3.0: self._mission_text_O.hide() props = WindowProperties() props.setCursorHidden(False) base.win.requestProperties(props) self._state_context.changeState( gameStateMenu.GameStateMenu(self._state_context)) print "MENU" return task.done return task.cont def nextLevel(self): taskMgr.remove('player-task') taskMgr.remove('panel-task') taskMgr.remove('panel-sound-task') self._mission_text_O = OnscreenText(text='Mission\nComplete', pos=(0, 0), scale=0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0)) taskMgr.add(self.nextLevelTransition, 'next-Level-transition') print "Mission Complete" def nextLevelTransition(self, task): base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) if task.time > 3.0: print "Next Level" self._mission_text_O.hide() self.loadLevel() return task.done return task.cont def theEnd(self): taskMgr.remove('player-task') taskMgr.remove('panel-task') taskMgr.remove('panel-sound-task') self._mission_text_O = OnscreenText(text='. The End .', pos=(0, 0), scale=0.5, fg=(1.0, 1.0, 1.0, 1.0), bg=(0.0, 0.0, 0.0, 1.0)) taskMgr.add(self.theEndTransition, 'theEnd-transition') #self.loadLevel() print "Mission Complete" def theEndTransition(self, task): base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2) if task.time > 3.0: self._mission_text_O.hide() props = WindowProperties() props.setCursorHidden(False) base.win.requestProperties(props) self._state_context.changeState( gameStateMenu.GameStateMenu(self._state_context)) print "The End" return task.done return task.cont
class DroneEnv(): def __init__(self): self.visualize = False self.actors = 2 #repair this in drone class as well if self.visualize == False : from pandac.PandaModules import loadPrcFileData loadPrcFileData("", "window-type none") import direct.directbase.DirectStart self.ep = 0 self.ep_rew = 0 self.t = 0 self.action_space = Box(-1,1,shape=((self.actors*3),)) self.observation_space = Box(-50,50,shape=((self.actors*3 + 3),)) self.target = 8*np.random.rand(3) self.construct() self.percentages = [] self.percentMean = [] self.percentStd = [] taskMgr.add(self.stepTask, 'update') taskMgr.add(self.lightTask, 'lights') self.forces = np.array([0,0,9.81]*self.actors, dtype = np.float) def construct(self) : if self.visualize : base.cam.setPos(17,17,1) base.cam.lookAt(0, 0, 0) wp = WindowProperties() wp.setSize(1200, 500) base.win.requestProperties(wp) # World self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) #skybox skybox = loader.loadModel('../models/skybox.gltf') skybox.setTexGen(TextureStage.getDefault(), TexGenAttrib.MWorldPosition) skybox.setTexProjector(TextureStage.getDefault(), render, skybox) skybox.setTexScale(TextureStage.getDefault(), 1) skybox.setScale(100) skybox.setHpr(0, -90, 0) tex = loader.loadCubeMap('../textures/s_#.jpg') skybox.setTexture(tex) skybox.reparentTo(render) render.setTwoSided(True) #Light plight = PointLight('plight') plight.setColor((1.0, 1.0, 1.0, 1)) plnp = render.attachNewNode(plight) plnp.setPos(0, 0, 0) render.setLight(plnp) # Create Ambient Light ambientLight = AmbientLight('ambientLight') ambientLight.setColor((0.15, 0.05, 0.05, 1)) ambientLightNP = render.attachNewNode(ambientLight) render.setLight(ambientLightNP) #multiple drone initialization wrgDistance = True positions = [] self.uavs = [Drone.uav() for i in range(self.actors)] [self.world.attachRigidBody(uav.drone) for uav in self.uavs] [uav.drone.setDeactivationEnabled(False) for uav in self.uavs] #target object self.targetObj = loader.loadModel("../models/target.gltf") self.targetObj.reparentTo(render) self.targetObj.setPos(Vec3(self.target[0], self.target[1], self.target[2])) def lightTask(self, task) : count = globalClock.getFrameCount() for uav in self.uavs : rest = count % 100 if rest < 10 : uav.plight2.setColor((0.1, 0.9, 0.1, 1)) elif rest > 30 and rest < 40 : uav.plight2.setColor((0.9, 0.1, 0.1, 1)) elif rest > 65 and rest < 70 : uav.plight2.setColor((0.9,0.9, 0.9, 1)) elif rest > 75 and rest < 80 : uav.plight2.setColor((0.9,0.9, 0.9, 1)) else : uav.plight2.setColor((0.1, 0.1, 0.1, 1)) return task.cont def getState(self) : stateVec = [] for uav in self.uavs : pos = uav.drone.transform.pos stateVec.append(pos) stateVec.append(self.target) state = np.array(stateVec).reshape((3*self.actors + 3),) state = np.around(state, decimals = 2) state = np.expand_dims(state, 0) return state def getReward(self) : reward1 = 0 reward2 = 0 distance = 0 pos1 = self.uavs[0].drone.transform.pos d1 = np.linalg.norm(pos1 - self.target) if d1 < 10 : reward1 = 10 - d1 reward1 = reward1/50 pos2 = self.uavs[1].drone.transform.pos d2 = np.linalg.norm(pos2 - self.target) if d2 < 10 : reward2 = 10 - d2 reward2 = reward2/50 return reward1, reward2 def reset(self): #log self.percentages.append(self.ep_rew) me = np.mean(self.percentages[-500:]) self.percentMean.append(me) self.percentStd.append(np.std(self.percentages[-500:])) state = self.getState() vel1 = self.uavs[0].drone.get_linear_velocity() velocity1 = np.nan_to_num(np.array([vel1[0], vel1[1], vel1[2]])) v1 = np.mean(np.abs(velocity1)) vel2 = self.uavs[1].drone.get_linear_velocity() velocity2 = np.nan_to_num(np.array([vel2[0], vel2[1], vel2[2]])) v2 = np.mean(np.abs(velocity2)) print(f'{self.ep} {self.t} {self.ep_rew:+8.2f} {me:+6.2f} {v1:+4.2f} {v2:+4.2f} {state}') #prevPos = 8*(np.random.rand(3)-0.5) + 8*(np.random.rand(3)-0.5) #- np.array([0,0,2], np.float32) #physics reset for uav in self.uavs : #inPos = prevPos + 8*(np.random.rand(3)-0.5) + 8*(np.random.rand(3)-0.5) #prevPos = np.copy(inPos) #uav.body.setPos(inPos[0], inPos[1], inPos[2]) uav.body.setHpr(0, 0, 0) uav.drone.set_linear_velocity(Vec3(0,0,0)) uav.drone.setAngularVelocity(Vec3(0,0,0)) self.uavs[0].body.setPos(-4, 0, -4) self.uavs[1].body.setPos(4,0,4) self.forces = np.array([0,0,9.81]*self.actors, dtype = np.float) #define new target: self.target = np.zeros((3)) #8*(2*np.random.rand(3)-1) self.target[2] = np.abs(self.target[2]) self.targetObj.setPos(Vec3(self.target[0], self.target[1], self.target[2])) self.ep +=1 self.t = 0 self.ep_rew = 0 state = self.getState() return state def stepTask(self, task) : dt = globalClock.getDt() if self.visualize : self.world.doPhysics(dt) else : self.world.doPhysics(0.1) for i, uav in enumerate(self.uavs) : force = self.forces[i*3: (i+1)*3] forceVec3 = Vec3(force[0], force[1], force[2]) if np.abs(force[0]) > 0 or np.abs(force[1]) > 0 or np.abs(force[2]) > 0 : uav.drone.applyCentralForce(forceVec3) return task.cont def step(self, action): done = False reward = 0 self.t += 1 reward1, reward2 = self.getReward() self.ep_rew += reward1 + reward2 state = self.getState() basis = np.array([0,0,9.81]*self.actors, dtype = np.float) self.forces = 0.1*action + basis #0.05*action + basis #10 sub steps in each step for i in range(5) : c = taskMgr.step() self.forces -= 0.05*(self.forces - basis) #collision test if self.world.contactTestPair(self.uavs[0].drone, self.uavs[1].drone).getNumContacts() > 0 : done = True break #time constraint if self.t > 150 : done = True #position constraint : for uav in self.uavs : pos = uav.drone.transform.pos if np.max(np.abs(pos)) > 30 : done = True return state, reward1, reward2, done, {}