def plannplay(self, transitoption = "useboth", choice = "startrgtgoalrgt"): """ updated 20190314 :param transitoption: "useplacement", "usehandover", "useboth" :return: """ # capture initial objpose self.robot.goinitpose() # start OC print("Finding motion to goal 1...") startrotmat4 = base.pg.np4ToMat4(self.goallist[0]) goalrotmat4 = base.pg.np4ToMat4(self.goallist[1]) regrip = regriptppfp.RegripTppFp(self.objcm.objpath, self.robot, sck918, self.gdb, base, self.obstaclecmlist, transitoption=transitoption) regrip.addStartGoal(startrotmat4 = startrotmat4, goalrotmat4 = goalrotmat4, choice = choice, goaltoolvec=(-1,0,0)) cdchecker = ccball.CollisionCheckerBall(self.robotball) # OUTERLOOP collision detection objmsmp, numikrmsmp, jawwidthmp, originalpathnidlist = rp.planRegrasp(self.robot, regrip, self.objcm, cdchecker, self.obstaclecmlist+[self.groove], switch="OC", previous = [], end = False, togglemp=True) if objmsmp is None: return False nextid = regrip.regg.node[originalpathnidlist[-1]]['globalgripid'] # nextgrasp = regrip.regg.node[originalpathnidlist[-1]]['globalgripid'] for id in range(1, len(self.goallist)-1): print("Finding motion to goal "+str(id+1)+"...") print("Start and goal hand global ids are ", nextid) startrotmat4 = base.pg.np4ToMat4(self.goallist[id]) goalrotmat4 = base.pg.np4ToMat4(self.goallist[id+1]) # regrip.reset() #-------------- for j, rotmat in enumerate(regrip.freegriprotmats): # vector between object and robot ee if regrip.freegripids[j] == nextid: ttgsrotmat = rotmat * goalrotmat4 ttgscct0 = goalrotmat4.xformPoint(regrip.freegripcontacts[j][0]) ttgscct1 = goalrotmat4.xformPoint(regrip.freegripcontacts[j][1]) ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2 # base.pggen.plotSphere(base.render,base.pg.v3ToNp(ttgsfgrcenter),radius=30) # base.pggen.plotAxis(base.render,spos=base.pg.v3ToNp(ttgsfgrcenter),pandamat3=ttgsrotmat.getUpper3()) goalrotmat4np = base.pg.mat4ToNp(ttgsrotmat) armname = 'rgt' start = [numikrmsmp[-1][-1][0]] + numikrmsmp[-1][-1][1].tolist() goal = self.robot.numikr(base.pg.v3ToNp(ttgsfgrcenter),goalrotmat4np[:3,:3],armname="rgt") startjawwidth = jawwidthmp[-1][-1][0] # if "lft" in originalpathnidlist[-1]: # armname = 'lft' # start = [numikrmsmp[-1][-1][0]] + numikrmsmp[-1][-1][2].tolist() # goal = self.robot.numikr(base.pg.v3ToNp(ttgsfgrcenter), goalrotmat4np[:3, :3], armname="lft") startjawwidth = jawwidthmp[-1][-1][1] if goal is None: print("Goal is None!!!!!") return False goal = [goal[0]] + goal[1].tolist() starttreesamplerate = 25 endtreesamplerate = 30 print(armname) print(startjawwidth) # ctcallback.setarmname(armname, usewaist=True) import motionplanning.ctcallback as ctcb import motionplanning.rrt.ddrrtconnect as ddrrtc import motionplanning.smoother as sm ctcallback = ctcb.CtCallback(base, self.robot, cdchecker) planner = ddrrtc.DDRRTConnect(start=start, goal=goal, ctcallback=ctcallback, starttreesamplerate=starttreesamplerate, endtreesamplerate=endtreesamplerate, expanddis=5, maxiter=200, maxtime=10.0) tempnumikrmsmp = [] tempjawwidthmp = [] tempobjmsmp = [] objcopy = copy.deepcopy(self.objcm) objmanp = base.pg.mat4ToNp(objmsmp[-1][-1]) self.robot.movearmfkr([start[0],np.array(start[1:])],armname) relpos, relrot = self.robot.getinhandpose(objmanp[:3,3], objmanp[:3,:3], armname) ctcallback.setarmname(armname, usewaist=True) path, sampledpoints = planner.planninghold(objcopy, [relpos, relrot], []) if path is False: print("Motion planning with hold failed! restarting...") # regrip.removeBadNodes([pathnidlist[i-1][:-1]]) return False smoother = sm.Smoother() path = smoother.pathsmoothinghold(path, planner, objcopy, [relpos, relrot], 30) npath = len(path) for j in range(npath): if armname == 'rgt': tempnumikrmsmp.append([path[j][0], np.array(path[j][1:]), numikrmsmp[-1][-1][2]]) else: tempnumikrmsmp.append([path[j][0], numikrmsmp[-1][-1][1], np.array(path[j][1:])]) self.robot.movearmfkr([path[j][0], np.array(path[j][1:])], armname=armname) tempjawwidthmp.append(jawwidthmp[-1][-1]) objpos, objrot = self.robot.getworldpose(relpos, relrot, armname) tempobjmsmp.append(base.pg.npToMat4(objrot, objpos)) objmsmp = objmsmp+[None]+[tempobjmsmp] numikrmsmp = numikrmsmp+[None]+[tempnumikrmsmp] jawwidthmp = jawwidthmp+[None]+[tempjawwidthmp] #----------------- # regrip.addStartGoal(startrotmat4 = startrotmat4, goalrotmat4 = goalrotmat4, choice = "startrgtgoalrgt", # startgraspgid=nextid, starttoolvec = [0,0,0], goaltoolvec = [-1,0,0]) # # # # cdchecker = ccball.CollisionCheckerBall(self.robotball) # # OUTERLOOP collision detection # objmsmp_t, numikrmsmp_t, jawwidthmp_t, originalpathnidlist_t = rp.planRegrasp(self.robot, regrip, # self.objcm, cdchecker, # self.obstaclecmlist, # switch="CC", # previous=[objmsmp[-1][-1], numikrmsmp[-1][-1], jawwidthmp[-1][-1]], # end = False, # togglemp=True) # if objmsmp_t is None and id == len(self.goallist)-2: # break # elif objmsmp_t is None: # return False # objmsmp = objmsmp+[None]+objmsmp_t # numikrmsmp = numikrmsmp+[None]+numikrmsmp_t # jawwidthmp = jawwidthmp+[None]+jawwidthmp_t # nextid = regrip.regg.node[originalpathnidlist_t[-1]]['globalgripid'] objmsmp = objmsmp + [None] numikrmsmp = numikrmsmp + [None] jawwidthmp = jawwidthmp + [None] self.rbtmnp.removeNode() self.rbtmnpani = [None, None] self.objmnpani = [None] motioncounter = [0] def updatemotionsec(objms, numikrms, jawwidth, rbtmnp, objmnp, motioncounter, robot, objcm, task): if motioncounter[0] < len(numikrms): if rbtmnp[0] is not None: rbtmnp[0].detachNode() # rbtmnp[1].detachNode() if objmnp[0] is not None: objmnp[0].detachNode() rgtarmjnts = numikrms[motioncounter[0]][1].tolist() lftarmjnts = numikrms[motioncounter[0]][2].tolist() robot.movealljnts([numikrms[motioncounter[0]][0], 0, 0] + rgtarmjnts + lftarmjnts) rgtjawwidth = jawwidth[motioncounter[0]][0] lftjawwidth = jawwidth[motioncounter[0]][1] # print rgtjawwidth, lftjawwidth rbtmnp[0] = self.robotmesh.genmnp(robot, rgtjawwidth, lftjawwidth) bcndict = self.robotball.genfullactivebcndict(robot) # rbtmnp[1] = self.robotball.showcn(base, bcndict) robot.goinitpose() rbtmnp[0].reparentTo(base.render) objmnp[0] = copy.deepcopy(objcm) objmnp[0].setMat(objms[motioncounter[0]]) objmnp[0].reparentTo(base.render) # objmnp[0].showLocalFrame() # objmnp[0].showcn() motioncounter[0] += 1 else: motioncounter[0] = 0 return task.done # base.win.saveScreenshot(Filename(str(motioncounter[0]) + '.jpg')) return task.again objmsmpactive = [objmsmp[0]] numikrmsmpactive = [numikrmsmp[0]] jawwidthmpactive = [jawwidthmp[0]] taskMgr.doMethodLater(0.1, updatemotionsec, "updatemotionsec", extraArgs=[objmsmpactive[0], numikrmsmpactive[0], jawwidthmpactive[0], self.rbtmnpani, self.objmnpani, motioncounter, self.robot, self.objcm], appendTask=True) motionseccounter = [0] def updatesection(objmsmpactive, numikrmsmpactive, jawwidthmpactive, objmsmp, numikrmsmp, jawwidthmp, rbtmnp, objmnp, motioncounter, motionseccounter, robot, objcm, task): # this_dir, this_filename = os.path.split(__file__) # f = open(os.path.join(this_dir, "document", "Planned_pegpath_deriv2_robot.txt"), "a") jntslist_waist = [] jntslist_rgt = [] jntslist_lft = [] jawwidthlist_rgt = [] jawwidthlist_lft = [] new_jntslist_waist_rad = [] jntslist_rgt_rad = [] jntslist_lft_rad = [] if base.inputmgr.keyMap['space'] is True or (motionseccounter[0] > 5 and not taskMgr.hasTaskNamed("updatemotionsec")): # print(motionseccounter[0], len(objmsmp) - 1) if motionseccounter[0] <= len(objmsmp) - 1: motionseccounter[0] = motionseccounter[0] + 1 if not (motionseccounter[0] == len(objmsmp)): if objmsmp[motionseccounter[0]] is not None: objmsmpactive[0] = objmsmp[motionseccounter[0]] numikrmsmpactive[0] = numikrmsmp[motionseccounter[0]] jawwidthmpactive[0] = jawwidthmp[motionseccounter[0]] base.inputmgr.keyMap['space'] = False # print(motionseccounter[0]) # print(jawwidthmpactive[0]) taskMgr.remove('updatemotionsec') motioncounter[0] = 0 taskMgr.doMethodLater(0.1, updatemotionsec, "updatemotionsec", extraArgs=[objmsmpactive[0], numikrmsmpactive[0], jawwidthmpactive[0], rbtmnp, objmnp, motioncounter, robot, objcm], appendTask=True) if objmsmp[motionseccounter[0]-1] is not None: # execute the last sequence # temp_objmsmpactive = objmsmp[motionseccounter[0]-1] temp_numikrmsmpactive = numikrmsmp[motionseccounter[0] - 1] # print "temp_numikrmsmpactive is %s" % temp_numikrmsmpactive temp_jawwidthmpactive = jawwidthmp[motionseccounter[0] - 1] # rgt and left # # jntslist_waist = [] # jntslist_rgt = [] # jntslist_lft = [] # jawwidthlist_rgt = [] # jawwidthlist_lft = [] # new_jntslist_waist_rad = [] # jntslist_rgt_rad = [] # jntslist_lft_rad = [] # angles = [] for eachtemp in temp_numikrmsmpactive: jntslist_waist.append(eachtemp[0]) # print ("Joint list waist:{} ".format(jntslist_waist)) jntslist_rgt.append(eachtemp[1].tolist()) # print ("Joint list right:{} ".format(jntslist_rgt)) jntslist_lft.append(eachtemp[2].tolist()) # print ("Joint list left:{} ".format(jntslist_lft)) # print ("Length of joint list is:{} ".format(len(jntslist_rgt))) for eachtemp in temp_jawwidthmpactive: jawwidthlist_rgt.append(eachtemp[0]) # print ("jaw width list right:{} ".format(jawwidthlist_rgt)) jawwidthlist_lft.append(eachtemp[1]) # print ("jaw width list left:{} ".format(jawwidthlist_lft)) # print jawwidthlist_rgt # uncomment this part to actuate the Nextage if len(jntslist_rgt) == 2: # do nothing; We dont need to examine lft since they are symmetric pass else: # check if the waist is moved waistdiff = abs(np.array(jntslist_waist[0])-np.array(jntslist_waist[-1])) # if waistdiff.max() > 1e-6: # print "jntslist_waist is %s" % jntslist_waist jntslist_waist_rad = [] for i in range(len(jntslist_waist)): # print "jntslist_waist[%d] is %s" % (i, jntslist_waist[i]) jntslist_waist_rad.append(math.radians(jntslist_waist[i])) # print "jntslist_waist_rad is %s" % jntslist_waist_rad # new_jntslist_waist_rad = [] for j in range(len(jntslist_waist_rad)): jnt_waist_rad = [] jnt_waist_rad.append(jntslist_waist_rad[j]) new_jntslist_waist_rad.append(jnt_waist_rad) # print "new_jntslist_waist_rad is %s" % new_jntslist_waist_rad # time = [0.2] # para_waist = ['torso', new_jntslist_waist_rad, time*len(new_jntslist_waist_rad)] # self.nxtu.playPatternOfGroup(para_waist) # check if the arm is moved rgtdiff = abs(np.array(jntslist_rgt[0])-np.array(jntslist_rgt[-1])) # if rgtdiff.max() > 1e-6: # print "jntslist_rgt is %s" % jntslist_rgt # jntslist_rgt_rad = [] jnts_rgt_rad = [] for i in range(len(jntslist_rgt)): # print "jntslist_rgt[%d] is %s" % (i, jntslist_rgt[i]) for j in range(len(jntslist_rgt[i])): jnts_rgt_rad.append(math.radians(jntslist_rgt[i][j])) # print "jntslist_rgt_rad[%d] is %s" % (i, jnts_rgt_rad[(len(jntslist_rgt[i]) * i):len(jntslist_rgt[i]) * (i + 1)]) jntslist_rgt_rad.append(jnts_rgt_rad[(len(jntslist_rgt[i])*i):len(jntslist_rgt[i])*(i+1)]) # print "jntslist_rgt_rad is %s" % jntslist_rgt_rad # time = [0.2] # para_rgt = ['rarm', jntslist_rgt_rad, time*len(jntslist_rgt_rad)] # self.nxtu.playPatternOfGroup(para_rgt) lftdiff = abs(np.array(jntslist_lft[0])-np.array(jntslist_lft[-1])) # if lftdiff.max() > 1e-6: # print "jntslist_lft is %s" % jntslist_lft # jntslist_lft_rad = [] jnts_lft_rad = [] for i in range(len(jntslist_lft)): # print "jntslist_rgt[%d] is %s" % (i, jntslist_lft[i]) for j in range(len(jntslist_lft[i])): jnts_lft_rad.append(math.radians(jntslist_lft[i][j])) # print "jntslist_lft_rad[%d] is %s" % (i, jnts_lft_rad[(len(jntslist_lft[i]) * i):len(jntslist_lft[i]) * (i + 1)]) jntslist_lft_rad.append(jnts_lft_rad[(len(jntslist_rgt[i]) * i):len(jntslist_lft[i]) * (i + 1)]) # print "jntslist_lft_rad is %s" % jntslist_lft_rad # time = [0.2] # para_lft = ['larm', jntslist_lft_rad, time*len(jntslist_lft_rad)] # self.nxtu.playPatternOfGroup(para_lft) angles = [] if new_jntslist_waist_rad: length = len(new_jntslist_waist_rad) else: length = len(jntslist_rgt_rad) for n in range(length): angle = [] # if new_jntslist_waist_rad: headwaistangles = [new_jntslist_waist_rad[n][0], 0, 0] for headwaistangle in headwaistangles: angle.append(headwaistangle) # if jntslist_rgt_rad: for jntsrgt in jntslist_rgt_rad[n]: angle.append(jntsrgt) # if jntslist_lft_rad: for jntslft in jntslist_lft_rad[n]: angle.append(jntslft) print("Angle is", angle) angles.append(angle) print("Angles are", angles) # for poses in angles: # # print(poses) # f.write(str(poses) + '\n') # f.close() # if jawwidthlist_rgt[-1] <85.0: # self.ur3u.closegripper("rgt") # else: # self.ur3u.opengripper("rgt") # if jawwidthlist_lft[-1] < 85.0: # self.ur3u.closegripper("lft") # else: # self.ur3u.opengripper("lft") else: #TODO add directokdialog print("One motion section is done!") else: motionseccounter[0] = 0 taskMgr.remove('updateshow') return task.done return task.again taskMgr.doMethodLater(0.04, updatesection, "updatesection", extraArgs=[objmsmpactive, numikrmsmpactive, jawwidthmpactive, objmsmp, numikrmsmp, jawwidthmp, self.rbtmnpani, self.objmnpani, motioncounter, motionseccounter, self.robot, self.objcm], appendTask=True) return True
def planRegrasp(robot, regrip, objcm, cdchecker, obstaclecmlist, id=0, switch="OC", previous=[], end=False, togglemp=True): """ plan the regrasp sequences :param objpath: :param robot: :param hand: :param dbase: :param obstaclecmlist: :param id = 0 :param switch in "OC" open-close "CC" close-close :param previous: set it to [] if the motion is not a continuing one, or else, set it to [lastobjmat4, lastikr, lastjawwidth] :param end: set it to True if it is the last one :param togglemp denotes whether the motion between the keyposes are planned or not, True by default :return: author: weiwei date: 20180924 """ while True: print("new search") regrip.updateshortestpath() [objms, numikrms, jawwidth, pathnidlist, originalpathnidlist] = \ sqdes.getMotionSequence(regrip, id = id, choice = regrip.choice, type=switch, previous=previous) if objms is None: return [None, None, None, None] bcdfree = True for i in range(len(numikrms)): rgtarmjnts = numikrms[i][1].tolist() lftarmjnts = numikrms[i][2].tolist() robot.movealljnts([numikrms[i][0], 0, 0] + rgtarmjnts + lftarmjnts) # skip the exact handover pose and only detect the cd between armhnd and body if pathnidlist[i].startswith('ho') and pathnidlist[ i + 1].startswith('ho'): abcd = cdchecker.isCollidedHO(robot, obstaclecmlist) if abcd: regrip.removeBadNodes([pathnidlist[i][:-1]]) print("Abcd collided at ho pose") bcdfree = False # robotmesh.genmnp(robot).reparentTo(base.render) # robotball.showbcn(base, robotball.genbcndict(robot)) # base.run() break else: # NOTE: we ignore both arms here for conciseness # This might be a potential bug if cdchecker.isCollided(robot, obstaclecmlist, holdarmname="all"): regrip.removeBadNodes([pathnidlist[i][:-1]]) print("Robot collided at non-ho pose") bcdfree = False # robotmesh.genmnp(robot).reparentTo(base.render) # robotball.showbcn(base, robotball.genbcndict(robot)) # base.run() break robot.goinitpose() if bcdfree: objmsmp = [] numikrmsmp = [] jawwidthmp = [] print(pathnidlist) if not togglemp: for i, numikrm in enumerate(numikrms): if i > 0: startid = pathnidlist[i - 1] endid = pathnidlist[i] if (not end) and (endid is 'end'): continue if (len(previous) > 0) and (startid is 'begin'): continue numikrmsmp.append([numikrms[i - 1], numikrms[i]]) objmsmp.append([objms[i - 1], objms[i]]) jawwidthmp.append([jawwidth[i - 1], jawwidth[i]]) return objmsmp, numikrmsmp, jawwidthmp, originalpathnidlist # INNERLOOP motion planning smoother = sm.Smoother() ctcallback = ctcb.CtCallback(base, robot, cdchecker) breakflag = False for i, numikrm in enumerate(numikrms): if i > 0: # determine which arm to plan # assume right # assume redundant planning robot.goinitpose() startid = pathnidlist[i - 1] endid = pathnidlist[i] objmat = base.pg.mat4ToNp(objms[i - 1]) objrot = objmat[:3, :3] objpos = objmat[:3, 3] if (not end) and (endid is 'end'): continue if (len(previous) > 0) and (startid is 'begin'): continue if (startid[-1] == "o" and endid[-1] == "c") or (startid[-1] == "c" and endid[-1] == "o"): # open and close gripper print("O/C hands, simply include ", pathnidlist[i - 1], " and ", pathnidlist[i]) numikrmsmp.append([numikrms[i - 1], numikrms[i]]) objmsmp.append([objms[i - 1], objms[i]]) jawwidthmp.append([jawwidth[i - 1], jawwidth[i]]) continue if (startid[:-1] == endid[:-1]): if (startid[-1] != "i") and (endid[-1] != "i"): print("Motion primitives, interplate ", pathnidlist[i - 1], " and ", pathnidlist[i]) # linear interpolation tempnumikrmsmp = [] tempjawwidthmp = [] tempobjmsmp = [] temparmname = "rgt" startjntags = [ numikrms[i - 1][0], numikrms[i - 1][1] ] goaljntags = [numikrms[i][0], numikrms[i][1]] if "lft" in startid: temparmname = "lft" startjntags = [ numikrms[i - 1][0], numikrms[i - 1][2] ] goaljntags = [numikrms[i][0], numikrms[i][2]] ctcallback.setarmname(temparmname, usewaist=True) [interplatedjnts, interplatedobjposes] = \ ctcallback.isLMAvailableJNTwithObj(startjntags, goaljntags, [objpos, objrot], armname = temparmname, type = startid[-1]) if len(interplatedjnts) == 0: print( "Failed to interpolate motion primitive! restarting..." ) regrip.removeBadNodes( [pathnidlist[i - 1][:-1]]) breakflag = True break for eachitem in interplatedjnts: if temparmname == "rgt": tempnumikrmsmp.append([ eachitem[0], eachitem[1], numikrms[i - 1][2] ]) else: tempnumikrmsmp.append([ eachitem[0], numikrms[i - 1][1], eachitem[1] ]) tempjawwidthmp.append(jawwidth[i - 1]) for eachitem in interplatedobjposes: tempobjmsmp.append( base.pg.npToMat4(eachitem[1], eachitem[0])) numikrmsmp.append(tempnumikrmsmp) jawwidthmp.append(tempjawwidthmp) objmsmp.append(tempobjmsmp) continue # numikrmsmp.append([numikrms[i-1], numikrms[i]]) # objmsmp.append([objms[i-1], objms[i]]) # jawwidthmp.append([jawwidth[i-1], jawwidth[i]]) # continue if (startid[0] not in [ 'b', 's', 'g' ]) and (endid[0] not in ['b', 's', 'g']): print("Planning ho motion between ", pathnidlist[i - 1], " and ", pathnidlist[i]) rgtarmjnts = numikrms[i - 1][1].tolist() lftarmjnts = numikrms[i - 1][2].tolist() robot.movealljnts([numikrms[i - 1][0], 0, 0] + rgtarmjnts + lftarmjnts) # assume rgt armname = 'rgt' start = numikrms[i - 1][1].tolist() goal = numikrms[i][1].tolist() startjawwidth = jawwidth[i - 1][0] if "lft" in endid: armname = 'lft' start = numikrms[i - 1][2].tolist() goal = numikrms[i][2].tolist() startjawwidth = jawwidth[i - 1][1] starttreesamplerate = 25 endtreesamplerate = 30 print(armname) print(startjawwidth) ctcallback.setarmname(armname, usewaist=False) planner = ddrrtc.DDRRTConnect( start=start, goal=goal, ctcallback=ctcallback, starttreesamplerate=starttreesamplerate, endtreesamplerate=endtreesamplerate, expanddis=5, maxiter=200, maxtime=10.0) tempnumikrmsmp = [] tempjawwidthmp = [] tempobjmsmp = [] if (endid[-1] == "c") or (endid[-1] == "w"): # if the arm is holding an object print("Planning hold motion between ", pathnidlist[i - 1], " and ", pathnidlist[i]) relpos, relrot = robot.getinhandpose( objpos, objrot, armname) path, sampledpoints = planner.planninghold( objcm, [relpos, relrot], obstaclecmlist) if path is False: print( "Motion planning with hold failed! restarting..." ) # regrip.removeBadNodes([pathnidlist[i-1][:-1]]) regrip.removeBadNodes([pathnidlist[i][:-1]]) breakflag = True break path = smoother.pathsmoothinghold( path, planner, objcm, [relpos, relrot], 30) npath = len(path) for j in range(npath): if armname == 'rgt': tempnumikrmsmp.append([ 0.0, np.array(path[j]), numikrms[i - 1][2] ]) else: tempnumikrmsmp.append([ 0.0, numikrms[i - 1][1], np.array(path[j]) ]) robot.movearmfk(np.array(path[j]), armname=armname) tempjawwidthmp.append(jawwidth[i - 1]) objpos, objrot = robot.getworldpose( relpos, relrot, armname) tempobjmsmp.append( base.pg.npToMat4(objrot, objpos)) else: # if the arm is not holding an object, the object will be treated as an obstacle print("Planning motion ", pathnidlist[i - 1], " and ", pathnidlist[i]) objcmcopy = copy.deepcopy(objcm) objcmcopy.setMat(objms[i - 1]) obstaclecmlistnew = obstaclecmlist + [objcmcopy] path, sampledpoints = planner.planning( obstaclecmlistnew) if path is False: print("Motion planning failed! restarting...") # regrip.removeBadNodes([pathnidlist[i-1][:-1]]) # regrip.removeBadNodes([pathnidlist[i][:-1]]) node0 = pathnidlist[i - 1][:-1] node1 = pathnidlist[i][:-1] regrip.removeBadEdge(node0, node1) breakflag = True break path = smoother.pathsmoothing(path, planner, 30) npath = len(path) for j in range(npath): if armname == 'rgt': tempnumikrmsmp.append([ 0.0, np.array(path[j]), numikrms[i - 1][2] ]) else: tempnumikrmsmp.append([ 0.0, numikrms[i - 1][1], np.array(path[j]) ]) tempjawwidthmp.append(jawwidth[i - 1]) tempobjmsmp.append(objms[i - 1]) numikrmsmp.append(tempnumikrmsmp) jawwidthmp.append(tempjawwidthmp) objmsmp.append(tempobjmsmp) continue # init robot pose rgtarmjnts = numikrms[i - 1][1].tolist() lftarmjnts = numikrms[i - 1][2].tolist() robot.movealljnts([numikrms[i - 1][0], 0, 0] + rgtarmjnts + lftarmjnts) # assume rgt armname = 'rgt' start = [numikrms[i - 1][0]] + numikrms[i - 1][1].tolist() goal = [numikrms[i][0]] + numikrms[i][1].tolist() startjawwidth = jawwidth[i - 1][0] if "lft" in endid: armname = 'lft' start = [numikrms[i - 1][0] ] + numikrms[i - 1][2].tolist() goal = [numikrms[i][0]] + numikrms[i][2].tolist() startjawwidth = jawwidth[i - 1][1] starttreesamplerate = 25 endtreesamplerate = 30 print(armname) print(startjawwidth) ctcallback.setarmname(armname, usewaist=True) planner = ddrrtc.DDRRTConnect( start=start, goal=goal, ctcallback=ctcallback, starttreesamplerate=starttreesamplerate, endtreesamplerate=endtreesamplerate, expanddis=5, maxiter=200, maxtime=10.0) tempnumikrmsmp = [] tempjawwidthmp = [] tempobjmsmp = [] if (endid[-1] == "c") or (endid[-1] == "w"): # if the arm is holding an object print("Planning hold motion between ", pathnidlist[i - 1], " and ", pathnidlist[i]) relpos, relrot = robot.getinhandpose( objpos, objrot, armname) # try: # [path, sampledpoints] = planner.planninghold(objcm, [relpos, relrot], obstaclecmlist) # except Exception as e: # print(e) # print("Motion planning with hold failed! restarting...") # regrip.removeBadNodes([pathnidlist[i-1][:-1]]) # regrip.removeBadNodes([pathnidlist[i][:-1]]) # breakflag = True # break path, sampledpoints = planner.planninghold( objcm, [relpos, relrot], obstaclecmlist) if path is False: print( "Motion planning with hold failed! restarting..." ) # regrip.removeBadNodes([pathnidlist[i-1][:-1]]) regrip.removeBadNodes([pathnidlist[i][:-1]]) breakflag = True break path = smoother.pathsmoothinghold( path, planner, objcm, [relpos, relrot], 30) npath = len(path) for j in range(npath): if armname == 'rgt': tempnumikrmsmp.append([ path[j][0], np.array(path[j][1:]), numikrms[i - 1][2] ]) else: tempnumikrmsmp.append([ path[j][0], numikrms[i - 1][1], np.array(path[j][1:]) ]) robot.movearmfkr( [path[j][0], np.array(path[j][1:])], armname=armname) tempjawwidthmp.append(jawwidth[i - 1]) objpos, objrot = robot.getworldpose( relpos, relrot, armname) tempobjmsmp.append(base.pg.npToMat4( objrot, objpos)) else: # if the arm is not holding an object, the object will be treated as an obstacle print("Planning motion ", pathnidlist[i - 1], " and ", pathnidlist[i]) objcmcopy = copy.deepcopy(objcm) objcmcopy.setMat(objms[i - 1]) obstaclecmlistnew = obstaclecmlist + [objcmcopy] path, sampledpoints = planner.planning( obstaclecmlistnew) if path is False: print("Motion planning failed! restarting...") # regrip.removeBadNodes([pathnidlist[i-1][:-1]]) # regrip.removeBadNodes([pathnidlist[i][:-1]]) if pathnidlist[i - 1] == "begin": regrip.removeBadNodes([pathnidlist[i][:-1]]) breakflag = True break if pathnidlist[1] == "end": regrip.removeBadNodes( [pathnidlist[i - 1][:-1]]) breakflag = True break node0 = pathnidlist[i - 1][:-1] node1 = pathnidlist[i][:-1] regrip.removeBadEdge(node0, node1) breakflag = True break path = smoother.pathsmoothing(path, planner, 30) npath = len(path) for j in range(npath): if armname == 'rgt': tempnumikrmsmp.append([ path[j][0], np.array(path[j][1:]), numikrms[i - 1][2] ]) else: tempnumikrmsmp.append([ path[j][0], numikrms[i - 1][1], np.array(path[j][1:]) ]) tempjawwidthmp.append(jawwidth[i - 1]) tempobjmsmp.append(objms[i - 1]) numikrmsmp.append(tempnumikrmsmp) jawwidthmp.append(tempjawwidthmp) objmsmp.append(tempobjmsmp) print(i, len(numikrms) - 1) if breakflag is False: # successfully finished! return [objmsmp, numikrmsmp, jawwidthmp, originalpathnidlist] else: # remov node and start new search continue
objrot_lft = [] objpos_rgt = [] objrot_rgt = [] nxtrobot = nxt.NxtRobot() robotball = nxtball.NxtBall() lfthand = rtq85.Rtq85(jawwidth=85, ftsensoroffset=0) rgthand = rtq85.Rtq85(jawwidth=85, ftsensoroffset=0) robotmesh = nxtmesh.NxtMesh(lfthand=lfthand, rgthand=rgthand) nxtmesh = robotmesh.genmnp(nxtrobot, jawwidthrgt=85.0, jawwidthlft=0.0, togglejntscoord=False) # nxtmesh.reparentTo(base.render) armname = 'rgt' nxtball = nxtball.NxtBall() cdchecker = cdck.CollisionCheckerBall(nxtball) ctcallback = ctcb.CtCallback(base, nxtrobot, cdchecker, ctchecker=None, armname=armname) smoother = sm.Smoother() starttreesamplerate = 25 endtreesamplerate = 30 pandamat4 = Mat4() pandamat4.setRow(3, Vec3(0, 0, 250)) nxtrobot.goinitpose() armjntsgoal6 = [] # # this part can be packed as a function filename = "document/action0.yaml" num = readyaml.getlength(filename) for ts in range(num): [main_object, s_tran, s_rot, s_tran_sub, s_rot_sub] = \
# another example -- load obj collision model independently # this_dir, this_filename = os.path.split(__file__) # objname = "tool_motordriver.stl" # objpath = os.path.join(this_dir, "objects", objname) # objcm = cm.CollisionModel(objpath) robot = ur3dual.Ur3DualRobot() robotball = ur3dualball.Ur3DualBall() rgthnd = rtq85.Rtq85(jawwidth=85) lfthnd = rtq85.Rtq85(jawwidth=85) robotmesh = ur3dualmesh.Ur3DualMesh(rgthand=rgthnd, lfthand=lfthnd) robotnp = robotmesh.genmnp(robot, jawwidthrgt=85.0, jawwidthlft=0.0) # robotnp.reparentTo(base.render) armname = 'rgt' cdchecker = cdck.CollisionCheckerBall(robotball) ctcallback = ctcb.CtCallback(base, robot, cdchecker, armname=armname) smoother = sm.Smoother() robot.goinitpose() # robotnp = robotmesh.genmnp(robot, jawwidthrgt = 85.0, jawwidthlft=0.0) # robotnp.reparentTo(base.render) # base.run() starttreesamplerate = 25 endtreesamplerate = 30 objstpos = np.array([354.5617667638, -400.0889005661, 1090.5]) objstrot = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]).T objgpos = np.array([304.5617667638, -277.1026725769, 1101.0]) objgrot = np.array([[1.0, 0.0, 0.0], [0.0, 6.12323426293e-17, -1.0], [0.0, 1.0, 6.12323426293e-17]]).T objcmcopys = copy.deepcopy(objcm)
#start and goal joints and move the robot goalpos1 = np.array([600, 400, 1550]) # 1350 #500 goalrot1 = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]]) goalrot1 = rot_transform(goalrot1, 0) start_armjnts = rbt.numik(startpos, startrot, "lft") goal1_armjnts = rbt.numik(goalpos1, goalrot1, "lft") rbt.movearmfk(start_armjnts, "lft") rbt.movearmfk(rbt.initrgtjnts, "rgt") # prepare the planner rbtball = ur3dualball.Ur3DualBall() cdchecker = cdck.CollisionCheckerBall(rbtball) ctcallback = ctcb.CtCallback(base, rbt, cdchecker=cdchecker, ctchecker=ctchecker, ctangle=ctangle, armname="lft") planner = rrtc.RRTConnect(start=start_armjnts, goal=goal1_armjnts, ctcallback=ctcallback, starttreesamplerate=30.0, endtreesamplerate=30.0, expanddis=10, maxiter=2000.0, maxtime=100.0) smoother = sm.Smoother() #calculate the object pose relative to the end effector pose rbtmesh = rbtmg.genmnp(rbt, jawwidthlft=obj.height) leepos = rbt.lftarm[-1]['linkend']