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 __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 updateshow(objms, numikrms, jawwidth, nxtmnp, objmnp, counter, nxtrobot, handpkg, objpath, task): if counter[0] < len(numikrms): if nxtmnp[0] is not None: nxtmnp[0].detachNode() if objmnp[0] is not None: objmnp[0].detachNode() print counter[0] print numikrms[counter[0]] nxtrobot.movearmfkr(numikrms[counter[0]]) #move robot alljnts = [numikrms[counter[0]][0], 0, 0] alljnts.extend([i for i in numikrms[counter[0]][1]]) alljnts.extend([i for i in nxtrobot.initjnts[9:]]) nxtrobot.movealljnts(alljnts) nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]]) nxtrobot.goinitpose() nxtmnp[0].reparentTo(base.render) objmnp[0] = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1)) objmnp[0].setMat(objms[counter[0]]) objmnp[0].reparentTo(base.render) if counter[0] >= 1: if base.inputmgr.keyMap['space'] is False: pass else: c2e = counter[0] - 1 alljnts = [numikrms[c2e][0], 0, 0] alljnts.extend([i for i in numikrms[c2e][1]]) alljnts.extend([i for i in nxtrobot.initjnts[9:]]) nxts.movejnts15(alljnts) if jawwidth[c2e] < 85: rtq85rs.openhandto(0) else: rtq85rs.openhandto(85) counter[0] += 1 base.inputmgr.keyMap['space'] = False else: counter[0] += 1 else: if base.inputmgr.keyMap['space'] is True: c2e = counter[0] - 1 alljnts = [numikrms[c2e][0], 0, 0] alljnts.extend([i for i in numikrms[c2e][1]]) alljnts.extend([i for i in nxtrobot.initjnts[9:]]) nxts.movejnts15(alljnts) if jawwidth[c2e] < 85: rtq85rs.openhandto(0) else: rtq85rs.openhandto(85) nxts.movejnts15(nxtrobot.initjnts) return task.done return task.again
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp, obj1mnp, counter, nxtrobot, obj0path, obj1path, task): base.win.saveScreenshot(Filename(str(counter[0]) + '.jpg')) if counter[0] < len(numikrms): if nxtmnp[0] is not None: nxtmnp[0].detachNode() if obj0mnp[0] is not None: obj0mnp[0].detachNode() if obj1mnp[0] is not None: obj1mnp[0].detachNode() print counter[0] print numikrms[counter[0]] alljnts = [numikrms[counter[0]][0], 0, 0] alljnts.extend([i for i in numikrms[counter[0]][1]]) alljnts.extend([i for i in numikrms[counter[0]][2]]) nxtrobot.movealljnts(alljnts) nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]][0], jawwidthlft=jawwidth[counter[0]][1]) nxtrobot.goinitpose() nxtmnp[0].reparentTo(base.render) obj0mnp[0] = pg.genObjmnp(obj0path, color=Vec4(.7, .3, 0, 1)) obj0mnp[0].setMat(objms0[counter[0]]) # pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]]) obj0mnp[0].reparentTo(base.render) obj1mnp[0] = pg.genObjmnp(obj1path, color=Vec4(0, .3, .7, 1)) obj1mnp[0].setMat(objms1[counter[0]]) # pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]]) obj1mnp[0].reparentTo(base.render) counter[0] += 1 else: if nxtmnp[0] is not None: nxtmnp[0].detachNode() nxtrobot.goinitpose() nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg) nxtmnp[0].reparentTo(base.render) # counter[0] = 0 return task.done pass return task.again
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp, obj1mnp, counter, nxtrobot, obj0path, obj1path, task): base.win.saveScreenshot(Filename(str(counter[0])+'.jpg')) if counter[0] < len(numikrms): if nxtmnp[0] is not None: nxtmnp[0].detachNode() if obj0mnp[0] is not None: obj0mnp[0].detachNode() if obj1mnp[0] is not None: obj1mnp[0].detachNode() print counter[0] print numikrms[counter[0]] alljnts = [numikrms[counter[0]][0], 0, 0] alljnts.extend([i for i in numikrms[counter[0]][1]]) alljnts.extend([i for i in numikrms[counter[0]][2]]) nxtrobot.movealljnts(alljnts) nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]][0], jawwidthlft=jawwidth[counter[0]][1]) nxtrobot.goinitpose() nxtmnp[0].reparentTo(base.render) obj0mnp[0] = pg.genObjmnp(obj0path, color = Vec4(.7,.3,0,1)) obj0mnp[0].setMat(objms0[counter[0]]) # pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]]) obj0mnp[0].reparentTo(base.render) obj1mnp[0] = pg.genObjmnp(obj1path, color = Vec4(0,.3,.7,1)) obj1mnp[0].setMat(objms1[counter[0]]) # pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]]) obj1mnp[0].reparentTo(base.render) counter[0] += 1 else: if nxtmnp[0] is not None: nxtmnp[0].detachNode() nxtrobot.goinitpose() nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg) nxtmnp[0].reparentTo(base.render) # counter[0] = 0 return task.done pass return task.again
def updateshow(objms, numikrms, jawwidth, hrp5mnp, objmnp, counter, hrp5robot, objpath, task): if counter[0] < len(numikrms): if hrp5mnp[0] is not None: hrp5mnp[0].detachNode() if objmnp[0] is not None: objmnp[0].detachNode() print counter[0] print numikrms[counter[0]] hrp5robot.movearmfkr(numikrms[counter[0]]) hrp5mnp[0] = hrp5plot.genHrp5mnp(hrp5robot, jawwidthrgt=jawwidth[counter[0]]) hrp5robot.goinitpose() hrp5mnp[0].reparentTo(base.render) objmnp[0] = pg.genObjmnp(objpath, color = Vec4(.7,.7,0,1)) objmnp[0].setMat(objms[counter[0]]) # pg.plotAxisSelf(base.render,objms[counter[0]].getRow3(3), objms[counter[0]]) objmnp[0].reparentTo(base.render) counter[0] += 1 return task.again
def updateshow(objms, numikrms, jawwidth, hrp5mnp, objmnp, counter, hrp5robot, objpath, task): if counter[0] < len(numikrms): if hrp5mnp[0] is not None: hrp5mnp[0].detachNode() if objmnp[0] is not None: objmnp[0].detachNode() print counter[0] print numikrms[counter[0]] hrp5robot.movearmfkr(numikrms[counter[0]]) hrp5mnp[0] = hrp5plot.genHrp5mnp(hrp5robot, jawwidthrgt=jawwidth[counter[0]]) hrp5robot.goinitpose() hrp5mnp[0].reparentTo(base.render) objmnp[0] = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1)) objmnp[0].setMat(objms[counter[0]]) # pg.plotAxisSelf(base.render,objms[counter[0]].getRow3(3), objms[counter[0]]) objmnp[0].reparentTo(base.render) counter[0] += 1 return task.again
def updateshow(rbtmnp, objmnp, counter, robot, objpath, task): if counter[0] < len(path): if rbtmnp[0] is not None: # rbtmnp[0].removeNode() pass if objmnp[0] is not None: objmnp[0].removeNode() robot.movearmfk(path[counter[0]], armid='rgt') rbtmnp[0] = robotmesh.gensnp(robot=robot) rbtmnp[0].reparentTo(base.render) objmnp[0] = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1)) objmnp[0].setMat(obsrotmat4) objmnp[0].reparentTo(base.render) counter[0] += 1 else: if rbtmnp[0] is not None: rbtmnp[0].removeNode() robot.goinitpose() rbtmnp[0] = robotmesh.genmnp(robot) rbtmnp[0].reparentTo(base.render) return task.again
result = np.asarray(result) idsglist = [int(x) for x in result[:, 0]] sgrotmatlist = [dc.strToMat4(x) for x in result[:, 1]] idfreeplacementlist = [int(x) for x in result[:, 2]] for idstart, start in zip(idsglist, sgrotmatlist): for idgoal, goal in zip(idsglist, sgrotmatlist): # print idstart, idgoal if idstart == 2 and idgoal == 1: print start, goal startrotmat4 = start goalrotmat4 = goal regrip.findshortestpath(start, goal, base) break # objstart = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1)) objstart.setMat(startrotmat4) objend = pg.genObjmnp(objpath, color=Vec4(.3, .3, 0, .5)) objend.setMat(goalrotmat4) objstart.reparentTo(base.render) objend.reparentTo(base.render) print "path result on table", len(regrip.directshortestpaths) # # # # toc = time.clock() # # # # print len(regrip.directshortestpaths[0]) # # # # print toc-tic # # # # assert False # # # # pltfig = plt.figure() regrip.plotgraph(pltfig)
jointlimits = [[robot.rgtarm[1]['rngmin'], robot.rgtarm[1]['rngmax']], [robot.rgtarm[2]['rngmin'], robot.rgtarm[2]['rngmax']], [robot.rgtarm[3]['rngmin'], robot.rgtarm[3]['rngmax']], [robot.rgtarm[4]['rngmin'], robot.rgtarm[4]['rngmax']], [robot.rgtarm[5]['rngmin'], robot.rgtarm[5]['rngmax']], [robot.rgtarm[6]['rngmin'], robot.rgtarm[6]['rngmax']], [robot.rgtarm[7]['rngmin'], robot.rgtarm[7]['rngmax']], [robot.rgtarm[8]['rngmin'], robot.rgtarm[8]['rngmax']], [robot.rgtarm[9]['rngmin'], robot.rgtarm[9]['rngmax']]] import os from panda3d.core import * import pandaplotutils.pandageom as pg obsrotmat4 = Mat4(1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,405.252044678,-300.073120117,300.0000038147,1.0) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(os.path.split(this_dir)[0], "manipulation/grip", "objects", "tool.stl") objmnp = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1)) objmnp.setMat(obsrotmat4) objmnp.reparentTo(base.render) # planner = rrt.RRT(start=start, goal=goal, iscollidedfunc = iscollidedfunc, # jointlimits = jointlimits, expanddis = 5, # robot = robot, cdchecker = cdchecker) # planner = rrtc.RRTConnect(start=start, goal=goal, iscollidedfunc = iscollidedfunc, # jointlimits = jointlimits, expanddis = 10, robot = robot, cdchecker = cdchecker) # planner = ddrrt.DDRRT(start=start, goal=goal, iscollidedfunc = iscollidedfunc, # jointlimits = jointlimits, goalsamplerate=30, expanddis = 5, robot = robot, # cdchecker = cdchecker) # planner = ddrrtc.DDRRTConnect(start=start, goal=goal, iscollidedfunc = iscollidedfunc, jointlimits = jointlimits, starttreesamplerate=30, expanddis = 5, robot = robot, cdchecker = cdchecker)
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp, obj1mnp, counter, nxtrobot, obj0path, obj1path, task): if counter[0] < len(numikrms): if nxtmnp[0] is not None: nxtmnp[0].detachNode() if obj0mnp[0] is not None: obj0mnp[0].detachNode() if obj1mnp[0] is not None: obj1mnp[0].detachNode() alljnts = [numikrms[counter[0]][0], 0, 0] alljnts.extend([i for i in numikrms[counter[0]][1]]) alljnts.extend([i for i in numikrms[counter[0]][2]]) nxtrobot.movealljnts(alljnts) nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]][0], jawwidthlft=jawwidth[counter[0]][1]) nxtrobot.goinitpose() nxtmnp[0].reparentTo(base.render) obj0mnp[0] = pg.genObjmnp(obj0path, color=Vec4(.7, .3, 0, 1)) obj0mnp[0].setMat(objms0[counter[0]]) # pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]]) obj0mnp[0].reparentTo(base.render) obj1mnp[0] = pg.genObjmnp(obj1path, color=Vec4(0, .3, .7, 1)) obj1mnp[0].setMat(objms1[counter[0]]) # pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]]) obj1mnp[0].reparentTo(base.render) if counter[0] >= 1: print base.inputmgr.keyMap['space'] if base.inputmgr.keyMap['space'] is False: pass else: c2e = counter[0] - 1 alljnts = [numikrms[c2e][0], 0, 0] alljnts.extend([i for i in numikrms[c2e][1]]) alljnts.extend([i for i in numikrms[c2e][2]]) nxts.movejnts15(alljnts) if jawwidth[c2e][0] < 85: rtq85rs.openhandto(0) else: rtq85rs.openhandto(85) if jawwidth[c2e][1] < 85: rtq85ls.openhandto(0) else: rtq85ls.openhandto(85) counter[0] += 1 base.inputmgr.keyMap['space'] = False else: counter[0] += 1 else: if base.inputmgr.keyMap['space'] is True: c2e = counter[0] - 1 alljnts = [numikrms[c2e][0], 0, 0] alljnts.extend([i for i in numikrms[c2e][1]]) alljnts.extend([i for i in numikrms[c2e][2]]) # nxts.movejnts15(alljnts) if jawwidth[c2e][0] < 85: rtq85rs.openhandto(0) else: rtq85rs.openhandto(85) if jawwidth[c2e][1] < 85: rtq85ls.openhandto(0) else: rtq85ls.openhandto(85) # if nxtmnp[0] is not None: # nxtmnp[0].detachNode() # nxtrobot.goinitpose() # nxtmnp[0] = nxtplot.genNxtmnp(nxtrobot) # nxtmnp[0].reparentTo(base.render) # counter[0] = 0 return task.done return task.again
-2.18294604779e-16, -0.98311150074, -0.183007523417, 0.0, 399.926879883, -196.8688812256, 79.7615509033, 1.0) # startrotmat4 = Mat4(0.129405856133,0.129405856133,0.98311150074,0.0, # 0.707106769085,-0.707106769085,0.0,0.0, # 0.69516479969,0.69516479969,-0.183007523417,0.0, # 327.126998901,-102.976409912,19.7615509033,1.0) # startrotmat4 = Mat4(-0.707106769085, 0.707106769085, 0.0, 0.0, -0.707106769085, -0.707106769085, 0.0, 0.0, 0.0, 0.0, # 1.0, 0.0, 396.234527588, -396.33795166, -55.0000038147, 1.0) goalrotmat4 = Mat4(-0.707106769085, -0.707106769085, 0.0, 0.0, 0.707106769085, -0.707106769085, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 406.33795166, 103.76546859741, -55.0000038147, 1.0) # goalrotmat4 = Mat4(-1.0,1.22464685259e-16,0.0,0.0,-1.22464685259e-16,-1.0,0.0,0.0,0.0,0.0,1.0,0.0,294.747955322,-200.073135376,-55.0000038147,1.0) # goalrotmat4 = Mat4(-0.707106769085,0.707106769085,0.0,0.0,-0.707106769085,-0.707106769085,0.0,0.0,0.0,0.0,1.0,0.0,396.234527588,-396.33795166,-55.0000038147,1.0) objstart = pg.genObjmnp(objpath, color=Vec4(.3, .0, .0, .1)) objstart.setMat(startrotmat4) objend = pg.genObjmnp(objpath, color=Vec4(.0, .3, .0, .1)) objend.setMat(goalrotmat4) objstart.reparentTo(base.render) objend.reparentTo(base.render) #tool # startrotmat4 = Mat4(-0.0176398064941,-0.0176398064941,-0.99968880415,0.0,-0.707106769085,0.707106769085,0.0,0.0,0.706886708736,0.706886708736,-0.0249464549124,0.0,225.010162354,100,44.9175643921,1.0) # startrotmat4 = Mat4(0.129405856133,0.129405856133,0.98311150074,0.0,0.707106769085,-0.707106769085,0.0,0.0,0.69516479969,0.69516479969,-0.183007523417,0.0,227.126983643,-327.023590088,74.7615509033,1.0) # goalrotmat4 = Mat4(-1.0,1.22464685259e-16,0.0,0.0,-1.22464685259e-16,-1.0,0.0,0.0,0.0,0.0,1.0,0.0,294.747955322,-300.0731293559074,-3.99246982852e-06,1.0) # planewheel # startrotmat4 = Mat4(0.707106769085,0.707106769085,0.0,0.0,-4.32978030171e-17,4.32978030171e-17,-1.0,0.0,-0.707106769085,0.707106769085,6.12323426293e-17,0.0,400.0,-400.0,29.9999980927,1.0) # goalrotmat4 = Mat4(0.707106769085,-0.707106769085,0.0,0.0,4.32978030171e-17,4.32978030171e-17,-1.0,0.0,0.707106769085,0.707106769085,6.12323426293e-17,0.0,400.0,-1.7017070846e-15,29.9999980927,1.0) # planelowerbody # startrotmat4 = Mat4(1.35963113277e-32,6.12323426293e-17,-1.0,0.0,-1.0,2.22044604925e-16,0.0,0.0,2.22044604925e-16,1.0,6.12323426293e-17,0.0,399.997558594,-16.3771038055,74.2884140015,1.0)
import numpy as np import pandaplotutils.pandactrl as pc import pandaplotutils.pandageom as pg import utils.robotmath as rm import trimesh import os from panda3d.core import Filename, TransparencyAttrib base = pc.World(camp=[1000, -300, 700], lookatp=[0, 10, 0], w=1000, h=1000) pggen = pg.PandaGeomGen() pggen.plotAxis(base.render) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "sandpart.stl") # objtrimesh=trimesh.load_mesh(objpath) # # # objmnp = pg.trimesh2Panda(objtrimesh) objmnp = pg.genObjmnp(objpath, color=(0, 0, 0, 1)) # # objmnp.reparentTo(base.render) # # eggpath = Filename.fromOsSpecific(os.path.join(this_dir, "bzcube.egg")) # objmnp2 = loader.loadModel(eggpath) # objmnp2.setScale(5.0) # objmnp2.setColor(0,0,1, 0.7) # objmnp2.setTransparency(TransparencyAttrib.MAlpha) # objmnp2.reparentTo(base.render) base.run()
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp, obj1mnp, counter, nxtrobot, obj0path, obj1path, task): if counter[0] < len(numikrms): if nxtmnp[0] is not None: nxtmnp[0].detachNode() if obj0mnp[0] is not None: obj0mnp[0].detachNode() if obj1mnp[0] is not None: obj1mnp[0].detachNode() alljnts = [numikrms[counter[0]][0], 0, 0] alljnts.extend([i for i in numikrms[counter[0]][1]]) alljnts.extend([i for i in numikrms[counter[0]][2]]) nxtrobot.movealljnts(alljnts) nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]][0], jawwidthlft=jawwidth[counter[0]][1]) nxtrobot.goinitpose() nxtmnp[0].reparentTo(base.render) obj0mnp[0] = pg.genObjmnp(obj0path, color = Vec4(.7,.3,0,1)) obj0mnp[0].setMat(objms0[counter[0]]) # pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]]) obj0mnp[0].reparentTo(base.render) obj1mnp[0] = pg.genObjmnp(obj1path, color = Vec4(0,.3,.7,1)) obj1mnp[0].setMat(objms1[counter[0]]) # pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]]) obj1mnp[0].reparentTo(base.render) if counter[0] >= 1: print base.inputmgr.keyMap['space'] if base.inputmgr.keyMap['space'] is False: pass else: c2e = counter[0]-1 alljnts = [numikrms[c2e][0], 0, 0] alljnts.extend([i for i in numikrms[c2e][1]]) alljnts.extend([i for i in numikrms[c2e][2]]) nxts.movejnts15(alljnts) if jawwidth[c2e][0] < 85: rtq85rs.openhandto(0) else: rtq85rs.openhandto(85) if jawwidth[c2e][1] < 85: rtq85ls.openhandto(0) else: rtq85ls.openhandto(85) counter[0] += 1 base.inputmgr.keyMap['space'] = False else: counter[0] += 1 else: if base.inputmgr.keyMap['space'] is True: c2e = counter[0]-1 alljnts = [numikrms[c2e][0], 0, 0] alljnts.extend([i for i in numikrms[c2e][1]]) alljnts.extend([i for i in numikrms[c2e][2]]) # nxts.movejnts15(alljnts) if jawwidth[c2e][0] < 85: rtq85rs.openhandto(0) else: rtq85rs.openhandto(85) if jawwidth[c2e][1] < 85: rtq85ls.openhandto(0) else: rtq85ls.openhandto(85) # if nxtmnp[0] is not None: # nxtmnp[0].detachNode() # nxtrobot.goinitpose() # nxtmnp[0] = nxtplot.genNxtmnp(nxtrobot) # nxtmnp[0].reparentTo(base.render) # counter[0] = 0 return task.done return task.again
nxtrobot.movearmfkr(numikrms[counter[0]]) nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]]) nxtrobot.goinitpose() nxtmnp[0].reparentTo(base.render) objmnp[0] = pg.genObjmnp(objpath, color = Vec4(.7,.7,0,1)) objmnp[0].setMat(objms[counter[0]]) # pg.plotAxisSelf(base.render,objms[counter[0]].getRow3(3), objms[counter[0]]) objmnp[0].reparentTo(base.render) counter[0] += 1 return task.again taskMgr.doMethodLater(1, updateshow, "updateshow", extraArgs = [objms, numikrms, jawwidth, nxtmnp, objmnp, counter, nxtrobot, handpkg, objpath], appendTask = True) # one time show for start and end objstart = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1)) objstart.setMat(startrotmat4) objend = pg.genObjmnp(objpath, color=Vec4(.3, .3, 0, .5)) objend.setMat(goalrotmat4) objstart.reparentTo(base.render) objend.reparentTo(base.render) # this_dir, this_filename = os.path.split(__file__) # ttpath = Filename.fromOsSpecific( # os.path.join(os.path.split(this_dir)[0] + os.sep, "grip", "supports", "tabletop.egg")) # ttnodepath = NodePath("tabletop") # ttl = loader.loadModel(ttpath) # ttl.instanceTo(ttnodepath) # ttnodepath.reparentTo(base.render)
hrp5nmnp = hrp5nmeshgen.genmnp(hrp5nrobot) # hrp5nmnp.reparentTo(base.render) startrotmat4 = Mat4(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 305.252044678, -400.073120117, -55.0000038147, 1.0) startrotmat4 = Mat4(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 405.252044678, -199.926864624, -55.0000038147, 1.0) import os this_dir, this_filename = os.path.split(__file__) objpath = os.path.join( os.path.split(this_dir)[0], "../manipulation/grip", "objects", "tool.stl") objmnp = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1)) objmnp.setMat(startrotmat4) # objmnp.reparentTo(base.render) # goal hand # from manipulation.grip.robotiq85 import rtq85nm # hrp5robotrgthnd = rtq85nm.Rtq85NM() # hrp5robotrgthnd.setColor([1,0,0,.3]) # hrp5robotrgtarmlj9_rotmat = pandageom.cvtMat4(objrot, objpos+objrot[:,0]*130) # pg.plotAxisSelf(base.render, objpos, hrp5robotrgtarmlj9_rotmat) # hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat) # hrp5robotrgthnd.reparentTo(base.render) # # angle = nxtik.eurgtbik(objpos) # nxtrobot.movewaist(angle) # armjntsgoal=nxtik.numik(nxtrobot, objpos, objrot) #
# ttube startrotmat4 = Mat4(-4.06358332355e-17,-0.183007523417,0.98311150074,0.0, -1.0,2.22044604925e-16,0.0,0.0, -2.18294604779e-16,-0.98311150074,-0.183007523417,0.0, 399.926879883,-196.8688812256,79.7615509033,1.0) # startrotmat4 = Mat4(0.129405856133,0.129405856133,0.98311150074,0.0, # 0.707106769085,-0.707106769085,0.0,0.0, # 0.69516479969,0.69516479969,-0.183007523417,0.0, # 327.126998901,-102.976409912,19.7615509033,1.0) # startrotmat4 = Mat4(-0.707106769085, 0.707106769085, 0.0, 0.0, -0.707106769085, -0.707106769085, 0.0, 0.0, 0.0, 0.0, # 1.0, 0.0, 396.234527588, -396.33795166, -55.0000038147, 1.0) goalrotmat4 = Mat4(-0.707106769085,-0.707106769085,0.0,0.0,0.707106769085,-0.707106769085,0.0,0.0,0.0,0.0,1.0,0.0,406.33795166,103.76546859741,-55.0000038147,1.0) # goalrotmat4 = Mat4(-1.0,1.22464685259e-16,0.0,0.0,-1.22464685259e-16,-1.0,0.0,0.0,0.0,0.0,1.0,0.0,294.747955322,-200.073135376,-55.0000038147,1.0) # goalrotmat4 = Mat4(-0.707106769085,0.707106769085,0.0,0.0,-0.707106769085,-0.707106769085,0.0,0.0,0.0,0.0,1.0,0.0,396.234527588,-396.33795166,-55.0000038147,1.0) objstart = pg.genObjmnp(objpath, color=Vec4(.3, .0, .0, .1)) objstart.setMat(startrotmat4) objend = pg.genObjmnp(objpath, color=Vec4(.0, .3, .0, .1)) objend.setMat(goalrotmat4) objstart.reparentTo(base.render) objend.reparentTo(base.render) #tool # startrotmat4 = Mat4(-0.0176398064941,-0.0176398064941,-0.99968880415,0.0,-0.707106769085,0.707106769085,0.0,0.0,0.706886708736,0.706886708736,-0.0249464549124,0.0,225.010162354,100,44.9175643921,1.0) # startrotmat4 = Mat4(0.129405856133,0.129405856133,0.98311150074,0.0,0.707106769085,-0.707106769085,0.0,0.0,0.69516479969,0.69516479969,-0.183007523417,0.0,227.126983643,-327.023590088,74.7615509033,1.0) # goalrotmat4 = Mat4(-1.0,1.22464685259e-16,0.0,0.0,-1.22464685259e-16,-1.0,0.0,0.0,0.0,0.0,1.0,0.0,294.747955322,-300.0731293559074,-3.99246982852e-06,1.0) # planewheel # startrotmat4 = Mat4(0.707106769085,0.707106769085,0.0,0.0,-4.32978030171e-17,4.32978030171e-17,-1.0,0.0,-0.707106769085,0.707106769085,6.12323426293e-17,0.0,400.0,-400.0,29.9999980927,1.0) # goalrotmat4 = Mat4(0.707106769085,-0.707106769085,0.0,0.0,4.32978030171e-17,4.32978030171e-17,-1.0,0.0,0.707106769085,0.707106769085,6.12323426293e-17,0.0,400.0,-1.7017070846e-15,29.9999980927,1.0) # planelowerbody # startrotmat4 = Mat4(1.35963113277e-32,6.12323426293e-17,-1.0,0.0,-1.0,2.22044604925e-16,0.0,0.0,2.22044604925e-16,1.0,6.12323426293e-17,0.0,399.997558594,-16.3771038055,74.2884140015,1.0)