def addGround(self): boxx = 1000.0 boxy = 1000.0 boxz = 1.0 offset=-55 pg.plotBox(self.base.render, pos = [0,0,-1.0+offset], x = boxx*2.0, y = boxy*2.0, z = boxz*2.0, rgba=None) pg.plotAxisSelf(self.base.render) groundGeom = OdePlaneGeom(self.odespace, Vec4(0, 0, 1, offset))
def genmnp(self, nxtrobot, jawwidthrgt = None, jawwidthlft = None): """ generate a panda3d nodepath for the nxtrobot mnp indicates this function generates a mesh nodepath :param nxtrobot: the nxtrobot object, see nxtrobot.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20180109 """ identmat4 = Mat4.identMat() # body nxtwaist_rotmat = pg.cvtMat4(nxtrobot.base['rotmat']) self.__nxtwaist_nodepath.setMat(nxtwaist_rotmat) # rgtarm nxtrgtarmlj0_rotmat = pg.cvtMat4(nxtrobot.rgtarm[1]['rotmat'], nxtrobot.rgtarm[1]['linkpos']) self.__nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat) nxtrgtarmlj1_rotmat = pg.cvtMat4(nxtrobot.rgtarm[2]['rotmat'], nxtrobot.rgtarm[2]['linkpos']) self.__nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat) nxtrgtarmlj2_rotmat = pg.cvtMat4(nxtrobot.rgtarm[3]['rotmat'], nxtrobot.rgtarm[3]['linkpos']) self.__nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat) nxtrgtarmlj3_rotmat = pg.cvtMat4(nxtrobot.rgtarm[4]['rotmat'], nxtrobot.rgtarm[4]['linkpos']) self.__nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat) nxtrgtarmlj4_rotmat = pg.cvtMat4(nxtrobot.rgtarm[5]['rotmat'], nxtrobot.rgtarm[5]['linkpos']) self.__nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat) # lftarm nxtlftarmlj0_rotmat = pg.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos']) self.__nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat) nxtlftarmlj1_rotmat = pg.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos']) self.__nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat) nxtlftarmlj2_rotmat = pg.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos']) self.__nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat) nxtlftarmlj3_rotmat = pg.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos']) self.__nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat) nxtlftarmlj4_rotmat = pg.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos']) self.__nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat) # rgthand nxtlftarmlj5_rotmat = pg.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos']) self.nxtrgthnd.setMat(nxtlftarmlj5_rotmat) pg.plotAxisSelf(self.__nxtmnp, nxtrobot.rgtarm[6]['linkend'], nxtlftarmlj5_rotmat) if jawwidthrgt is not None: self.nxtrgthnd.setJawwidth(jawwidthrgt) # lfthand nxtlftarmlj5_rotmat = pg.cvtMat4(nxtrobot.lftarm[6]['rotmat'], nxtrobot.lftarm[6]['linkpos']) self.nxtlfthnd.setMat(nxtlftarmlj5_rotmat) pg.plotAxisSelf(self.__nxtmnp, nxtrobot.lftarm[6]['linkend'], nxtlftarmlj5_rotmat) if jawwidthlft is not None: self.nxtlfthnd.setJawwidth(jawwidthlft) return copy.deepcopy(self.__nxtmnp)
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)
# rtq85hnd.rtq85np.showTightBounds() base.taskMgr.add(updateworld, "updateworld", extraArgs=[base.world], appendTask=True) result = base.world.contactTestPair(ilkbullnode, lftbullnode) print result print result.getContacts() import pandaplotutils.pandageom as pandageom for contact in result.getContacts(): cp = contact.getManifoldPoint() print cp.getLocalPointA() pandageom.plotSphere(base, pos=cp.getLocalPointA(), radius=10, rgba=Vec4(1, 0, 0, 1)) pandageom.plotAxisSelf(base.render, spos=Vec3(0, 0, 0)) debugNode = BulletDebugNode('Debug') debugNode.showWireframe(True) debugNode.showConstraints(True) debugNode.showBoundingBoxes(False) debugNode.showNormals(False) debugNP = bullcldrnp.attachNewNode(debugNode) # debugNP.show() base.world.setDebugNode(debugNP.node()) base.run()
from manipulation.grip.robotiq85 import rtq85nm from manipulation.grip.hrp5three import hrp5threenm from nxtmesh import NxtMesh base = pandactrl.World(camp=[1400,0,3000], lookatp=[0,0,0]) nxtrobot = NxtRobot() nxtrobot.goinitpose() # nxtrobot.movewaist(0) import nxtik objpos = np.array([200,0,300]) # objrot = np.array([[0,1,0],[1,0,0],[0,0,-1]]) objrot = np.array([[0,1,0],[0,0,-1],[-1,0,0]]) # plot goal axis pg.plotAxisSelf(nodepath = base.render, spos = objpos, pandamat4 = pg.cvtMat4(objrot)) import nxtplot handpkg = rtq85nm # handpkg = hrp5threenm # nxtrobot.movewaist(-15) # nxtmnp = nxtplot.genmnp(nxtrobot, handpkg) # nxtmnp.reparentTo(base.render) nmg = NxtMesh(handpkg) nxtmesh = nmg.genmnp(nxtrobot) nxtmesh.reparentTo(base.render) # nxtplot.plotstick(base.render, nxtrobot) # # # angle = nxtik.eurgtbik(objpos)
# hrp5mnp = hrp5plot.genHrp5mnp(hrp5robot) # hrp5mnp.reparentTo(base.render) armjntsgoal = hrp5robot.numik(objpos, objrot, armid) if armjntsgoal is not None: hrp5robot.movearmfk6(armjntsgoal, armid) hrp5plot.plotstick(base.render, hrp5robot) hrp5mnp = hrp5plot.genHrp5mnp(hrp5robot) # hrp5mnp.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) # # # nxtplot.plotstick(base.render, nxtrobot) # pandamat4=Mat4() # pandamat4.setRow(3,Vec3(0,0,250)) # # pg.plotAxis(base.render, pandamat4) # # nxtplot.plotmesh(base, nxtrobot) # # pandageom.plotAxis(base.render, pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos'])) pg.plotDumbbell(base.render, objpos, objpos, rgba=[1, 0, 0, 1]) # pg.plotArrow(base.render, hrp5robot.rgtarm[8]['linkpos'], hrp5robot.rgtarm[8]['linkpos']+hrp5robot.rgtarm[8]['rotax']*1000)
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) # # # nxtplot.plotstick(base.render, nxtrobot) # pandamat4=Mat4() # pandamat4.setRow(3,Vec3(0,0,250)) # # pg.plotAxis(base.render, pandamat4) # # nxtplot.plotmesh(base, nxtrobot) # # pandageom.plotAxis(base.render, pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos'])) # pg.plotDumbbell(base.render, objpos, objpos, rgba = [1,0,0,1]) # pg.plotAxisSelf(base.render, objpos, pg.npToMat4(objrot), thickness=20) # pg.plotArrow(base.render, hrp5robot.rgtarm[8]['linkpos'], hrp5robot.rgtarm[8]['linkpos']+hrp5robot.rgtarm[8]['rotax']*1000) # # # nxtrobot.movearmfk6(armjntsgoal) # # nxtmnp = nxtplot.genNxtmnp_nm(nxtrobot, plotcolor=[1,0,0,1]) # # nxtmnp.reparentTo(base.render) pg.plotAxisSelf(base.render, Vec3(400, 0, 150), Mat4.identMat()) base.run()
hrp5nrobot = Hrp5NRobot() # hrp5nrobot.goinitpose() import hrp5nplot hrp5nplot.plotstick(base.render, hrp5nrobot) handpkg = hrp5threenm # handpkg = rtq85nm hrp5nmeshgen = hrp5nmesh.Hrp5NMesh(handpkg) hrp5nmnp = hrp5nmeshgen.genmnp(hrp5nrobot) hrp5nmnp.reparentTo(base.render) objpos = np.array([500,-400,305]) objrot = np.array([[-1,0,0],[0,1,0],[0,0,-1]]) # plot goal axis pg.plotAxisSelf(nodepath = base.render, spos = objpos, pandamat4 = pg.cvtMat4(objrot)) # objrot = np.array([[0.125178158283, 0.00399381108582, 0.992126166821], [0.98617619276, -0.109927728772, -0.123984932899], [0.108567006886, 0.993931531906, -0.0176991540939]]).T # objrot = np.array([[0,1,0],[0,0,1],[1,0,0]]) # objrotmat4 = pg.npToMat4(objrot) # objrotmat4 = objrotmat4*Mat4.rotateMat(30, Vec3(1,0,0)) # objrot = pg.mat3ToNp(objrotmat4.getUpper3()) # objpos = np.array([401.67913818,-644.12841797,0]) # objrot = np.array([[1.93558640e-06,-8.36298645e-01,5.48274219e-01], # [1.93560686e-06,-5.48274219e-01,-8.36298645e-01], # [1.00000000e+00,2.67997166e-06,5.57513317e-07]]) # lfthnd # objpos = np.array([180,130,100]) # objrot = np.array([[0,0,-1],[1,0,0],[0,-1,0]]) armid="rgt" armjntsgoal = hrp5nrobot.numikr(objpos, objrot, armid)
def genmnp(ur5dual, handpkg, jawwidthrgt=None, jawwidthlft=None): """ generate a panda3d nodepath for the ur5dual mnp indicates this function generates a mesh nodepath :param ur5dual: :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20161202 """ ur5mnp = NodePath("ur5dualmnp") this_dir, this_filename = os.path.split(__file__) # chest0-2, head1 (neck is not plotted) ur5base_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "ur5egg", "base.egg")) ur5upperarm_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "ur5egg", "upperarm.egg")) ur5shoulder_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "ur5egg", "shoulder.egg")) ur5forearm_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "ur5egg", "forearm.egg")) ur5wrist1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "ur5egg", "wrist1.egg")) ur5wrist2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "ur5egg", "wrist2.egg")) ur5wrist3_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "ur5egg", "wrist3.egg")) ur5base_model = loader.loadModel(ur5base_filepath) ur5upperarm_model = loader.loadModel(ur5upperarm_filepath) ur5shoulder_model = loader.loadModel(ur5shoulder_filepath) ur5forearm_model = loader.loadModel(ur5forearm_filepath) ur5wrist1_model = loader.loadModel(ur5wrist1_filepath) ur5wrist2_model = loader.loadModel(ur5wrist2_filepath) ur5wrist3_model = loader.loadModel(ur5wrist3_filepath) # rgt ur5rgtbase_nodepath = NodePath("ur5rgtbase") ur5rgtshoulder_nodepath = NodePath("ur5rgtshoulder") ur5rgtupperarm_nodepath = NodePath("ur5rgtupperarm") ur5rgtforearm_nodepath = NodePath("ur5rgtforearm") ur5rgtwrist1_nodepath = NodePath("ur5rgtwrist1") ur5rgtwrist2_nodepath = NodePath("ur5rgtwrist2") ur5rgtwrist3_nodepath = NodePath("ur5rgtwrist3") # # base egg coordinates is retracted 89.2 along z to fit parameters ur5base_model.instanceTo(ur5rgtbase_nodepath) ur5base_rotmat = pg.cvtMat4(ur5dual.rgtarm[1]['inherentR'], ur5dual.rgtarm[1]['linkpos']) ur5rgtbase_nodepath.setMat(ur5base_rotmat) ur5rgtbase_nodepath.setColor(.5, .5, .5, 1) ur5rgtbase_nodepath.reparentTo(ur5mnp) # ur5shoulder_model.instanceTo(ur5rgtshoulder_nodepath) ur5shoulder_rotmat = pg.cvtMat4(ur5dual.rgtarm[1]['rotmat'], ur5dual.rgtarm[1]['linkpos']) ur5rgtshoulder_nodepath.setMat(ur5shoulder_rotmat) ur5rgtshoulder_nodepath.setColor(.5, .7, .3, 1) ur5rgtshoulder_nodepath.reparentTo(ur5mnp) # ur5upperarm_model.instanceTo(ur5rgtupperarm_nodepath) ur5upperarm_rotmat = pg.cvtMat4(ur5dual.rgtarm[2]['rotmat'], ur5dual.rgtarm[2]['linkpos']) ur5rgtupperarm_nodepath.setMat(ur5upperarm_rotmat) ur5rgtupperarm_nodepath.setColor(.5, .5, .5, 1) ur5rgtupperarm_nodepath.reparentTo(ur5mnp) # ur5forearm_model.instanceTo(ur5rgtforearm_nodepath) ur5forearm_rotmat = pg.cvtMat4(ur5dual.rgtarm[3]['rotmat'], ur5dual.rgtarm[3]['linkpos']) ur5rgtforearm_nodepath.setMat(ur5forearm_rotmat) ur5rgtforearm_nodepath.setColor(.5, .5, .5, 1) ur5rgtforearm_nodepath.reparentTo(ur5mnp) # ur5wrist1_model.instanceTo(ur5rgtwrist1_nodepath) ur5wrist1_rotmat = pg.cvtMat4(ur5dual.rgtarm[4]['rotmat'], ur5dual.rgtarm[4]['linkpos']) ur5rgtwrist1_nodepath.setMat(ur5wrist1_rotmat) ur5rgtwrist1_nodepath.setColor(.5, .7, .3, 1) ur5rgtwrist1_nodepath.reparentTo(ur5mnp) # ur5wrist2_model.instanceTo(ur5rgtwrist2_nodepath) ur5wrist2_rotmat = pg.cvtMat4(ur5dual.rgtarm[5]['rotmat'], ur5dual.rgtarm[5]['linkpos']) ur5rgtwrist2_nodepath.setMat(ur5wrist2_rotmat) ur5rgtwrist2_nodepath.setColor(.5, .5, .5, 1) ur5rgtwrist2_nodepath.reparentTo(ur5mnp) # # wrist3 egg coordinates is rotated 90 around z retracted 72.33752-81.82489 to fit parameters ur5wrist3_model.instanceTo(ur5rgtwrist3_nodepath) ur5wrist3_rotmat = pg.cvtMat4(ur5dual.rgtarm[6]['rotmat'], ur5dual.rgtarm[6]['linkpos']) ur5rgtwrist3_nodepath.setMat(ur5wrist3_rotmat) ur5rgtwrist3_nodepath.setColor(.5, .5, .5, 1) ur5rgtwrist3_nodepath.reparentTo(ur5mnp) # lft ur5lftbase_nodepath = NodePath("ur5lftbase") ur5lftupperarm_nodepath = NodePath("ur5lftupperarm") ur5lftshoulder_nodepath = NodePath("ur5lftshoulder") ur5lftforearm_nodepath = NodePath("ur5lftforearm") ur5lftwrist1_nodepath = NodePath("ur5lftwrist1") ur5lftwrist2_nodepath = NodePath("ur5lftwrist2") ur5lftwrist3_nodepath = NodePath("ur5lftwrist3") # # base egg coordinates is retracted 89.2 along z to fit parameters ur5base_model.instanceTo(ur5lftbase_nodepath) ur5base_rotmat = pg.cvtMat4(ur5dual.lftarm[1]['inherentR'], ur5dual.lftarm[1]['linkpos']) ur5lftbase_nodepath.setMat(ur5base_rotmat) ur5lftbase_nodepath.setColor(.5, .5, .5, 1) ur5lftbase_nodepath.reparentTo(ur5mnp) # ur5shoulder_model.instanceTo(ur5lftshoulder_nodepath) ur5shoulder_rotmat = pg.cvtMat4(ur5dual.lftarm[1]['rotmat'], ur5dual.lftarm[1]['linkpos']) ur5lftshoulder_nodepath.setMat(ur5shoulder_rotmat) ur5lftshoulder_nodepath.setColor(.5, .7, .3, 1) ur5lftshoulder_nodepath.reparentTo(ur5mnp) # ur5upperarm_model.instanceTo(ur5lftupperarm_nodepath) ur5upperarm_rotmat = pg.cvtMat4(ur5dual.lftarm[2]['rotmat'], ur5dual.lftarm[2]['linkpos']) ur5lftupperarm_nodepath.setMat(ur5upperarm_rotmat) ur5lftupperarm_nodepath.setColor(.5, .5, .5, 1) ur5lftupperarm_nodepath.reparentTo(ur5mnp) # ur5forearm_model.instanceTo(ur5lftforearm_nodepath) ur5forearm_rotmat = pg.cvtMat4(ur5dual.lftarm[3]['rotmat'], ur5dual.lftarm[3]['linkpos']) ur5lftforearm_nodepath.setMat(ur5forearm_rotmat) ur5lftforearm_nodepath.setColor(.5, .5, .5, 1) ur5lftforearm_nodepath.reparentTo(ur5mnp) # ur5wrist1_model.instanceTo(ur5lftwrist1_nodepath) ur5wrist1_rotmat = pg.cvtMat4(ur5dual.lftarm[4]['rotmat'], ur5dual.lftarm[4]['linkpos']) ur5lftwrist1_nodepath.setMat(ur5wrist1_rotmat) ur5lftwrist1_nodepath.setColor(.5, .7, .3, 1) ur5lftwrist1_nodepath.reparentTo(ur5mnp) # ur5wrist2_model.instanceTo(ur5lftwrist2_nodepath) ur5wrist2_rotmat = pg.cvtMat4(ur5dual.lftarm[5]['rotmat'], ur5dual.lftarm[5]['linkpos']) ur5lftwrist2_nodepath.setMat(ur5wrist2_rotmat) ur5lftwrist2_nodepath.setColor(.5, .5, .5, 1) ur5lftwrist2_nodepath.reparentTo(ur5mnp) # ur5wrist3_model.instanceTo(ur5lftwrist3_nodepath) ur5wrist3_rotmat = pg.cvtMat4(ur5dual.lftarm[6]['rotmat'], ur5dual.lftarm[6]['linkpos']) ur5lftwrist3_nodepath.setMat(ur5wrist3_rotmat) ur5lftwrist3_nodepath.setColor(.5, .5, .5, 1) ur5lftwrist3_nodepath.reparentTo(ur5mnp) # rgthnd ur5robotrgthnd = handpkg.newHand('rgt') ur5robotrgtarmljend_rotmat = pg.cvtMat4(ur5dual.rgtarm[6]['rotmat'], ur5dual.rgtarm[6]['linkpos']) pg.plotAxisSelf(ur5mnp, ur5dual.rgtarm[6]['linkend'], ur5robotrgtarmljend_rotmat) ur5robotrgthnd.setMat(ur5robotrgtarmljend_rotmat) ur5robotrgthnd.reparentTo(ur5mnp) if jawwidthrgt is not None: ur5robotrgthnd.setJawwidth(jawwidthrgt) # lfthnd ur5robotlfthnd = handpkg.newHand('lft') ur5robotlftarmljend_rotmat = pg.cvtMat4(ur5dual.lftarm[6]['rotmat'], ur5dual.lftarm[6]['linkpos']) pg.plotAxisSelf(ur5mnp, ur5dual.lftarm[6]['linkend'], ur5robotlftarmljend_rotmat) ur5robotlfthnd.setMat(ur5robotlftarmljend_rotmat) ur5robotlfthnd.reparentTo(ur5mnp) if jawwidthlft is not None: ur5robotlfthnd.setJawwidth(jawwidthlft) return ur5mnp
lastxy = minlastxy else: lastxy = [-9999999999999.9, -9999999999999.9] for cobsintersection in cobsintersections: # print cobsintersection if cobsintersection.y > lastxy[1]: lastxy = [cobsintersection.x, cobsintersection.y] stratcurveverts3d.append([lastxy[0], lastxy[1], height]) sppoint13d.append([sppoint1.x, sppoint1.y, height]) sppoint23d.append([sppoint2.x, sppoint2.y, height]) polygonx = rotate(polygon, height/10.0) polygonx = translate(polygonx, Point([lastxy[0], lastxy[1]])) # pg.plotDumbbell(base.render, spos = [lastxy[0], lastxy[1], height], epos = [lastxy[0], lastxy[1], height], length = 0, thickness=30 ) polygonsnp = genPolygonsnp(polygonx, height, color=objcolorarray[int(height/float(steplength))], thickness=20) for polygonnp in polygonsnp: polygonnp.reparentTo(base.render) if height > heightrange-steplength: pg.plotLinesegs(base.render, stratcurveverts3d, rgba = [1.0,1.0,0.0,1], thickness=30) stratcurveverts3d = [] lastxy = [] else: pg.plotLinesegs(base.render, stratcurveverts3d, rgba = [1.0,1.0,0.0,1], thickness=30) stratcurveverts3d = [] lastxy = [] pg.plotLinesegs(base.render, sppoint13d, rgba=[.5,.2,.5,.3], thickness=30, headscale = 3) pg.plotLinesegs(base.render, sppoint23d, rgba=[.2,.5,.5,.3], thickness=30, headscale = 3) pg.plotAxisSelf(base.render, length=1500, thickness = 70) base.run()
ur5mnp = ur5dualplot.genmnp(ur5dualrobot, handpkg) ur5mnp.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) # # # nxtplot.plotstick(base.render, nxtrobot) # pandamat4=Mat4() # pandamat4.setRow(3,Vec3(0,0,250)) # # pg.plotAxis(base.render, pandamat4) # # nxtplot.plotmesh(base, nxtrobot) # # pandageom.plotAxis(base.render, pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos'])) # pg.plotDumbbell(base.render, objpos, objpos, rgba = [1,0,0,1]) pg.plotAxisSelf(base.render, objpos, pg.npToMat4(objrot)) # pg.plotArrow(base.render, hrp5robot.rgtarm[8]['linkpos'], hrp5robot.rgtarm[8]['linkpos']+hrp5robot.rgtarm[8]['rotax']*1000) # # # nxtrobot.movearmfk6(armjntsgoal) # # nxtmnp = nxtplot.genNxtmnp_nm(nxtrobot, plotcolor=[1,0,0,1]) # # nxtmnp.reparentTo(base.render) base.run()
def genmnp(self, nxtrobot, jawwidthrgt=None, jawwidthlft=None): """ generate a panda3d nodepath for the nxtrobot mnp indicates this function generates a mesh nodepath :param nxtrobot: the nxtrobot object, see nxtrobot.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20180109 """ identmat4 = Mat4.identMat() # body nxtwaist_rotmat = pg.cvtMat4(nxtrobot.base['rotmat']) self.__nxtwaist_nodepath.setMat(nxtwaist_rotmat) # rgtarm nxtrgtarmlj0_rotmat = pg.cvtMat4(nxtrobot.rgtarm[1]['rotmat'], nxtrobot.rgtarm[1]['linkpos']) self.__nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat) nxtrgtarmlj1_rotmat = pg.cvtMat4(nxtrobot.rgtarm[2]['rotmat'], nxtrobot.rgtarm[2]['linkpos']) self.__nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat) nxtrgtarmlj2_rotmat = pg.cvtMat4(nxtrobot.rgtarm[3]['rotmat'], nxtrobot.rgtarm[3]['linkpos']) self.__nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat) nxtrgtarmlj3_rotmat = pg.cvtMat4(nxtrobot.rgtarm[4]['rotmat'], nxtrobot.rgtarm[4]['linkpos']) self.__nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat) nxtrgtarmlj4_rotmat = pg.cvtMat4(nxtrobot.rgtarm[5]['rotmat'], nxtrobot.rgtarm[5]['linkpos']) self.__nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat) # lftarm nxtlftarmlj0_rotmat = pg.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos']) self.__nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat) nxtlftarmlj1_rotmat = pg.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos']) self.__nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat) nxtlftarmlj2_rotmat = pg.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos']) self.__nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat) nxtlftarmlj3_rotmat = pg.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos']) self.__nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat) nxtlftarmlj4_rotmat = pg.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos']) self.__nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat) # rgthand nxtlftarmlj5_rotmat = pg.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos']) self.nxtrgthnd.setMat(nxtlftarmlj5_rotmat) pg.plotAxisSelf(self.__nxtmnp, nxtrobot.rgtarm[6]['linkend'], nxtlftarmlj5_rotmat) if jawwidthrgt is not None: self.nxtrgthnd.setJawwidth(jawwidthrgt) # lfthand nxtlftarmlj5_rotmat = pg.cvtMat4(nxtrobot.lftarm[6]['rotmat'], nxtrobot.lftarm[6]['linkpos']) self.nxtlfthnd.setMat(nxtlftarmlj5_rotmat) pg.plotAxisSelf(self.__nxtmnp, nxtrobot.lftarm[6]['linkend'], nxtlftarmlj5_rotmat) if jawwidthlft is not None: self.nxtlfthnd.setJawwidth(jawwidthlft) return copy.deepcopy(self.__nxtmnp)
def genmnp(ur5dual, handpkg, jawwidthrgt = None, jawwidthlft = None): """ generate a panda3d nodepath for the ur5dual mnp indicates this function generates a mesh nodepath :param ur5dual: :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20161202 """ ur5mnp = NodePath("ur5dualmnp") this_dir, this_filename = os.path.split(__file__) # chest0-2, head1 (neck is not plotted) ur5base_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "base.egg")) ur5upperarm_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "upperarm.egg")) ur5shoulder_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "shoulder.egg")) ur5forearm_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "forearm.egg")) ur5wrist1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "wrist1.egg")) ur5wrist2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "wrist2.egg")) ur5wrist3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur5egg", "wrist3.egg")) ur5base_model = loader.loadModel(ur5base_filepath) ur5upperarm_model = loader.loadModel(ur5upperarm_filepath) ur5shoulder_model = loader.loadModel(ur5shoulder_filepath) ur5forearm_model = loader.loadModel(ur5forearm_filepath) ur5wrist1_model = loader.loadModel(ur5wrist1_filepath) ur5wrist2_model = loader.loadModel(ur5wrist2_filepath) ur5wrist3_model = loader.loadModel(ur5wrist3_filepath) # rgt ur5rgtbase_nodepath = NodePath("ur5rgtbase") ur5rgtshoulder_nodepath = NodePath("ur5rgtshoulder") ur5rgtupperarm_nodepath = NodePath("ur5rgtupperarm") ur5rgtforearm_nodepath = NodePath("ur5rgtforearm") ur5rgtwrist1_nodepath = NodePath("ur5rgtwrist1") ur5rgtwrist2_nodepath = NodePath("ur5rgtwrist2") ur5rgtwrist3_nodepath = NodePath("ur5rgtwrist3") # # base egg coordinates is retracted 89.2 along z to fit parameters ur5base_model.instanceTo(ur5rgtbase_nodepath) ur5base_rotmat = pg.cvtMat4(ur5dual.rgtarm[1]['inherentR'], ur5dual.rgtarm[1]['linkpos']) ur5rgtbase_nodepath.setMat(ur5base_rotmat) ur5rgtbase_nodepath.setColor(.5,.5,.5,1) ur5rgtbase_nodepath.reparentTo(ur5mnp) # ur5shoulder_model.instanceTo(ur5rgtshoulder_nodepath) ur5shoulder_rotmat = pg.cvtMat4(ur5dual.rgtarm[1]['rotmat'], ur5dual.rgtarm[1]['linkpos']) ur5rgtshoulder_nodepath.setMat(ur5shoulder_rotmat) ur5rgtshoulder_nodepath.setColor(.5,.7,.3,1) ur5rgtshoulder_nodepath.reparentTo(ur5mnp) # ur5upperarm_model.instanceTo(ur5rgtupperarm_nodepath) ur5upperarm_rotmat = pg.cvtMat4(ur5dual.rgtarm[2]['rotmat'], ur5dual.rgtarm[2]['linkpos']) ur5rgtupperarm_nodepath.setMat(ur5upperarm_rotmat) ur5rgtupperarm_nodepath.setColor(.5,.5,.5,1) ur5rgtupperarm_nodepath.reparentTo(ur5mnp) # ur5forearm_model.instanceTo(ur5rgtforearm_nodepath) ur5forearm_rotmat = pg.cvtMat4(ur5dual.rgtarm[3]['rotmat'], ur5dual.rgtarm[3]['linkpos']) ur5rgtforearm_nodepath.setMat(ur5forearm_rotmat) ur5rgtforearm_nodepath.setColor(.5,.5,.5,1) ur5rgtforearm_nodepath.reparentTo(ur5mnp) # ur5wrist1_model.instanceTo(ur5rgtwrist1_nodepath) ur5wrist1_rotmat = pg.cvtMat4(ur5dual.rgtarm[4]['rotmat'], ur5dual.rgtarm[4]['linkpos']) ur5rgtwrist1_nodepath.setMat(ur5wrist1_rotmat) ur5rgtwrist1_nodepath.setColor(.5,.7,.3,1) ur5rgtwrist1_nodepath.reparentTo(ur5mnp) # ur5wrist2_model.instanceTo(ur5rgtwrist2_nodepath) ur5wrist2_rotmat = pg.cvtMat4(ur5dual.rgtarm[5]['rotmat'], ur5dual.rgtarm[5]['linkpos']) ur5rgtwrist2_nodepath.setMat(ur5wrist2_rotmat) ur5rgtwrist2_nodepath.setColor(.5,.5,.5,1) ur5rgtwrist2_nodepath.reparentTo(ur5mnp) # # wrist3 egg coordinates is rotated 90 around z retracted 72.33752-81.82489 to fit parameters ur5wrist3_model.instanceTo(ur5rgtwrist3_nodepath) ur5wrist3_rotmat = pg.cvtMat4(ur5dual.rgtarm[6]['rotmat'], ur5dual.rgtarm[6]['linkpos']) ur5rgtwrist3_nodepath.setMat(ur5wrist3_rotmat) ur5rgtwrist3_nodepath.setColor(.5,.5,.5,1) ur5rgtwrist3_nodepath.reparentTo(ur5mnp) # lft ur5lftbase_nodepath = NodePath("ur5lftbase") ur5lftupperarm_nodepath = NodePath("ur5lftupperarm") ur5lftshoulder_nodepath = NodePath("ur5lftshoulder") ur5lftforearm_nodepath = NodePath("ur5lftforearm") ur5lftwrist1_nodepath = NodePath("ur5lftwrist1") ur5lftwrist2_nodepath = NodePath("ur5lftwrist2") ur5lftwrist3_nodepath = NodePath("ur5lftwrist3") # # base egg coordinates is retracted 89.2 along z to fit parameters ur5base_model.instanceTo(ur5lftbase_nodepath) ur5base_rotmat = pg.cvtMat4(ur5dual.lftarm[1]['inherentR'], ur5dual.lftarm[1]['linkpos']) ur5lftbase_nodepath.setMat(ur5base_rotmat) ur5lftbase_nodepath.setColor(.5,.5,.5,1) ur5lftbase_nodepath.reparentTo(ur5mnp) # ur5shoulder_model.instanceTo(ur5lftshoulder_nodepath) ur5shoulder_rotmat = pg.cvtMat4(ur5dual.lftarm[1]['rotmat'], ur5dual.lftarm[1]['linkpos']) ur5lftshoulder_nodepath.setMat(ur5shoulder_rotmat) ur5lftshoulder_nodepath.setColor(.5,.7,.3,1) ur5lftshoulder_nodepath.reparentTo(ur5mnp) # ur5upperarm_model.instanceTo(ur5lftupperarm_nodepath) ur5upperarm_rotmat = pg.cvtMat4(ur5dual.lftarm[2]['rotmat'], ur5dual.lftarm[2]['linkpos']) ur5lftupperarm_nodepath.setMat(ur5upperarm_rotmat) ur5lftupperarm_nodepath.setColor(.5,.5,.5,1) ur5lftupperarm_nodepath.reparentTo(ur5mnp) # ur5forearm_model.instanceTo(ur5lftforearm_nodepath) ur5forearm_rotmat = pg.cvtMat4(ur5dual.lftarm[3]['rotmat'], ur5dual.lftarm[3]['linkpos']) ur5lftforearm_nodepath.setMat(ur5forearm_rotmat) ur5lftforearm_nodepath.setColor(.5,.5,.5,1) ur5lftforearm_nodepath.reparentTo(ur5mnp) # ur5wrist1_model.instanceTo(ur5lftwrist1_nodepath) ur5wrist1_rotmat = pg.cvtMat4(ur5dual.lftarm[4]['rotmat'], ur5dual.lftarm[4]['linkpos']) ur5lftwrist1_nodepath.setMat(ur5wrist1_rotmat) ur5lftwrist1_nodepath.setColor(.5,.7,.3,1) ur5lftwrist1_nodepath.reparentTo(ur5mnp) # ur5wrist2_model.instanceTo(ur5lftwrist2_nodepath) ur5wrist2_rotmat = pg.cvtMat4(ur5dual.lftarm[5]['rotmat'], ur5dual.lftarm[5]['linkpos']) ur5lftwrist2_nodepath.setMat(ur5wrist2_rotmat) ur5lftwrist2_nodepath.setColor(.5,.5,.5,1) ur5lftwrist2_nodepath.reparentTo(ur5mnp) # ur5wrist3_model.instanceTo(ur5lftwrist3_nodepath) ur5wrist3_rotmat = pg.cvtMat4(ur5dual.lftarm[6]['rotmat'], ur5dual.lftarm[6]['linkpos']) ur5lftwrist3_nodepath.setMat(ur5wrist3_rotmat) ur5lftwrist3_nodepath.setColor(.5,.5,.5,1) ur5lftwrist3_nodepath.reparentTo(ur5mnp) # rgthnd ur5robotrgthnd = handpkg.newHand('rgt') ur5robotrgtarmljend_rotmat = pg.cvtMat4(ur5dual.rgtarm[6]['rotmat'], ur5dual.rgtarm[6]['linkpos']) pg.plotAxisSelf(ur5mnp, ur5dual.rgtarm[6]['linkend'], ur5robotrgtarmljend_rotmat) ur5robotrgthnd.setMat(ur5robotrgtarmljend_rotmat) ur5robotrgthnd.reparentTo(ur5mnp) if jawwidthrgt is not None: ur5robotrgthnd.setJawwidth(jawwidthrgt) # lfthnd ur5robotlfthnd = handpkg.newHand('lft') ur5robotlftarmljend_rotmat = pg.cvtMat4(ur5dual.lftarm[6]['rotmat'], ur5dual.lftarm[6]['linkpos']) pg.plotAxisSelf(ur5mnp, ur5dual.lftarm[6]['linkend'], ur5robotlftarmljend_rotmat) ur5robotlfthnd.setMat(ur5robotlftarmljend_rotmat) ur5robotlfthnd.reparentTo(ur5mnp) if jawwidthlft is not None: ur5robotlfthnd.setJawwidth(jawwidthlft) return ur5mnp
def genmnplist(ur3dual, handpkg, jawwidthrgt = None, jawwidthlft = None): """ generate a list of panda3d nodepath for the ur3dual mnp indicates this function generates a mesh nodepath the return value is in the following format: [[rightarm mnp list], [leftarm mnp list], [body]] # Right goes first! # The order of an arm mnp list is from base to end-effector :param ur3dual: :return: a list of mesh nodepaths author: weiwei date: 20170608 """ ur3mnp = NodePath("ur3dualmnp") this_dir, this_filename = os.path.split(__file__) # chest0-2, head1 (neck is not plotted) ur3base_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "base.egg")) ur3upperarm_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "upperarm.egg")) ur3shoulder_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "shoulder.egg")) ur3forearm_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "forearm.egg")) ur3wrist1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "wrist1.egg")) ur3wrist2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "wrist2.egg")) ur3wrist3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "ur3egg", "wrist3.egg")) ur3base_model = loader.loadModel(ur3base_filepath) ur3upperarm_model = loader.loadModel(ur3upperarm_filepath) ur3shoulder_model = loader.loadModel(ur3shoulder_filepath) ur3forearm_model = loader.loadModel(ur3forearm_filepath) ur3wrist1_model = loader.loadModel(ur3wrist1_filepath) ur3wrist2_model = loader.loadModel(ur3wrist2_filepath) ur3wrist3_model = loader.loadModel(ur3wrist3_filepath) # rgt ur3rgtbase_nodepath = NodePath("ur3rgtbase") ur3rgtshoulder_nodepath = NodePath("ur3rgtshoulder") ur3rgtupperarm_nodepath = NodePath("ur3rgtupperarm") ur3rgtforearm_nodepath = NodePath("ur3rgtforearm") ur3rgtwrist1_nodepath = NodePath("ur3rgtwrist1") ur3rgtwrist2_nodepath = NodePath("ur3rgtwrist2") ur3rgtwrist3_nodepath = NodePath("ur3rgtwrist3") # # base egg coordinates is retracted 89.2 along z to fit parameters ur3base_model.instanceTo(ur3rgtbase_nodepath) ur3base_rotmat = pg.cvtMat4(ur3dual.rgtarm[1]['inherentR'], ur3dual.rgtarm[1]['linkpos']) ur3rgtbase_nodepath.setMat(ur3base_rotmat) ur3rgtbase_nodepath.setColor(.5,.5,.5,1) ur3rgtbase_nodepath.reparentTo(ur3mnp) # ur3shoulder_model.instanceTo(ur3rgtshoulder_nodepath) ur3shoulder_rotmat = pg.cvtMat4(ur3dual.rgtarm[1]['rotmat'], ur3dual.rgtarm[1]['linkpos']) ur3rgtshoulder_nodepath.setMat(ur3shoulder_rotmat) ur3rgtshoulder_nodepath.setColor(.5,.7,.3,1) ur3rgtshoulder_nodepath.reparentTo(ur3mnp) # ur3upperarm_model.instanceTo(ur3rgtupperarm_nodepath) ur3upperarm_rotmat = pg.cvtMat4(ur3dual.rgtarm[2]['rotmat'], ur3dual.rgtarm[2]['linkpos']) ur3rgtupperarm_nodepath.setMat(ur3upperarm_rotmat) ur3rgtupperarm_nodepath.setColor(.5,.5,.5,1) ur3rgtupperarm_nodepath.reparentTo(ur3mnp) # ur3forearm_model.instanceTo(ur3rgtforearm_nodepath) ur3forearm_rotmat = pg.cvtMat4(ur3dual.rgtarm[3]['rotmat'], ur3dual.rgtarm[3]['linkpos']) ur3rgtforearm_nodepath.setMat(ur3forearm_rotmat) ur3rgtforearm_nodepath.setColor(.5,.5,.5,1) ur3rgtforearm_nodepath.reparentTo(ur3mnp) # ur3wrist1_model.instanceTo(ur3rgtwrist1_nodepath) ur3wrist1_rotmat = pg.cvtMat4(ur3dual.rgtarm[4]['rotmat'], ur3dual.rgtarm[4]['linkpos']) ur3rgtwrist1_nodepath.setMat(ur3wrist1_rotmat) ur3rgtwrist1_nodepath.setColor(.5,.7,.3,1) ur3rgtwrist1_nodepath.reparentTo(ur3mnp) # ur3wrist2_model.instanceTo(ur3rgtwrist2_nodepath) ur3wrist2_rotmat = pg.cvtMat4(ur3dual.rgtarm[5]['rotmat'], ur3dual.rgtarm[5]['linkpos']) ur3rgtwrist2_nodepath.setMat(ur3wrist2_rotmat) ur3rgtwrist2_nodepath.setColor(.5,.5,.5,1) ur3rgtwrist2_nodepath.reparentTo(ur3mnp) # # wrist3 egg coordinates is rotated 90 around z retracted 72.33752-81.82489 to fit parameters ur3wrist3_model.instanceTo(ur3rgtwrist3_nodepath) ur3wrist3_rotmat = pg.cvtMat4(ur3dual.rgtarm[6]['rotmat'], ur3dual.rgtarm[6]['linkpos']) ur3rgtwrist3_nodepath.setMat(ur3wrist3_rotmat) ur3rgtwrist3_nodepath.setColor(.5,.5,.5,1) ur3rgtwrist3_nodepath.reparentTo(ur3mnp) # lft ur3lftbase_nodepath = NodePath("ur3lftbase") ur3lftupperarm_nodepath = NodePath("ur3lftupperarm") ur3lftshoulder_nodepath = NodePath("ur3lftshoulder") ur3lftforearm_nodepath = NodePath("ur3lftforearm") ur3lftwrist1_nodepath = NodePath("ur3lftwrist1") ur3lftwrist2_nodepath = NodePath("ur3lftwrist2") ur3lftwrist3_nodepath = NodePath("ur3lftwrist3") # # base egg coordinates is retracted 89.2 along z to fit parameters ur3base_model.instanceTo(ur3lftbase_nodepath) ur3base_rotmat = pg.cvtMat4(ur3dual.lftarm[1]['inherentR'], ur3dual.lftarm[1]['linkpos']) ur3lftbase_nodepath.setMat(ur3base_rotmat) ur3lftbase_nodepath.setColor(.5,.5,.5,1) ur3lftbase_nodepath.reparentTo(ur3mnp) # ur3shoulder_model.instanceTo(ur3lftshoulder_nodepath) ur3shoulder_rotmat = pg.cvtMat4(ur3dual.lftarm[1]['rotmat'], ur3dual.lftarm[1]['linkpos']) ur3lftshoulder_nodepath.setMat(ur3shoulder_rotmat) ur3lftshoulder_nodepath.setColor(.5,.7,.3,1) ur3lftshoulder_nodepath.reparentTo(ur3mnp) # ur3upperarm_model.instanceTo(ur3lftupperarm_nodepath) ur3upperarm_rotmat = pg.cvtMat4(ur3dual.lftarm[2]['rotmat'], ur3dual.lftarm[2]['linkpos']) ur3lftupperarm_nodepath.setMat(ur3upperarm_rotmat) ur3lftupperarm_nodepath.setColor(.5,.5,.5,1) ur3lftupperarm_nodepath.reparentTo(ur3mnp) # ur3forearm_model.instanceTo(ur3lftforearm_nodepath) ur3forearm_rotmat = pg.cvtMat4(ur3dual.lftarm[3]['rotmat'], ur3dual.lftarm[3]['linkpos']) ur3lftforearm_nodepath.setMat(ur3forearm_rotmat) ur3lftforearm_nodepath.setColor(.5,.5,.5,1) ur3lftforearm_nodepath.reparentTo(ur3mnp) # ur3wrist1_model.instanceTo(ur3lftwrist1_nodepath) ur3wrist1_rotmat = pg.cvtMat4(ur3dual.lftarm[4]['rotmat'], ur3dual.lftarm[4]['linkpos']) ur3lftwrist1_nodepath.setMat(ur3wrist1_rotmat) ur3lftwrist1_nodepath.setColor(.5,.7,.3,1) ur3lftwrist1_nodepath.reparentTo(ur3mnp) # ur3wrist2_model.instanceTo(ur3lftwrist2_nodepath) ur3wrist2_rotmat = pg.cvtMat4(ur3dual.lftarm[5]['rotmat'], ur3dual.lftarm[5]['linkpos']) ur3lftwrist2_nodepath.setMat(ur3wrist2_rotmat) ur3lftwrist2_nodepath.setColor(.5,.5,.5,1) ur3lftwrist2_nodepath.reparentTo(ur3mnp) # ur3wrist3_model.instanceTo(ur3lftwrist3_nodepath) ur3wrist3_rotmat = pg.cvtMat4(ur3dual.lftarm[6]['rotmat'], ur3dual.lftarm[6]['linkpos']) ur3lftwrist3_nodepath.setMat(ur3wrist3_rotmat) ur3lftwrist3_nodepath.setColor(.5,.5,.5,1) ur3lftwrist3_nodepath.reparentTo(ur3mnp) # rgthnd ur3robotrgthnd = handpkg.newHand('rgt', ftsensoroffset = -50) ur3robotrgtarmljend_rotmat = pg.cvtMat4(ur3dual.rgtarm[6]['rotmat'], ur3dual.rgtarm[6]['linkpos']) pg.plotAxisSelf(ur3mnp, ur3dual.rgtarm[6]['linkend'], ur3robotrgtarmljend_rotmat) ur3robotrgthnd.setMat(ur3robotrgtarmljend_rotmat) ur3robotrgthnd.reparentTo(ur3mnp) if jawwidthrgt is not None: ur3robotrgthnd.setJawwidth(jawwidthrgt) # lfthnd ur3robotlfthnd = handpkg.newHand('lft', ftsensoroffset = -50) ur3robotlftarmljend_rotmat = pg.cvtMat4(ur3dual.lftarm[6]['rotmat'], ur3dual.lftarm[6]['linkpos']) pg.plotAxisSelf(ur3mnp, ur3dual.lftarm[6]['linkend'], ur3robotlftarmljend_rotmat) ur3robotlfthnd.setMat(ur3robotlftarmljend_rotmat) ur3robotlfthnd.reparentTo(ur3mnp) if jawwidthlft is not None: ur3robotlfthnd.setJawwidth(jawwidthlft) return [[ur3rgtbase_nodepath, ur3rgtshoulder_nodepath, ur3rgtupperarm_nodepath, ur3rgtforearm_nodepath, ur3rgtwrist1_nodepath, ur3rgtwrist2_nodepath, ur3rgtwrist3_nodepath, ur3robotrgthnd.handnp], [ur3lftbase_nodepath, ur3lftshoulder_nodepath, ur3lftupperarm_nodepath, ur3lftforearm_nodepath, ur3lftwrist1_nodepath, ur3lftwrist2_nodepath, ur3lftwrist3_nodepath, ur3robotlfthnd.handnp], []]
posfgr0 = self.fgr0body.getPosition() rotfgr0= self.fgr0body.getRotation() matfgr0 = Mat4(rotfgr0) matfgr0.setRow(3, posfgr0) self.fgr0geom.setMat(matfgr0) # fgr1 posfgr1 = self.fgr1body.getPosition() rotfgr1= self.fgr1body.getRotation() matfgr1 = Mat4(rotfgr1) matfgr1.setRow(3, posfgr1) self.fgr1geom.setMat(matfgr1) if __name__=='__main__': base = pandactrl.World(camp=[0,0,550], lookatp=[0,0,0]) pg.plotAxisSelf(base.render, length = 10, thickness = 1) odeWorld = OdeWorld() odeWorld.setGravity(0, 0, -9.81) odeSpace = OdeSimpleSpace() gper = Gripper(base, odeSpace, odeWorld) gper.resethand() def updateshow(task): odeWorld.quickStep(globalClock.getDt()) gper.update() gper.gripProc() # if gper.getTipWidth() > 0.0:
robotmnp = robotmesh.genmnp(robot) robotmnp.reparentTo(base.render) # robot = nxt.NxtRobot() # handpkg = rtq85nm # robotplot = nxtplot # robot.goinitpose() # robot.movearmfk([0.0,-20.0,-60.0,-70.0,-100.0,-30.0,0.0,0.0,0.0]) # robot.movearmfk([0.0,-20.0,60.0,70.0,-100.0,30.0,0.0,0.0,0.0], armid = 'lft') # robotmnp = robotmesh.genmnp(robot) # robotmnp.reparentTo(base.render) # ur5dualmnp = ur5dualplot.genUr5dualmnp(robot, handpkg) # ur5dualmnp.reparentTo(base.render) pg.plotAxisSelf(base.render, Vec3(0,0,0)) cdchecker = CollisionChecker(robotmesh) print cdchecker.isSelfCollided(robot) bullcldrnp = base.render.attachNewNode("bulletcollider") debugNode = BulletDebugNode('Debug') debugNode.showWireframe(True) debugNode.showConstraints(True) debugNode.showBoundingBoxes(False) debugNode.showNormals(False) debugNP = bullcldrnp.attachNewNode(debugNode) debugNP.show() cdchecker.bulletworld.setDebugNode(debugNP.node())
while True: if kinect.has_new_depth_frame(): cframe = kinect.get_last_color_frame() cwidth = kinect.color_frame_desc.Width cheight = kinect.color_frame_desc.Height dframe = kinect.get_last_depth_frame() dwidth = kinect.depth_frame_desc.Width dheight = kinect.depth_frame_desc.Height tmpvertslist = getRotMat(depthToXYZ(dframe, dwidth, dheight)) for tmpverts in tmpvertslist: dverts= kinxyzToRobXYZ(tmpverts) color = [np.random.rand(), np.random.rand(), np.random.rand(), np.random.rand()] colors = [] for dvert in dverts: colors.append(color) pntsnp = pg.genPntsnp(dverts, colors = colors) pntsnp.reparentTo(base.render) break import robotsim.nextage.nxt as nxt import robotsim.nextage.nxtplot as nxtplot import pandaplotutils.pandageom as pg from manipulation.grip.robotiq85 import rtq85nm handpkg = rtq85nm nxtrobot = nxt.NxtRobot() nxtmnp = nxtplot.genmnp(nxtrobot, handpkg) nxtmnp.reparentTo(base.render) pg.plotAxisSelf(base.render) base.run()
lfgrbullnode = BulletRigidBodyNode('glfgr') lfgrbullnode.addShape(BulletTriangleMeshShape(glfgrmesh, dynamic=True), glfgrts) lfgrcollidernp=bullcldrnp.attachNewNode(lfgrbullnode) base.world.attachRigidBody(lfgrbullnode) lfgrcollidernp.setCollideMask(BitMask32.allOn()) # rtq85hnd.rtq85np.find("**/rtq85lfgr").showTightBounds() # rtq85hnd.rtq85np.showTightBounds() base.taskMgr.add(updateworld, "updateworld", extraArgs=[base.world], appendTask=True) result = base.world.contactTestPair(ilkbullnode, lftbullnode) print result print result.getContacts() import pandaplotutils.pandageom as pandageom for contact in result.getContacts(): cp = contact.getManifoldPoint() print cp.getLocalPointA() pandageom.plotSphere(base, pos=cp.getLocalPointA(), radius=10, rgba=Vec4(1,0,0,1)) pandageom.plotAxisSelf(base.render, spos = Vec3(0,0,0)) debugNode = BulletDebugNode('Debug') debugNode.showWireframe(True) debugNode.showConstraints(True) debugNode.showBoundingBoxes(False) debugNode.showNormals(False) debugNP = bullcldrnp.attachNewNode(debugNode) # debugNP.show() base.world.setDebugNode(debugNP.node()) base.run()