def updateshow(rbd, rbdnp, framenp, task): for frame in framenp: frame.detachNode() dtime = 0.001 skipframe = 15 for i in range(skipframe): force, torque = genForce(rbd, dtime) [P, L] = doPhysics(rbd, force, torque, dtime) writer.writerow([str(L[0]), str(L[1]), str(L[2])]) updateRbdPR(rbd, dtime) if task.time > 2.0: writer.close() # model = loader.loadModel(model_filepath) # model.reparentTo(base.render) model.setMat(pg.cvtMat4(rbd.rotmat)) model.setPos(pg.npToV3(rbd.pos)) arrownp = pg.plotArrow(base.render, epos=rbd.angularw * 500.0, thickness=15, rgba=[1, 0.5, 0.5, 1]) framenp.append(arrownp) return task.again
def updateshow(rbd, rbdnp, framenp, task): for frame in framenp: frame.detachNode() dtime = 0.02 updateRbdVW(rbd, dtime) model = loader.loadModel(model_filepath) model.reparentTo(base.render) model.setMat(pg.cvtMat4(rbd.rotmat)) model.setPos(pg.npToV3(rbd.pos)) return task.again
def gettcpframe(self, armid = 'rgt'): """ get the local frame of tcp in Mat4 format :param armid: author: weiwei date: 20170412 """ armlj = self.rgtarm if armid == "lft": armlj = self.lftarm return pg.cvtMat4(armlj[-1]['rotmat'], armlj[-1]['linkpos'])
def gettcpframe(self, armname='rgt'): """ get the local frame of tcp in Mat4 format :param armname: author: weiwei date: 20170412 """ armlj = self.rgtarm if armname == "lft": armlj = self.lftarm return pg.cvtMat4(armlj[-1]['rotmat'], armlj[-1]['linkpos'])
def updateshow(rbd, rbdnp, framenp, task): for frame in framenp: frame.detachNode() dtime = 0.001 skipframe = 15 for i in range(skipframe): force, torque = genForce(rbd, dtime) [P, L] = doPhysics(rbd, force, torque, dtime) writer.writerow([str(L[0]), str(L[1]), str(L[2])]) updateRbdPR(rbd, dtime) if task.time > 2.0: writer.close() # model = loader.loadModel(model_filepath) # model.reparentTo(base.render) model.setMat(pg.cvtMat4(rbd.rotmat)) model.setPos(pg.npToV3(rbd.pos)) arrownp = pg.plotArrow(base.render, epos = rbd.angularw*500.0, thickness = 15, rgba=[1,0.5,0.5,1]) framenp.append(arrownp) return task.again
def updateshow(rbd, rbdnp, framenp, task): for frame in framenp: frame.detachNode() dtime = 0.002 angularmomentum = doEulerPhysics(rbd, dtime) model.setMat(pg.cvtMat4(rbd.rotmat)) # writer.writerow([str(rbd.anglew[0]), str(rbd.anglew[1]), str(rbd.anglew[2])]) writer.writerow([ str(angularmomentum[0]), str(angularmomentum[1]), str(angularmomentum[2]) ]) arrownp = pg.plotArrow(base.render, epos=rbd.anglew * 500, thickness=15) framenp.append(arrownp) if task.time > 10.0: writer.close() return task.again
def genmnp(nxtrobot, handpkg, 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 nextage.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20161109 """ nxtmnp = NodePath("nxtmnp") this_dir, this_filename = os.path.split(__file__) # mainbody, waist, body, head (neck is not plotted) nxtwaist_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_waist.egg")) nxtbody_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_body.egg")) nxthead_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_head.egg")) nxtwaist_model = loader.loadModel(nxtwaist_filepath) nxtbody_model = loader.loadModel(nxtbody_filepath) nxthead_model = loader.loadModel(nxthead_filepath) nxtwaist_nodepath = NodePath("nxtwaist") nxtbody_nodepath = NodePath("nxtbody") nxthead_nodepath = NodePath("nxthead") nxtwaist_model.instanceTo(nxtwaist_nodepath) nxtwaist_rotmat = pandageom.cvtMat4(nxtrobot.base['rotmat']) nxtwaist_nodepath.setMat(nxtwaist_rotmat) nxtbody_model.instanceTo(nxtbody_nodepath) nxthead_model.instanceTo(nxthead_nodepath) nxthead_nodepath.setH(nxtrobot.initjnts[1]) nxthead_nodepath.setP(nxtrobot.initjnts[2]) nxthead_nodepath.setZ(569.5) nxtwaist_nodepath.reparentTo(nxtmnp) nxtbody_nodepath.reparentTo(nxtwaist_nodepath) nxthead_nodepath.reparentTo(nxtwaist_nodepath) # rgtarm nxtrgtarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj0.egg")) nxtrgtarmlj0_model = loader.loadModel(nxtrgtarmlj0_filepath) nxtrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath") nxtrgtarmlj0_model.instanceTo(nxtrgtarmlj0_nodepath) nxtrgtarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[1]['rotmat'], nxtrobot.rgtarm[1]['linkpos']) nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat) nxtrgtarmlj0_nodepath.reparentTo(nxtmnp) nxtrgtarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj1.egg")) nxtrgtarmlj1_model = loader.loadModel(nxtrgtarmlj1_filepath) nxtrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath") nxtrgtarmlj1_model.instanceTo(nxtrgtarmlj1_nodepath) nxtrgtarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[2]['rotmat'], nxtrobot.rgtarm[2]['linkpos']) nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat) nxtrgtarmlj1_nodepath.reparentTo(nxtmnp) nxtrgtarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj2.egg")) nxtrgtarmlj2_model = loader.loadModel(nxtrgtarmlj2_filepath) nxtrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath") nxtrgtarmlj2_model.instanceTo(nxtrgtarmlj2_nodepath) nxtrgtarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[3]['rotmat'], nxtrobot.rgtarm[3]['linkpos']) nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat) nxtrgtarmlj2_nodepath.reparentTo(nxtmnp) nxtrgtarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj3.egg")) nxtrgtarmlj3_model = loader.loadModel(nxtrgtarmlj3_filepath) nxtrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath") nxtrgtarmlj3_model.instanceTo(nxtrgtarmlj3_nodepath) nxtrgtarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[4]['rotmat'], nxtrobot.rgtarm[4]['linkpos']) nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat) nxtrgtarmlj3_nodepath.reparentTo(nxtmnp) nxtrgtarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj4.egg")) nxtrgtarmlj4_model = loader.loadModel(nxtrgtarmlj4_filepath) nxtrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath") nxtrgtarmlj4_model.instanceTo(nxtrgtarmlj4_nodepath) nxtrgtarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[5]['rotmat'], nxtrobot.rgtarm[5]['linkpos']) nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat) nxtrgtarmlj4_nodepath.reparentTo(nxtmnp) # lftarm nxtlftarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj0.egg")) nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath) nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath") nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath) nxtlftarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos']) nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat) nxtlftarmlj0_nodepath.reparentTo(nxtmnp) nxtlftarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj1.egg")) nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath) nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath") nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath) nxtlftarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos']) nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat) nxtlftarmlj1_nodepath.reparentTo(nxtmnp) nxtlftarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj2.egg")) nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath) nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath") nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath) nxtlftarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos']) nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat) nxtlftarmlj2_nodepath.reparentTo(nxtmnp) nxtlftarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj3.egg")) nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath) nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath") nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath) nxtlftarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos']) nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat) nxtlftarmlj3_nodepath.reparentTo(nxtmnp) nxtlftarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj4.egg")) nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath) nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath") nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath) nxtlftarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos']) nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat) nxtlftarmlj4_nodepath.reparentTo(nxtmnp) # rgthnd nxtrgthnd = handpkg.newHand('rgt') nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos']) nxtrgthnd.setMat(nxtlftarmlj5_rotmat) nxtrgthnd.reparentTo(nxtmnp) if jawwidthrgt is not None: nxtrgthnd.setJawwidth(jawwidthrgt) # lfthnd nxtlfthnd = handpkg.newHand('lft') nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[6]['rotmat'], nxtrobot.lftarm[6]['linkpos']) nxtlfthnd.setMat(nxtlftarmlj5_rotmat) nxtlfthnd.reparentTo(nxtmnp) if jawwidthlft is not None: nxtlfthnd.setJawwidth(jawwidthlft) return nxtmnp
from panda3d.core import * import pandaplotutils.pandageom as pg import pandaplotutils.pandactrl as pc base = pc.World(camp = [1000,0,0], lookatp = [0,0,0]) rbd = Rigidbody(mass = 10.0, pos = np.array([0,0,0.0]), com = [0.0,0.0,0.0], rotmat = rm.rodrigues(np.array([1,0,0]), 1.8), inertiatensor = np.array([[1732.0,0.0,0.0],[0.0,1732.0,0.0],[0.0,0.0,3393.0]])) rbd.linearv = np.array([0,0,0]) rbd.angularw = np.array([0,0,50]) this_dir, this_filename = os.path.split(__file__) model_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "models", "top.egg")) model = loader.loadModel(model_filepath) model.reparentTo(base.render) model.setMat(pg.cvtMat4(rbd.rotmat)) model.setPos(pg.npToV3(rbd.pos)) # base = pc.World(camp=[3000, 0, 3000], lookatp=[0, 0, 0]) # # rbd = Rigidbody(mass = 1.0, pos=np.array([0, 0, 0.0]), rotmat=np.identity(3), # inertiatensor=np.array([[80833.3, 0.0, 0.0], [0.0, 68333.3, 0.0], [0.0, 0.0, 14166.7]])) # rbd.linearv = np.array([0,0,0]) # rbd.angularw = np.array([1, 1, 1]) # # this_dir, this_filename = os.path.split(__file__) # model_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "models", "box.egg")) # model = loader.loadModel(model_filepath) # model.reparentTo(base.render)
def genHrp5mnp(hrp5robot, jawwidthrgt=None, jawwidthlft=None): """ generate a panda3d nodepath for the hrp5robot mnp indicates this function generates a mesh nodepath :param hrp5robot: the Hrp5Robot object, see hrp5.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20161202 """ hrp5mnp = NodePath("hrp5mnp") this_dir, this_filename = os.path.split(__file__) # chest0-2, head1 (neck is not plotted) hrp5chest0_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_chest0.egg")) hrp5chest1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_chest1.egg")) hrp5chest2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_chest2.egg")) hrp5head1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_head1.egg")) hrp5chest0_model = loader.loadModel(hrp5chest0_filepath) hrp5chest1_model = loader.loadModel(hrp5chest1_filepath) hrp5chest2_model = loader.loadModel(hrp5chest2_filepath) hrp5head1_model = loader.loadModel(hrp5head1_filepath) hrp5chest0_nodepath = NodePath("hrp5chest0") hrp5chest1_nodepath = NodePath("hrp5chest1") hrp5chest2_nodepath = NodePath("hrp5chest2") hrp5head1_nodepath = NodePath("hrp5head1") hrp5chest0_model.instanceTo(hrp5chest0_nodepath) hrp5chest0_rotmat = pg.cvtMat4(hrp5robot.base['rotmat']) hrp5chest0_nodepath.setMat(hrp5chest0_rotmat) hrp5chest0_nodepath.setZ(223) hrp5chest1_model.instanceTo(hrp5chest1_nodepath) hrp5chest1_nodepath.setColor(.7, .7, .2, 1) hrp5chest2_model.instanceTo(hrp5chest2_nodepath) hrp5chest2_nodepath.setX(-68) hrp5chest2_nodepath.setColor(.7, .7, .7, 1) hrp5head1_model.instanceTo(hrp5head1_nodepath) hrp5head1_nodepath.setH(hrp5robot.initjnts[0]) hrp5head1_nodepath.setP(hrp5robot.initjnts[1]) hrp5head1_nodepath.setX(43) hrp5head1_nodepath.setZ(440) hrp5head1_nodepath.setColor(.5, .5, .5, 1) hrp5chest0_nodepath.reparentTo(hrp5mnp) hrp5chest1_nodepath.reparentTo(hrp5chest0_nodepath) hrp5chest2_nodepath.reparentTo(hrp5chest0_nodepath) hrp5head1_nodepath.reparentTo(hrp5chest0_nodepath) # rgtarm hrp5rgtarmlj0_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_rscap.egg")) hrp5rgtarmlj0_model = loader.loadModel(hrp5rgtarmlj0_filepath) hrp5rgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath") hrp5rgtarmlj0_model.instanceTo(hrp5rgtarmlj0_nodepath) hrp5rgtarmlj0_rotmat = pg.cvtMat4(hrp5robot.rgtarm[1]['rotmat'], hrp5robot.rgtarm[1]['linkpos']) hrp5rgtarmlj0_nodepath.setMat(hrp5rgtarmlj0_rotmat) hrp5rgtarmlj0_nodepath.setColor(.5, .5, .5, 1) hrp5rgtarmlj0_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_rlink0.egg")) hrp5rgtarmlj1_model = loader.loadModel(hrp5rgtarmlj1_filepath) hrp5rgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath") hrp5rgtarmlj1_model.instanceTo(hrp5rgtarmlj1_nodepath) hrp5rgtarmlj1_rotmat = pg.cvtMat4(hrp5robot.rgtarm[2]['rotmat'], hrp5robot.rgtarm[2]['linkpos']) hrp5rgtarmlj1_nodepath.setMat(hrp5rgtarmlj1_rotmat) hrp5rgtarmlj1_nodepath.setColor(.5, .5, .5, 1) hrp5rgtarmlj1_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_rlink1.egg")) hrp5rgtarmlj2_model = loader.loadModel(hrp5rgtarmlj2_filepath) hrp5rgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath") hrp5rgtarmlj2_model.instanceTo(hrp5rgtarmlj2_nodepath) hrp5rgtarmlj2_rotmat = pg.cvtMat4(hrp5robot.rgtarm[3]['rotmat'], hrp5robot.rgtarm[3]['linkpos']) hrp5rgtarmlj2_nodepath.setMat(hrp5rgtarmlj2_rotmat) hrp5rgtarmlj2_nodepath.setColor(.5, .5, .5, 1) hrp5rgtarmlj2_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj3_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_rlink2.egg")) hrp5rgtarmlj3_model = loader.loadModel(hrp5rgtarmlj3_filepath) hrp5rgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath") hrp5rgtarmlj3_model.instanceTo(hrp5rgtarmlj3_nodepath) hrp5rgtarmlj3_rotmat = pg.cvtMat4(hrp5robot.rgtarm[4]['rotmat'], hrp5robot.rgtarm[4]['linkpos']) hrp5rgtarmlj3_nodepath.setMat(hrp5rgtarmlj3_rotmat) hrp5rgtarmlj3_nodepath.setColor(.5, .5, .5, 1) hrp5rgtarmlj3_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj4_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_rlink3.egg")) hrp5rgtarmlj4_model = loader.loadModel(hrp5rgtarmlj4_filepath) hrp5rgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath") hrp5rgtarmlj4_model.instanceTo(hrp5rgtarmlj4_nodepath) hrp5rgtarmlj4_rotmat = pg.cvtMat4(hrp5robot.rgtarm[5]['rotmat'], hrp5robot.rgtarm[5]['linkpos']) hrp5rgtarmlj4_nodepath.setMat(hrp5rgtarmlj4_rotmat) hrp5rgtarmlj4_nodepath.setColor(.5, .5, .5, 1) hrp5rgtarmlj4_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj5_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_rlink4.egg")) hrp5rgtarmlj5_model = loader.loadModel(hrp5rgtarmlj5_filepath) hrp5rgtarmlj5_nodepath = NodePath("nxtrgtarmlj5_nodepath") hrp5rgtarmlj5_model.instanceTo(hrp5rgtarmlj5_nodepath) hrp5rgtarmlj5_rotmat = pg.cvtMat4(hrp5robot.rgtarm[6]['rotmat'], hrp5robot.rgtarm[6]['linkpos']) hrp5rgtarmlj5_nodepath.setMat(hrp5rgtarmlj5_rotmat) hrp5rgtarmlj5_nodepath.setColor(.5, .5, .5, 1) hrp5rgtarmlj5_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj6_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_rlink5.egg")) hrp5rgtarmlj6_model = loader.loadModel(hrp5rgtarmlj6_filepath) hrp5rgtarmlj6_nodepath = NodePath("nxtrgtarmlj6_nodepath") hrp5rgtarmlj6_model.instanceTo(hrp5rgtarmlj6_nodepath) hrp5rgtarmlj6_rotmat = pg.cvtMat4(hrp5robot.rgtarm[7]['rotmat'], hrp5robot.rgtarm[7]['linkpos']) hrp5rgtarmlj6_nodepath.setMat(hrp5rgtarmlj6_rotmat) hrp5rgtarmlj6_nodepath.setColor(.5, .5, .5, 1) hrp5rgtarmlj6_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj7_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5egg", "hrp5_rlink6.egg")) hrp5rgtarmlj7_model = loader.loadModel(hrp5rgtarmlj7_filepath) hrp5rgtarmlj7_nodepath = NodePath("nxtrgtarmlj7_nodepath") hrp5rgtarmlj7_model.instanceTo(hrp5rgtarmlj7_nodepath) hrp5rgtarmlj7_rotmat = pg.cvtMat4(hrp5robot.rgtarm[8]['rotmat'], hrp5robot.rgtarm[8]['linkpos']) hrp5rgtarmlj7_nodepath.setMat(hrp5rgtarmlj7_rotmat) hrp5rgtarmlj7_nodepath.setColor(.5, .5, .5, 1) hrp5rgtarmlj7_nodepath.reparentTo(hrp5mnp) # # # lftarm # nxtlftarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj0.egg")) # nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath) # nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath") # nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath) # nxtlftarmlj0_rotmat = pg.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos']) # nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat) # nxtlftarmlj0_nodepath.reparentTo(nxtmnp) # # nxtlftarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj1.egg")) # nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath) # nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath") # nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath) # nxtlftarmlj1_rotmat = pg.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos']) # nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat) # nxtlftarmlj1_nodepath.reparentTo(nxtmnp) # # nxtlftarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj2.egg")) # nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath) # nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath") # nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath) # nxtlftarmlj2_rotmat = pg.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos']) # nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat) # nxtlftarmlj2_nodepath.reparentTo(nxtmnp) # # nxtlftarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj3.egg")) # nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath) # nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath") # nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath) # nxtlftarmlj3_rotmat = pg.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos']) # nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat) # nxtlftarmlj3_nodepath.reparentTo(nxtmnp) # # nxtlftarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj4.egg")) # nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath) # nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath") # nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath) # nxtlftarmlj4_rotmat = pg.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos']) # nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat) # nxtlftarmlj4_nodepath.reparentTo(nxtmnp) # rgthnd hrp5robotrgthnd = rtq85.Rtq85() hrp5robotrgtarmlj9_rotmat = pg.cvtMat4(hrp5robot.rgtarm[9]['rotmat'], hrp5robot.rgtarm[9]['linkpos']) # pg.plotAxisSelf(hrp5mnp, hrp5robot.rgtarm[9]['linkend'], hrp5robotrgtarmlj9_rotmat) hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat) hrp5robotrgthnd.reparentTo(hrp5mnp) if jawwidthrgt is not None: hrp5robotrgthnd.setJawwidth(jawwidthrgt) # lfthnd # hrp5robotlfthnd = rtq85.Rtq85() # hrp5robotlftarmlj9_rotmat = pg.cvtMat4(hrp5robot.lftarm[9]['rotmat'], hrp5robot.lftarm[9]['linkpos']) # pg.plotAxisSelf(hrp5mnp, hrp5robot.lftarm[9]['linkend'], hrp5robotlftarmlj9_rotmat) # hrp5robotlfthnd.setMat(hrp5robotlftarmlj9_rotmat) # hrp5robotlfthnd.reparentTo(hrp5mnp) # if jawwidthlft is not None: # hrp5robotlfthnd.setJawwidth(jawwidthlft) return hrp5mnp
def genmnp(self, robot, jawwidthrgt=0, jawwidthlft=0, toggleendcoord=True, togglejntscoord=False, name='robotmesh'): """ generate a panda3d nodepath for the robot mnp indicates this function generates a mesh nodepath :param robot: the robot object, see robot.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20180109 """ robotmesh_nodepath = NodePath(name) identmat4 = Mat4.identMat() # body nxtwaist_rotmat = pg.cvtMat4(robot.base['rotmat']) self.__nxtwaist_nodepath.setMat(nxtwaist_rotmat) # rgtarm nxtrgtarmlj0_rotmat = pg.cvtMat4(robot.rgtarm[1]['rotmat'], robot.rgtarm[1]['linkpos']) self.__nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat) nxtrgtarmlj1_rotmat = pg.cvtMat4(robot.rgtarm[2]['rotmat'], robot.rgtarm[2]['linkpos']) self.__nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat) nxtrgtarmlj2_rotmat = pg.cvtMat4(robot.rgtarm[3]['rotmat'], robot.rgtarm[3]['linkpos']) self.__nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat) nxtrgtarmlj3_rotmat = pg.cvtMat4(robot.rgtarm[4]['rotmat'], robot.rgtarm[4]['linkpos']) self.__nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat) nxtrgtarmlj4_rotmat = pg.cvtMat4(robot.rgtarm[5]['rotmat'], robot.rgtarm[5]['linkpos']) self.__nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat) # lftarm nxtlftarmlj0_rotmat = pg.cvtMat4(robot.lftarm[1]['rotmat'], robot.lftarm[1]['linkpos']) self.__nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat) nxtlftarmlj1_rotmat = pg.cvtMat4(robot.lftarm[2]['rotmat'], robot.lftarm[2]['linkpos']) self.__nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat) nxtlftarmlj2_rotmat = pg.cvtMat4(robot.lftarm[3]['rotmat'], robot.lftarm[3]['linkpos']) self.__nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat) nxtlftarmlj3_rotmat = pg.cvtMat4(robot.lftarm[4]['rotmat'], robot.lftarm[4]['linkpos']) self.__nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat) nxtlftarmlj4_rotmat = pg.cvtMat4(robot.lftarm[5]['rotmat'], robot.lftarm[5]['linkpos']) self.__nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat) self.__nxtwaist_nodepath.reparentTo(robotmesh_nodepath) self.__nxtrgtarmlj0_nodepath.reparentTo(robotmesh_nodepath) self.__nxtrgtarmlj1_nodepath.reparentTo(robotmesh_nodepath) self.__nxtrgtarmlj2_nodepath.reparentTo(robotmesh_nodepath) self.__nxtrgtarmlj3_nodepath.reparentTo(robotmesh_nodepath) self.__nxtrgtarmlj4_nodepath.reparentTo(robotmesh_nodepath) self.__nxtlftarmlj0_nodepath.reparentTo(robotmesh_nodepath) self.__nxtlftarmlj1_nodepath.reparentTo(robotmesh_nodepath) self.__nxtlftarmlj2_nodepath.reparentTo(robotmesh_nodepath) self.__nxtlftarmlj3_nodepath.reparentTo(robotmesh_nodepath) # self.__nxtlftarmlj4_nodepath.reparentTo(robotmesh_nodepath) # rgthand nxtrgtarmlj5_rotmat = pg.cvtMat4(robot.rgtarm[6]['rotmat'], robot.rgtarm[6]['linkpos']) if self.rgthnd is not None: self.rgthnd.setMat(nxtrgtarmlj5_rotmat) self.rgthnd.setJawwidth(jawwidthrgt) self.rgthnd.reparentTo(robotmesh_nodepath) # lfthand nxtlftarmlj5_rotmat = pg.cvtMat4(robot.lftarm[6]['rotmat'], robot.lftarm[6]['linkpos']) if self.lfthnd is not None: self.lfthnd.setMat(nxtlftarmlj5_rotmat) self.lfthnd.setJawwidth(jawwidthlft) self.lfthnd.reparentTo(robotmesh_nodepath) # endcoord if toggleendcoord: self.pggen.plotAxis(robotmesh_nodepath, spos=robot.lftarm[6]['linkend'], pandamat4=nxtlftarmlj5_rotmat) self.pggen.plotAxis(robotmesh_nodepath, spos=robot.rgtarm[6]['linkend'], pandamat4=nxtrgtarmlj5_rotmat) # toggle all coord if togglejntscoord: for i in range(1, 7): self.pggen.plotAxis(robotmesh_nodepath, spos=robot.lftarm[i]['linkpos'], pandamat4=pg.cvtMat4( robot.lftarm[i - 1]['rotmat'], robot.lftarm[i]['linkpos'])) return copy.deepcopy(robotmesh_nodepath)
def removeHndccShow(self, base, discretesize=8): """ Handcc means hand collision detection This one is developed for demonstration This function should be called after executing removeHndcc :param discretesize: the number of hand orientations :return: delayTime author: weiwei date: 20161212, tsukuba """ # isplotted = 0 if self.rtq85plotlist: for rtq85plotnode in self.rtq85plotlist: rtq85plotnode.removeNode() self.rtq85plotlist = [] self.gripcontacts = [] self.griprotmats = [] self.gripjawwidth = [] self.gripcontactnormals = [] plotoffsetfp = 6 if self.counter2 == 0: self.counter += 1 if self.counter >= self.facetpairs.shape[0]: self.counter = 0 self.counter2 += 1 if self.counter2 >= discretesize: self.counter2 = 0 print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1) facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]): if j == 0: print j, contactpair # for angleid in range(discretesize): angleid = self.counter2 cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1] cctnormal0 = self.gripcontactpairnormals_precc[self.counter][j][0] cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]] tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) # save initial hand pose fgrdist = np.linalg.norm((cctpnt0 - cctpnt1)) # TODO setting jawwidth to 80 is enough # since fgrpcc already detects inner collisions tmprtq85.setJawwidth(fgrdist) tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2]) rotax = [0, 1, 0] rotangle = 360.0 / discretesize * angleid rotmat = rm.rodrigues(rotax, rotangle) tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat()) axx = tmprtq85.getMat().getRow3(0) # 130 is the distance from hndbase to fingertip cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]]) tmprtq85.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2])) # collision detection self.hndbullnode = cd.genCollisionMeshMultiNp(tmprtq85.rtq85np, base.render) result = self.bulletworld.contactTest(self.hndbullnode) if not result.getNumContacts(): self.gripcontacts.append(contactpair) self.griprotmats.append(tmprtq85.getMat()) self.gripjawwidth.append(fgrdist) self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j]) # pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1]) # pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]), # tmprtq85.getMat(), length=30, thickness=2) tmprtq85.setColor([1, 1, 1, .3]) tmprtq85.reparentTo(base.render) self.rtq85plotlist.append(tmprtq85) # isplotted = 1 else: for contact in result.getContacts(): # cp = contact.getManifoldPoint() # pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1)) # pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1)) tmprtq85.setColor([.5, 0, 0, .3]) tmprtq85.reparentTo(base.render) self.rtq85plotlist.append(tmprtq85)
def genmnp_list(self, hrp5nrobot, jawwidthrgt = None, jawwidthlft = None): """ generate a panda3d nodepath for the hrp5robot mnp indicates this function generates a mesh nodepath return [[right arm mnp list], [leftarm mnp list], [body mnp list]] where one arm mnp list is ordered from base to end-effector this function is designed for collision detection :param hrp5robot: the Hrp5Robot object, see hrp5.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20170613 """ identmat4 = Mat4.identMat() # body self.__hrp5nbody_nodepath.setMat(identmat4) self.__hrp5nbody_nodepath.setZ(self.__hrp5nmnp, 0) # chest 0 self.__hrp5nchest0_nodepath.setMat(identmat4) self.__hrp5nchest0_nodepath.setZ(self.__hrp5nmnp, 274) # chest 1 self.__hrp5nchest1_nodepath.setMat(identmat4) self.__hrp5nchest1_nodepath.setColor(.7,.7,.2,1) # chest 2 hrp5nchest2_rotmat = pg.cvtMat4(hrp5nrobot.base['rotmat']) self.__hrp5nchest2_nodepath.setMat(hrp5nchest2_rotmat) self.__hrp5nchest2_nodepath.setColor(.7,.7,.7,1) # head 0 self.__hrp5nhead0_nodepath.setMat(identmat4) self.__hrp5nhead0_nodepath.setH(hrp5nrobot.initjnts[0]) self.__hrp5nhead0_nodepath.setX(32) self.__hrp5nhead0_nodepath.setZ(521) # head 1 self.__hrp5nhead1_nodepath.setP(hrp5nrobot.initjnts[1]) self.__hrp5nhead1_nodepath.setX(40) self.__hrp5nhead1_nodepath.setColor(.5,.5,.5,1) # rgtarm 0 hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'], hrp5nrobot.rgtarm[1]['linkpos']) self.__hrp5nrgtarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj0_rotmat) self.__hrp5nrgtarmlj0_nodepath.setColor(.5, .5, .5, 1) # rgtarm 1 hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'], hrp5nrobot.rgtarm[2]['linkpos']) self.__hrp5nrgtarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj1_rotmat) self.__hrp5nrgtarmlj1_nodepath.setColor(.5, .5, .5, 1) # rgtarm 2 hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'], hrp5nrobot.rgtarm[3]['linkpos']) self.__hrp5nrgtarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj2_rotmat) self.__hrp5nrgtarmlj2_nodepath.setColor(.5, .5, .5, 1) # rgtarm 3 hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'], hrp5nrobot.rgtarm[4]['linkpos']) self.__hrp5nrgtarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj3_rotmat) self.__hrp5nrgtarmlj3_nodepath.setColor(.5, .5, .5, 1) # rgtarm 4 hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'], hrp5nrobot.rgtarm[5]['linkpos']) self.__hrp5nrgtarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj4_rotmat) self.__hrp5nrgtarmlj4_nodepath.setColor(.5, .5, .5, 1) # rgtarm 5 hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'], hrp5nrobot.rgtarm[6]['linkpos']) self.__hrp5nrgtarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj5_rotmat) self.__hrp5nrgtarmlj5_nodepath.setColor(.5, .5, .5, 1) # rgtarm 6 hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'], hrp5nrobot.rgtarm[7]['linkpos']) self.__hrp5nrgtarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj6_rotmat) self.__hrp5nrgtarmlj6_nodepath.setColor(.5, .5, .5, 1) # rgtarm 7 hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'], hrp5nrobot.rgtarm[8]['linkpos']) self.__hrp5nrgtarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj7_rotmat) self.__hrp5nrgtarmlj7_nodepath.setColor(.5, .5, .5, 1) # lftarm 0 hrp5nlftarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[1]['rotmat'], hrp5nrobot.lftarm[1]['linkpos']) self.__hrp5nlftarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj0_rotmat) self.__hrp5nlftarmlj0_nodepath.setColor(.5, .5, .5, 1) # lftarm 1 hrp5nlftarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[2]['rotmat'], hrp5nrobot.lftarm[2]['linkpos']) self.__hrp5nlftarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj1_rotmat) self.__hrp5nlftarmlj1_nodepath.setColor(.5, .5, .5, 1) # lftarm 2 hrp5nlftarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[3]['rotmat'], hrp5nrobot.lftarm[3]['linkpos']) self.__hrp5nlftarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj2_rotmat) self.__hrp5nlftarmlj2_nodepath.setColor(.5, .5, .5, 1) # lftarm 3 hrp5nlftarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[4]['rotmat'], hrp5nrobot.lftarm[4]['linkpos']) self.__hrp5nlftarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj3_rotmat) self.__hrp5nlftarmlj3_nodepath.setColor(.5, .5, .5, 1) # lftarm 4 hrp5nlftarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[5]['rotmat'], hrp5nrobot.lftarm[5]['linkpos']) self.__hrp5nlftarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj4_rotmat) self.__hrp5nlftarmlj4_nodepath.setColor(.5, .5, .5, 1) # lftarm 5 hrp5nlftarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[6]['rotmat'], hrp5nrobot.lftarm[6]['linkpos']) self.__hrp5nlftarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj5_rotmat) self.__hrp5nlftarmlj5_nodepath.setColor(.5, .5, .5, 1) # lftarm 6 hrp5nlftarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[7]['rotmat'], hrp5nrobot.lftarm[7]['linkpos']) self.__hrp5nlftarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj6_rotmat) self.__hrp5nlftarmlj6_nodepath.setColor(.5, .5, .5, 1) # lftarm 7 hrp5nlftarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[8]['rotmat'], hrp5nrobot.lftarm[8]['linkpos']) self.__hrp5nlftarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj7_rotmat) self.__hrp5nlftarmlj7_nodepath.setColor(.5, .5, .5, 1) # rgthand hrp5nrobotrgtarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[9]['rotmat'], hrp5nrobot.rgtarm[9]['linkpos']) self.hrp5nrobotrgthnd.setMat(self.__hrp5nmnp, hrp5nrobotrgtarmlj9_rotmat) if jawwidthrgt is not None: self.hrp5nrobotrgthnd.setJawwidth(jawwidthrgt) # lfthand hrp5nrobotlftarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[9]['rotmat'], hrp5nrobot.lftarm[9]['linkpos']) self.hrp5nrobotlfthnd.setMat(self.__hrp5nmnp, hrp5nrobotlftarmlj9_rotmat) if jawwidthlft is not None: self.hrp5nrobotlfthnd.setJawwidth(jawwidthlft) return copy.deepcopy([[self.__hrp5nrgtarmlj0_nodepath, self.__hrp5nrgtarmlj1_nodepath, self.__hrp5nrgtarmlj2_nodepath, self.__hrp5nrgtarmlj3_nodepath, self.__hrp5nrgtarmlj4_nodepath, self.__hrp5nrgtarmlj5_nodepath, self.__hrp5nrgtarmlj6_nodepath, self.__hrp5nrgtarmlj7_nodepath, self.hrp5nrobotrgthnd.handnp], [self.__hrp5nlftarmlj0_nodepath, self.__hrp5nlftarmlj1_nodepath, self.__hrp5nlftarmlj2_nodepath, self.__hrp5nlftarmlj3_nodepath, self.__hrp5nlftarmlj4_nodepath, self.__hrp5nlftarmlj5_nodepath, self.__hrp5nlftarmlj6_nodepath, self.__hrp5nlftarmlj7_nodepath, self.hrp5nrobotlfthnd.handnp], [self.__hrp5nbody_nodepath]])
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 __init__(self, jawwidth=85): ''' load the robotiq85 model, set jawwidth and return a nodepath the rtq85 gripper is composed of a parallelism and a fixed triangle, the parallelism: 1.905-1.905; 5.715-5.715; 70/110 degree the triangle: 4.75 (finger) 5.715 (inner knuckle) 3.175 (outer knuckle) ## input pandabase: the showbase() object jawwidth: the distance between fingertips ## output rtq85np: the nodepath of this rtq85 hand author: weiwei date: 20160627 ''' self.rtq85np = NodePath("rtq85hnd") self.handnp = self.rtq85np self.jawwidth = jawwidth this_dir, this_filename = os.path.split(__file__) rtq85basepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg", "robotiq_85_base_link.egg")) rtq85fingerpath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg", "robotiq_85_finger_link.egg")) rtq85fingertippath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg", "robotiq_85_finger_tip_link.egg")) rtq85innerknucklepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg", "robotiq_85_inner_knuckle_link.egg")) rtq85knucklepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg", "robotiq_85_knuckle_link.egg")) rtq85base = NodePath("rtq85base") rtq85lknuckle = NodePath("rtq85lknuckle") rtq85rknuckle = NodePath("rtq85rknuckle") rtq85lfgr = NodePath("rtq85lfgr") rtq85rfgr = NodePath("rtq85rfgr") rtq85ilknuckle = NodePath("rtq85ilknuckle") rtq85irknuckle = NodePath("rtq85irknuckle") rtq85lfgrtip = NodePath("rtq85lfgrtip") rtq85rfgrtip = NodePath("rtq85rfgrtip") # loader is a global variable defined by panda3d rtq85_basel = loader.loadModel(rtq85basepath) rtq85_fingerl = loader.loadModel(rtq85fingerpath) rtq85_fingertipl = loader.loadModel(rtq85fingertippath) rtq85_innerknucklel = loader.loadModel(rtq85innerknucklepath) rtq85_knucklel = loader.loadModel(rtq85knucklepath) # base rtq85_basel.instanceTo(rtq85base) rtq85base.setPos(0,0,0) # left and right outer knuckle rtq85_knucklel.instanceTo(rtq85lknuckle) rtq85lknuckle.setPos(-30.60114443, 54.90451627, 0) rtq85lknuckle.setHpr(0, 0, 180) rtq85lknuckle.reparentTo(rtq85base) rtq85_knucklel.instanceTo(rtq85rknuckle) rtq85rknuckle.setPos(30.60114443, 54.90451627, 0) rtq85rknuckle.setHpr(0, 0, 0) rtq85rknuckle.reparentTo(rtq85base) # left and right finger rtq85_fingerl.instanceTo(rtq85lfgr) rtq85lfgr.setPos(31.48504435, -4.08552455, 0) rtq85lfgr.reparentTo(rtq85lknuckle) rtq85_fingerl.instanceTo(rtq85rfgr) rtq85rfgr.setPos(31.48504435, -4.08552455, 0) rtq85rfgr.reparentTo(rtq85rknuckle) # left and right inner knuckle rtq85_innerknucklel.instanceTo(rtq85ilknuckle) rtq85ilknuckle.setPos(-12.7, 61.42, 0) rtq85ilknuckle.setHpr(0, 0, 180) rtq85ilknuckle.reparentTo(rtq85base) rtq85_innerknucklel.instanceTo(rtq85irknuckle) rtq85irknuckle.setPos(12.7, 61.42, 0) rtq85irknuckle.setHpr(0, 0, 0) rtq85irknuckle.reparentTo(rtq85base) # left and right fgr tip rtq85_fingertipl.instanceTo(rtq85lfgrtip) rtq85lfgrtip.setPos(37.59940821, 43.03959807, 0) rtq85lfgrtip.reparentTo(rtq85ilknuckle) rtq85_fingertipl.instanceTo(rtq85rfgrtip) rtq85rfgrtip.setPos(37.59940821, 43.03959807, 0) rtq85rfgrtip.setHpr(0, 0, 0) rtq85rfgrtip.reparentTo(rtq85irknuckle) # rotate to x, y, z coordinates (this one rotates the base, not the self.rtq85np) # the default x direction is facing the ee, the default z direction is facing downward # execute this file to see the default pose rtq85base.setMat(pandageom.cvtMat4(rm.rodrigues([0,0,1], 90))*rtq85base.getMat()) rtq85base.reparentTo(self.rtq85np) self.setJawwidth(jawwidth) self.__jawwidthopen = 85.0 self.__jawwidthclosed = 0.0
def saveToDB(self, positionlist, gdb, discretesize=8.0): """ :param positionlist: a list of positions to place the object one the table :param discretesize: the discretization of rotation angles around z axis :return: author: weiwei date: 20161215, osaka """ # save discretiezed angle sql = "SELECT * FROM angle" result = gdb.execute(sql) if len(result) == 0: sql = "INSERT INTO angle(value) VALUES " for i in range(discretesize): sql += "("+str(360*i*1.0/discretesize)+"), " sql = sql[:-2]+";" gdb.execute(sql) else: print "Angles already set!" # save tabletopplacements sql = "SELECT idtabletopplacements FROM tabletopplacements,freetabletopplacement,object WHERE \ tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \ freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: # 1) select the freetabletopplacement sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \ FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \ AND object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: raise ValueError("Plan the freetabletopplacement first!") result = np.asarray(result) idfreelist = [int(x) for x in result[:, 0]] rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]] # 2) select the angle sql = "SELECT angle.idangle,angle.value FROM angle" result = np.asarray(gdb.execute(sql)) idanglelist = [int(x) for x in result[:, 0]] anglevaluelist = [float(x) for x in result[:, 1]] # 3) save to database sql = "INSERT INTO tabletopplacements(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES " for ttoppos in positionlist: ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2]) for idfree, rotmatfree in zip(idfreelist, rotmatfreelist): for idangle, anglevalue in zip(idanglelist, anglevaluelist): rotangle = anglevalue rotmat = rm.rodrigues([0, 0, 1], rotangle) # rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0, # rotmat[1][0], rotmat[1][1], rotmat[1][2], 0, # rotmat[2][0], rotmat[2][1], rotmat[2][2], 0, # ttoppos[0], ttoppos[1], ttoppos[2], 1) rotmat4 = pg.cvtMat4(rotmat, ttoppos) varrotmat = rotmatfree * rotmat4 sql += "('%s', '%s', %d, %d), " % \ (dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree) sql = sql[:-2]+";" gdb.execute(sql) else: print "Tabletopplacements already exist!" # save tabletopgrips idhand = gdb.loadIdHand(self.handname) sql = "SELECT tabletopgrips.idtabletopgrips FROM tabletopgrips,freeairgrip,object WHERE \ tabletopgrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \ freeairgrip.idobject=object.idobject AND object.name LIKE '%s' AND \ freeairgrip.idhand = %d" % (self.dbobjname, idhand) result = gdb.execute(sql) if len(result) == 0: sql = "SELECT freetabletopplacement.idfreetabletopplacement \ FROM freetabletopplacement,object WHERE \ freetabletopplacement.idobject = object.idobject AND \ object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: raise ValueError("Plan the freetabletopplacement first!") for idfree in result: idfree = int(idfree[0]) sql = "SELECT tabletopplacements.idtabletopplacements, \ tabletopplacements.tabletopposition, angle.value,\ freetabletopgrip.contactpoint0, freetabletopgrip.contactpoint1, \ freetabletopgrip.contactnormal0, freetabletopgrip.contactnormal1, \ freetabletopgrip.rotmat, freetabletopgrip.jawwidth, freetabletopgrip.idfreeairgrip \ FROM tabletopplacements,freetabletopplacement,freetabletopgrip,freeairgrip,angle WHERE \ tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \ tabletopplacements.idangle = angle.idangle AND \ freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \ freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \ freeairgrip.idhand = %d AND \ freetabletopplacement.idfreetabletopplacement = %d" % (idhand, idfree) result1 = gdb.execute(sql) if len(result1) == 0: # no grasp availalbe? continue # sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, \ # contactnormal1, rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES " if len(result1) > 20000: result1 = result1[0::int(len(result1)/20000.0)] result1 = np.asarray(result1) idtabletopplacementslist = [int(x) for x in result1[:,0]] tabletoppositionlist = [dc.strToV3(x) for x in result1[:,1]] rotanglelist = [float(x) for x in result1[:,2]] freegripcontactpoint0list = [dc.strToV3(x) for x in result1[:,3]] freegripcontactpoint1list = [dc.strToV3(x) for x in result1[:,4]] freegripcontactnormal0list = [dc.strToV3(x) for x in result1[:,5]] freegripcontactnormal1list = [dc.strToV3(x) for x in result1[:,6]] freegriprotmatlist = [dc.strToMat4(x) for x in result1[:,7]] freegripjawwidthlist = [float(x) for x in result1[:,8]] freegripidlist = [int(x) for x in result1[:,9]] for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \ freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \ tabletoppositionlist, rotanglelist, freegripcontactpoint0list, freegripcontactpoint1list, \ freegripcontactnormal0list, freegripcontactnormal1list, freegriprotmatlist, freegripjawwidthlist, \ freegripidlist): rotmat = rm.rodrigues([0, 0, 1], rotangle) # rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0, # rotmat[1][0], rotmat[1][1], rotmat[1][2], 0, # rotmat[2][0], rotmat[2][1], rotmat[2][2], 0, # ttoppos[0], ttoppos[1], ttoppos[2], 1) rotmat4 = pg.cvtMat4(rotmat, ttoppos) ttpcct0 = rotmat4.xformPoint(cct0) ttpcct1 = rotmat4.xformPoint(cct1) ttpcctn0 = rotmat4.xformVec(cctn0) ttpcctn1 = rotmat4.xformVec(cctn1) ttpgriprotmat = freegriprotmat*rotmat4 sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \ rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES \ ('%s', '%s', '%s', '%s', '%s', '%s', %d, %d) " % \ (dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \ dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements) gdb.execute(sql) else: print "Tabletopgrips already exist!" print "Save to DB done!"
def genmnp(nxtrobot, handpkg, 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 nextage.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20161109 """ nxtmnp = NodePath("nxtmnp") this_dir, this_filename = os.path.split(__file__) # mainbody, waist, body, head (neck is not plotted) nxtwaist_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_waist.egg")) nxtbody_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_body.egg")) nxthead_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_head.egg")) nxtwaist_model = loader.loadModel(nxtwaist_filepath) nxtbody_model = loader.loadModel(nxtbody_filepath) nxthead_model = loader.loadModel(nxthead_filepath) nxtwaist_nodepath = NodePath("nxtwaist") nxtbody_nodepath = NodePath("nxtbody") nxthead_nodepath = NodePath("nxthead") nxtwaist_model.instanceTo(nxtwaist_nodepath) nxtwaist_rotmat = pandageom.cvtMat4(nxtrobot.base['rotmat']) nxtwaist_nodepath.setMat(nxtwaist_rotmat) nxtbody_model.instanceTo(nxtbody_nodepath) nxthead_model.instanceTo(nxthead_nodepath) nxthead_nodepath.setH(nxtrobot.initjnts[1]) nxthead_nodepath.setP(nxtrobot.initjnts[2]) nxthead_nodepath.setZ(569.5) nxtwaist_nodepath.reparentTo(nxtmnp) nxtbody_nodepath.reparentTo(nxtwaist_nodepath) nxthead_nodepath.reparentTo(nxtwaist_nodepath) # rgtarm nxtrgtarmlj0_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj0.egg")) nxtrgtarmlj0_model = loader.loadModel(nxtrgtarmlj0_filepath) nxtrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath") nxtrgtarmlj0_model.instanceTo(nxtrgtarmlj0_nodepath) nxtrgtarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[1]['rotmat'], nxtrobot.rgtarm[1]['linkpos']) nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat) nxtrgtarmlj0_nodepath.reparentTo(nxtmnp) nxtrgtarmlj1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj1.egg")) nxtrgtarmlj1_model = loader.loadModel(nxtrgtarmlj1_filepath) nxtrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath") nxtrgtarmlj1_model.instanceTo(nxtrgtarmlj1_nodepath) nxtrgtarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[2]['rotmat'], nxtrobot.rgtarm[2]['linkpos']) nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat) nxtrgtarmlj1_nodepath.reparentTo(nxtmnp) nxtrgtarmlj2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj2.egg")) nxtrgtarmlj2_model = loader.loadModel(nxtrgtarmlj2_filepath) nxtrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath") nxtrgtarmlj2_model.instanceTo(nxtrgtarmlj2_nodepath) nxtrgtarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[3]['rotmat'], nxtrobot.rgtarm[3]['linkpos']) nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat) nxtrgtarmlj2_nodepath.reparentTo(nxtmnp) nxtrgtarmlj3_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj3.egg")) nxtrgtarmlj3_model = loader.loadModel(nxtrgtarmlj3_filepath) nxtrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath") nxtrgtarmlj3_model.instanceTo(nxtrgtarmlj3_nodepath) nxtrgtarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[4]['rotmat'], nxtrobot.rgtarm[4]['linkpos']) nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat) nxtrgtarmlj3_nodepath.reparentTo(nxtmnp) nxtrgtarmlj4_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj4.egg")) nxtrgtarmlj4_model = loader.loadModel(nxtrgtarmlj4_filepath) nxtrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath") nxtrgtarmlj4_model.instanceTo(nxtrgtarmlj4_nodepath) nxtrgtarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[5]['rotmat'], nxtrobot.rgtarm[5]['linkpos']) nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat) nxtrgtarmlj4_nodepath.reparentTo(nxtmnp) # lftarm nxtlftarmlj0_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj0.egg")) nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath) nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath") nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath) nxtlftarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos']) nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat) nxtlftarmlj0_nodepath.reparentTo(nxtmnp) nxtlftarmlj1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj1.egg")) nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath) nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath") nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath) nxtlftarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos']) nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat) nxtlftarmlj1_nodepath.reparentTo(nxtmnp) nxtlftarmlj2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj2.egg")) nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath) nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath") nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath) nxtlftarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos']) nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat) nxtlftarmlj2_nodepath.reparentTo(nxtmnp) nxtlftarmlj3_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj3.egg")) nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath) nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath") nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath) nxtlftarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos']) nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat) nxtlftarmlj3_nodepath.reparentTo(nxtmnp) nxtlftarmlj4_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj4.egg")) nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath) nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath") nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath) nxtlftarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos']) nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat) nxtlftarmlj4_nodepath.reparentTo(nxtmnp) # rgthnd nxtrgthnd = handpkg.newHand('rgt') nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos']) nxtrgthnd.setMat(nxtlftarmlj5_rotmat) nxtrgthnd.reparentTo(nxtmnp) if jawwidthrgt is not None: nxtrgthnd.setJawwidth(jawwidthrgt) # lfthnd nxtlfthnd = handpkg.newHand('lft') nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[6]['rotmat'], nxtrobot.lftarm[6]['linkpos']) nxtlfthnd.setMat(nxtlftarmlj5_rotmat) nxtlfthnd.reparentTo(nxtmnp) if jawwidthlft is not None: nxtlfthnd.setJawwidth(jawwidthlft) return nxtmnp
def genmnp_nm(nxtrobot, handpkg, plotcolor=[.5, .5, .5, .3], jawwidthrgt=None, jawwidthlft=None): """ generate a panda3d nodepath for the nxtrobot mnp indicates this function generates a mesh nodepath nm means no material :param nxtrobot: the NxtRobot object, see nextage.py :param plotcolor: the color of the model, alpha allowed :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20161109 """ nxtmnp = NodePath("nxtmnp") this_dir, this_filename = os.path.split(__file__) # mainbody, waist, body, head (neck is not plotted) nxtwaist_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_waist_nm.egg")) nxtbody_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_body_nm.egg")) nxthead_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_head_nm.egg")) nxtwaist_model = loader.loadModel(nxtwaist_filepath) nxtbody_model = loader.loadModel(nxtbody_filepath) nxthead_model = loader.loadModel(nxthead_filepath) nxtwaist_nodepath = NodePath("nxtwaist_nm") nxtbody_nodepath = NodePath("nxtbody_nm") nxthead_nodepath = NodePath("nxthead_nm") nxtwaist_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtwaist_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtbody_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtbody_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxthead_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxthead_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtwaist_model.instanceTo(nxtwaist_nodepath) nxtwaist_rotmat = pandageom.cvtMat4(nxtrobot.base['rotmat']) nxtwaist_nodepath.setMat(nxtwaist_rotmat) nxtbody_model.instanceTo(nxtbody_nodepath) nxthead_model.instanceTo(nxthead_nodepath) nxthead_nodepath.setH(nxtrobot.initjnts[1]) nxthead_nodepath.setP(nxtrobot.initjnts[2]) nxthead_nodepath.setZ(569.5) nxtwaist_nodepath.reparentTo(nxtmnp) nxtbody_nodepath.reparentTo(nxtwaist_nodepath) nxthead_nodepath.reparentTo(nxtwaist_nodepath) # rgtarm nxtrgtarmlj0_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj0_nm.egg")) nxtrgtarmlj0_model = loader.loadModel(nxtrgtarmlj0_filepath) nxtrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath") nxtrgtarmlj0_model.instanceTo(nxtrgtarmlj0_nodepath) nxtrgtarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[1]['rotmat'], nxtrobot.rgtarm[1]['linkpos']) nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat) nxtrgtarmlj0_nodepath.reparentTo(nxtmnp) nxtrgtarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtrgtarmlj0_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtrgtarmlj1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj1_nm.egg")) nxtrgtarmlj1_model = loader.loadModel(nxtrgtarmlj1_filepath) nxtrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath") nxtrgtarmlj1_model.instanceTo(nxtrgtarmlj1_nodepath) nxtrgtarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[2]['rotmat'], nxtrobot.rgtarm[2]['linkpos']) nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat) nxtrgtarmlj1_nodepath.reparentTo(nxtmnp) nxtrgtarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtrgtarmlj1_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtrgtarmlj2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj2_nm.egg")) nxtrgtarmlj2_model = loader.loadModel(nxtrgtarmlj2_filepath) nxtrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath") nxtrgtarmlj2_model.instanceTo(nxtrgtarmlj2_nodepath) nxtrgtarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[3]['rotmat'], nxtrobot.rgtarm[3]['linkpos']) nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat) nxtrgtarmlj2_nodepath.reparentTo(nxtmnp) nxtrgtarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtrgtarmlj2_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtrgtarmlj3_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj3_nm.egg")) nxtrgtarmlj3_model = loader.loadModel(nxtrgtarmlj3_filepath) nxtrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath") nxtrgtarmlj3_model.instanceTo(nxtrgtarmlj3_nodepath) nxtrgtarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[4]['rotmat'], nxtrobot.rgtarm[4]['linkpos']) nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat) nxtrgtarmlj3_nodepath.reparentTo(nxtmnp) nxtrgtarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtrgtarmlj3_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtrgtarmlj4_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj4_nm.egg")) nxtrgtarmlj4_model = loader.loadModel(nxtrgtarmlj4_filepath) nxtrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath") nxtrgtarmlj4_model.instanceTo(nxtrgtarmlj4_nodepath) nxtrgtarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[5]['rotmat'], nxtrobot.rgtarm[5]['linkpos']) nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat) nxtrgtarmlj4_nodepath.reparentTo(nxtmnp) nxtrgtarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtrgtarmlj4_nodepath.setTransparency(TransparencyAttrib.MAlpha) # lftarm nxtlftarmlj0_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj0_nm.egg")) nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath) nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath") nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath) nxtlftarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos']) nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat) nxtlftarmlj0_nodepath.reparentTo(nxtmnp) nxtlftarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtlftarmlj0_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtlftarmlj1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj1_nm.egg")) nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath) nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath") nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath) nxtlftarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos']) nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat) nxtlftarmlj1_nodepath.reparentTo(nxtmnp) nxtlftarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtlftarmlj1_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtlftarmlj2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj2_nm.egg")) nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath) nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath") nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath) nxtlftarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos']) nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat) nxtlftarmlj2_nodepath.reparentTo(nxtmnp) nxtlftarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtlftarmlj2_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtlftarmlj3_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj3_nm.egg")) nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath) nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath") nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath) nxtlftarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos']) nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat) nxtlftarmlj3_nodepath.reparentTo(nxtmnp) nxtlftarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtlftarmlj3_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtlftarmlj4_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj4_nm.egg")) nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath) nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath") nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath) nxtlftarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos']) nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat) nxtlftarmlj4_nodepath.reparentTo(nxtmnp) nxtlftarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) nxtlftarmlj4_nodepath.setTransparency(TransparencyAttrib.MAlpha) # rgthnd nxtrgthnd = handpkg.newHandNM('rgt', hndcolor=plotcolor) nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos']) nxtrgthnd.setMat(nxtlftarmlj5_rotmat) nxtrgthnd.reparentTo(nxtmnp) if jawwidthrgt is not None: nxtrgthnd.setJawwidth(jawwidthrgt) # lfthnd nxtlfthnd = handpkg.newHandNM('lft', hndcolor=plotcolor) nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[6]['rotmat'], nxtrobot.lftarm[6]['linkpos']) nxtlfthnd.setMat(nxtlftarmlj5_rotmat) nxtlfthnd.reparentTo(nxtmnp) if jawwidthlft is not None: nxtlfthnd.setJawwidth(jawwidthlft) nxtmnp.setTransparency(TransparencyAttrib.MAlpha) return nxtmnp
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_nm(hrp5nrobot, handpkg, jawwidthrgt = None, jawwidthlft = None): """ generate a panda3d nodepath for the hrp5robot mnp indicates this function generates a mesh nodepath :param hrp5robot: the Hrp5Robot object, see hrp5.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20161202 """ hrp5nmnp = NodePath("hrp5nmnp") this_dir, this_filename = os.path.split(__file__) # chest0-2, head1 (neck is not plotted) hrp5nbody_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Body.egg")) hrp5nchest0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Chest_Link0.egg")) hrp5nchest1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Chest_Link1.egg")) hrp5nchest2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Chest_Link2.egg")) hrp5nhead0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Head_Link0.egg")) hrp5nhead1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg/nomat", "HRP-5P_ConceptDesign_Head_Link1.egg")) hrp5nbody_model = loader.loadModel(hrp5nbody_filepath) hrp5nchest0_model = loader.loadModel(hrp5nchest0_filepath) hrp5nchest1_model = loader.loadModel(hrp5nchest1_filepath) hrp5nchest2_model = loader.loadModel(hrp5nchest2_filepath) hrp5nhead0_model = loader.loadModel(hrp5nhead0_filepath) hrp5nhead1_model = loader.loadModel(hrp5nhead1_filepath) hrp5nbody_nodepath = NodePath("hrp5nbody") hrp5nchest0_nodepath = NodePath("hrp5nchest0") hrp5nchest1_nodepath = NodePath("hrp5nchest1") hrp5nchest2_nodepath = NodePath("hrp5nchest2") hrp5nhead0_nodepath = NodePath("hrp5nhead0") hrp5nhead1_nodepath = NodePath("hrp5nhead1") # body hrp5nbody_model.instanceTo(hrp5nbody_nodepath) hrp5nbody_rotmat = Mat4.identMat() hrp5nbody_nodepath.setMat(hrp5nbody_rotmat) hrp5nbody_nodepath.setZ(0) # chest hrp5nchest0_model.instanceTo(hrp5nchest0_nodepath) hrp5nchest0_rotmat = Mat4.identMat() hrp5nchest0_nodepath.setMat(hrp5nchest0_rotmat) hrp5nchest0_nodepath.setZ(274) hrp5nchest1_model.instanceTo(hrp5nchest1_nodepath) hrp5nchest1_nodepath.setColor(.7,.7,.2,1) hrp5nchest2_model.instanceTo(hrp5nchest2_nodepath) hrp5nchest2_rotmat = pg.cvtMat4(hrp5nrobot.base['rotmat']) hrp5nchest2_nodepath.setMat(hrp5nchest2_rotmat) hrp5nchest2_nodepath.setColor(.7,.7,.7,1) hrp5nhead0_model.instanceTo(hrp5nhead0_nodepath) hrp5nhead0_nodepath.setH(hrp5nrobot.initjnts[0]) hrp5nhead0_nodepath.setX(32) hrp5nhead0_nodepath.setZ(521) hrp5nhead1_model.instanceTo(hrp5nhead1_nodepath) hrp5nhead1_nodepath.setP(hrp5nrobot.initjnts[1]) hrp5nhead1_nodepath.setX(40) hrp5nhead1_nodepath.setColor(.5,.5,.5,1) hrp5nbody_nodepath.reparentTo(hrp5nmnp) hrp5nchest0_nodepath.reparentTo(hrp5nbody_nodepath) hrp5nchest1_nodepath.reparentTo(hrp5nchest0_nodepath) hrp5nchest2_nodepath.reparentTo(hrp5nchest1_nodepath) hrp5nhead0_nodepath.reparentTo(hrp5nchest2_nodepath) hrp5nhead1_nodepath.reparentTo(hrp5nhead0_nodepath) # rgtarm hrp5nrgtarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rscap_Link0.egg")) hrp5nrgtarmlj0_model = loader.loadModel(hrp5nrgtarmlj0_filepath) hrp5nrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath") hrp5nrgtarmlj0_model.instanceTo(hrp5nrgtarmlj0_nodepath) hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'], hrp5nrobot.rgtarm[1]['linkpos']) hrp5nrgtarmlj0_nodepath.setMat(hrp5nrgtarmlj0_rotmat) hrp5nrgtarmlj0_nodepath.setColor(.5,.5,.5,1) hrp5nrgtarmlj0_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link0.egg")) hrp5nrgtarmlj1_model = loader.loadModel(hrp5nrgtarmlj1_filepath) hrp5nrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath") hrp5nrgtarmlj1_model.instanceTo(hrp5nrgtarmlj1_nodepath) hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'], hrp5nrobot.rgtarm[2]['linkpos']) hrp5nrgtarmlj1_nodepath.setMat(hrp5nrgtarmlj1_rotmat) hrp5nrgtarmlj1_nodepath.setColor(.5,.5,.5,1) hrp5nrgtarmlj1_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link1.egg")) hrp5nrgtarmlj2_model = loader.loadModel(hrp5nrgtarmlj2_filepath) hrp5nrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath") hrp5nrgtarmlj2_model.instanceTo(hrp5nrgtarmlj2_nodepath) hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'], hrp5nrobot.rgtarm[3]['linkpos']) hrp5nrgtarmlj2_nodepath.setMat(hrp5nrgtarmlj2_rotmat) hrp5nrgtarmlj2_nodepath.setColor(.5,.5,.5,1) hrp5nrgtarmlj2_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link2.egg")) hrp5nrgtarmlj3_model = loader.loadModel(hrp5nrgtarmlj3_filepath) hrp5nrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath") hrp5nrgtarmlj3_model.instanceTo(hrp5nrgtarmlj3_nodepath) hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'], hrp5nrobot.rgtarm[4]['linkpos']) hrp5nrgtarmlj3_nodepath.setMat(hrp5nrgtarmlj3_rotmat) hrp5nrgtarmlj3_nodepath.setColor(.5,.5,.5,1) hrp5nrgtarmlj3_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link3.egg")) hrp5nrgtarmlj4_model = loader.loadModel(hrp5nrgtarmlj4_filepath) hrp5nrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath") hrp5nrgtarmlj4_model.instanceTo(hrp5nrgtarmlj4_nodepath) hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'], hrp5nrobot.rgtarm[5]['linkpos']) hrp5nrgtarmlj4_nodepath.setMat(hrp5nrgtarmlj4_rotmat) hrp5nrgtarmlj4_nodepath.setColor(.5,.5,.5,1) hrp5nrgtarmlj4_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj5_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link4.egg")) hrp5nrgtarmlj5_model = loader.loadModel(hrp5nrgtarmlj5_filepath) hrp5nrgtarmlj5_nodepath = NodePath("nxtrgtarmlj5_nodepath") hrp5nrgtarmlj5_model.instanceTo(hrp5nrgtarmlj5_nodepath) hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'], hrp5nrobot.rgtarm[6]['linkpos']) hrp5nrgtarmlj5_nodepath.setMat(hrp5nrgtarmlj5_rotmat) hrp5nrgtarmlj5_nodepath.setColor(.5,.5,.5,1) hrp5nrgtarmlj5_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj6_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link5.egg")) hrp5nrgtarmlj6_model = loader.loadModel(hrp5nrgtarmlj6_filepath) hrp5nrgtarmlj6_nodepath = NodePath("nxtrgtarmlj6_nodepath") hrp5nrgtarmlj6_model.instanceTo(hrp5nrgtarmlj6_nodepath) hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'], hrp5nrobot.rgtarm[7]['linkpos']) hrp5nrgtarmlj6_nodepath.setMat(hrp5nrgtarmlj6_rotmat) hrp5nrgtarmlj6_nodepath.setColor(.5,.5,.5,1) hrp5nrgtarmlj6_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj7_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link6.egg")) hrp5nrgtarmlj7_model = loader.loadModel(hrp5nrgtarmlj7_filepath) hrp5nrgtarmlj7_nodepath = NodePath("nxtrgtarmlj7_nodepath") hrp5nrgtarmlj7_model.instanceTo(hrp5nrgtarmlj7_nodepath) hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'], hrp5nrobot.rgtarm[8]['linkpos']) hrp5nrgtarmlj7_nodepath.setMat(hrp5nrgtarmlj7_rotmat) hrp5nrgtarmlj7_nodepath.setColor(.5,.5,.5,1) hrp5nrgtarmlj7_nodepath.reparentTo(hrp5nmnp) # # rgtarm hrp5nrgtarmlj0_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rscap_Link0.egg")) hrp5nrgtarmlj0_model = loader.loadModel(hrp5nrgtarmlj0_filepath) hrp5nrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath") hrp5nrgtarmlj0_model.instanceTo(hrp5nrgtarmlj0_nodepath) hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'], hrp5nrobot.rgtarm[1]['linkpos']) hrp5nrgtarmlj0_nodepath.setMat(hrp5nrgtarmlj0_rotmat) hrp5nrgtarmlj0_nodepath.setColor(.5, .5, .5, 1) hrp5nrgtarmlj0_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link0.egg")) hrp5nrgtarmlj1_model = loader.loadModel(hrp5nrgtarmlj1_filepath) hrp5nrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath") hrp5nrgtarmlj1_model.instanceTo(hrp5nrgtarmlj1_nodepath) hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'], hrp5nrobot.rgtarm[2]['linkpos']) hrp5nrgtarmlj1_nodepath.setMat(hrp5nrgtarmlj1_rotmat) hrp5nrgtarmlj1_nodepath.setColor(.5, .5, .5, 1) hrp5nrgtarmlj1_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link1.egg")) hrp5nrgtarmlj2_model = loader.loadModel(hrp5nrgtarmlj2_filepath) hrp5nrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath") hrp5nrgtarmlj2_model.instanceTo(hrp5nrgtarmlj2_nodepath) hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'], hrp5nrobot.rgtarm[3]['linkpos']) hrp5nrgtarmlj2_nodepath.setMat(hrp5nrgtarmlj2_rotmat) hrp5nrgtarmlj2_nodepath.setColor(.5, .5, .5, 1) hrp5nrgtarmlj2_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj3_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link2.egg")) hrp5nrgtarmlj3_model = loader.loadModel(hrp5nrgtarmlj3_filepath) hrp5nrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath") hrp5nrgtarmlj3_model.instanceTo(hrp5nrgtarmlj3_nodepath) hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'], hrp5nrobot.rgtarm[4]['linkpos']) hrp5nrgtarmlj3_nodepath.setMat(hrp5nrgtarmlj3_rotmat) hrp5nrgtarmlj3_nodepath.setColor(.5, .5, .5, 1) hrp5nrgtarmlj3_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj4_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link3.egg")) hrp5nrgtarmlj4_model = loader.loadModel(hrp5nrgtarmlj4_filepath) hrp5nrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath") hrp5nrgtarmlj4_model.instanceTo(hrp5nrgtarmlj4_nodepath) hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'], hrp5nrobot.rgtarm[5]['linkpos']) hrp5nrgtarmlj4_nodepath.setMat(hrp5nrgtarmlj4_rotmat) hrp5nrgtarmlj4_nodepath.setColor(.5, .5, .5, 1) hrp5nrgtarmlj4_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj5_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link4.egg")) hrp5nrgtarmlj5_model = loader.loadModel(hrp5nrgtarmlj5_filepath) hrp5nrgtarmlj5_nodepath = NodePath("nxtrgtarmlj5_nodepath") hrp5nrgtarmlj5_model.instanceTo(hrp5nrgtarmlj5_nodepath) hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'], hrp5nrobot.rgtarm[6]['linkpos']) hrp5nrgtarmlj5_nodepath.setMat(hrp5nrgtarmlj5_rotmat) hrp5nrgtarmlj5_nodepath.setColor(.5, .5, .5, 1) hrp5nrgtarmlj5_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj6_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link5.egg")) hrp5nrgtarmlj6_model = loader.loadModel(hrp5nrgtarmlj6_filepath) hrp5nrgtarmlj6_nodepath = NodePath("nxtrgtarmlj6_nodepath") hrp5nrgtarmlj6_model.instanceTo(hrp5nrgtarmlj6_nodepath) hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'], hrp5nrobot.rgtarm[7]['linkpos']) hrp5nrgtarmlj6_nodepath.setMat(hrp5nrgtarmlj6_rotmat) hrp5nrgtarmlj6_nodepath.setColor(.5, .5, .5, 1) hrp5nrgtarmlj6_nodepath.reparentTo(hrp5nmnp) # hrp5nrgtarmlj7_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Rarm_Link6.egg")) hrp5nrgtarmlj7_model = loader.loadModel(hrp5nrgtarmlj7_filepath) hrp5nrgtarmlj7_nodepath = NodePath("nxtrgtarmlj7_nodepath") hrp5nrgtarmlj7_model.instanceTo(hrp5nrgtarmlj7_nodepath) hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'], hrp5nrobot.rgtarm[8]['linkpos']) hrp5nrgtarmlj7_nodepath.setMat(hrp5nrgtarmlj7_rotmat) hrp5nrgtarmlj7_nodepath.setColor(.5, .5, .5, 1) hrp5nrgtarmlj7_nodepath.reparentTo(hrp5nmnp) # lftarm hrp5nlftarmlj0_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Lscap_Link0.egg")) hrp5nlftarmlj0_model = loader.loadModel(hrp5nlftarmlj0_filepath) hrp5nlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath") hrp5nlftarmlj0_model.instanceTo(hrp5nlftarmlj0_nodepath) hrp5nlftarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[1]['rotmat'], hrp5nrobot.lftarm[1]['linkpos']) hrp5nlftarmlj0_nodepath.setMat(hrp5nlftarmlj0_rotmat) hrp5nlftarmlj0_nodepath.setColor(.5, .5, .5, 1) hrp5nlftarmlj0_nodepath.reparentTo(hrp5nmnp) # hrp5nlftarmlj1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link0.egg")) hrp5nlftarmlj1_model = loader.loadModel(hrp5nlftarmlj1_filepath) hrp5nlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath") hrp5nlftarmlj1_model.instanceTo(hrp5nlftarmlj1_nodepath) hrp5nlftarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[2]['rotmat'], hrp5nrobot.lftarm[2]['linkpos']) hrp5nlftarmlj1_nodepath.setMat(hrp5nlftarmlj1_rotmat) hrp5nlftarmlj1_nodepath.setColor(.5, .5, .5, 1) hrp5nlftarmlj1_nodepath.reparentTo(hrp5nmnp) # hrp5nlftarmlj2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link1.egg")) hrp5nlftarmlj2_model = loader.loadModel(hrp5nlftarmlj2_filepath) hrp5nlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath") hrp5nlftarmlj2_model.instanceTo(hrp5nlftarmlj2_nodepath) hrp5nlftarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[3]['rotmat'], hrp5nrobot.lftarm[3]['linkpos']) hrp5nlftarmlj2_nodepath.setMat(hrp5nlftarmlj2_rotmat) hrp5nlftarmlj2_nodepath.setColor(.5, .5, .5, 1) hrp5nlftarmlj2_nodepath.reparentTo(hrp5nmnp) # hrp5nlftarmlj3_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link2.egg")) hrp5nlftarmlj3_model = loader.loadModel(hrp5nlftarmlj3_filepath) hrp5nlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath") hrp5nlftarmlj3_model.instanceTo(hrp5nlftarmlj3_nodepath) hrp5nlftarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[4]['rotmat'], hrp5nrobot.lftarm[4]['linkpos']) hrp5nlftarmlj3_nodepath.setMat(hrp5nlftarmlj3_rotmat) hrp5nlftarmlj3_nodepath.setColor(.5, .5, .5, 1) hrp5nlftarmlj3_nodepath.reparentTo(hrp5nmnp) # hrp5nlftarmlj4_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link3.egg")) hrp5nlftarmlj4_model = loader.loadModel(hrp5nlftarmlj4_filepath) hrp5nlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath") hrp5nlftarmlj4_model.instanceTo(hrp5nlftarmlj4_nodepath) hrp5nlftarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[5]['rotmat'], hrp5nrobot.lftarm[5]['linkpos']) hrp5nlftarmlj4_nodepath.setMat(hrp5nlftarmlj4_rotmat) hrp5nlftarmlj4_nodepath.setColor(.5, .5, .5, 1) hrp5nlftarmlj4_nodepath.reparentTo(hrp5nmnp) # hrp5nlftarmlj5_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link4.egg")) hrp5nlftarmlj5_model = loader.loadModel(hrp5nlftarmlj5_filepath) hrp5nlftarmlj5_nodepath = NodePath("nxtlftarmlj5_nodepath") hrp5nlftarmlj5_model.instanceTo(hrp5nlftarmlj5_nodepath) hrp5nlftarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[6]['rotmat'], hrp5nrobot.lftarm[6]['linkpos']) hrp5nlftarmlj5_nodepath.setMat(hrp5nlftarmlj5_rotmat) hrp5nlftarmlj5_nodepath.setColor(.5, .5, .5, 1) hrp5nlftarmlj5_nodepath.reparentTo(hrp5nmnp) # hrp5nlftarmlj6_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link5.egg")) hrp5nlftarmlj6_model = loader.loadModel(hrp5nlftarmlj6_filepath) hrp5nlftarmlj6_nodepath = NodePath("nxtlftarmlj6_nodepath") hrp5nlftarmlj6_model.instanceTo(hrp5nlftarmlj6_nodepath) hrp5nlftarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[7]['rotmat'], hrp5nrobot.lftarm[7]['linkpos']) hrp5nlftarmlj6_nodepath.setMat(hrp5nlftarmlj6_rotmat) hrp5nlftarmlj6_nodepath.setColor(.5, .5, .5, 1) hrp5nlftarmlj6_nodepath.reparentTo(hrp5nmnp) # hrp5nlftarmlj7_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp5negg", "HRP-5P_ConceptDesign_Larm_Link6.egg")) hrp5nlftarmlj7_model = loader.loadModel(hrp5nlftarmlj7_filepath) hrp5nlftarmlj7_nodepath = NodePath("nxtlftarmlj7_nodepath") hrp5nlftarmlj7_model.instanceTo(hrp5nlftarmlj7_nodepath) hrp5nlftarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[8]['rotmat'], hrp5nrobot.lftarm[8]['linkpos']) hrp5nlftarmlj7_nodepath.setMat(hrp5nlftarmlj7_rotmat) hrp5nlftarmlj7_nodepath.setColor(.5, .5, .5, 1) hrp5nlftarmlj7_nodepath.reparentTo(hrp5nmnp) # rgthnd hrp5nrobotrgthnd = handpkg.newHand('rgt') hrp5nrobotrgtarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[9]['rotmat'], hrp5nrobot.rgtarm[9]['linkpos']) # pg.plotAxisSelf(hrp5nmnp, hrp5nrobot.rgtarm[9]['linkend'], hrp5nrobotrgtarmlj9_rotmat) hrp5nrobotrgthnd.setMat(pandanpmat4 = hrp5nrobotrgtarmlj9_rotmat) hrp5nrobotrgthnd.reparentTo(hrp5nmnp) if jawwidthrgt is not None: hrp5nrobotrgthnd.setJawwidth(jawwidthrgt) # lfthnd hrp5nrobotlfthnd = handpkg.newHand('lft') hrp5nrobotlftarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[9]['rotmat'], hrp5nrobot.lftarm[9]['linkpos']) # pg.plotAxisSelf(hrp5nmnp, hrp5nrobot.lftarm[9]['linkend'], hrp5nrobotlftarmlj9_rotmat) hrp5nrobotlfthnd.setMat(pandanpmat4 = hrp5nrobotlftarmlj9_rotmat) hrp5nrobotlfthnd.reparentTo(hrp5nmnp) if jawwidthlft is not None: hrp5nrobotlfthnd.setJawwidth(jawwidthlft) return hrp5nmnp
rbd = Rigidbody(mass=10.0, pos=np.array([0, 0, 0.0]), com=[0.0, 0.0, 0.0], rotmat=rm.rodrigues(np.array([1, 0, 0]), 1.8), inertiatensor=np.array([[1732.0, 0.0, 0.0], [0.0, 1732.0, 0.0], [0.0, 0.0, 3393.0]])) rbd.linearv = np.array([0, 0, 0]) rbd.angularw = np.array([0, 0, 50]) this_dir, this_filename = os.path.split(__file__) model_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "models", "top.egg")) model = loader.loadModel(model_filepath) model.reparentTo(base.render) model.setMat(pg.cvtMat4(rbd.rotmat)) model.setPos(pg.npToV3(rbd.pos)) # base = pc.World(camp=[3000, 0, 3000], lookatp=[0, 0, 0]) # # rbd = Rigidbody(mass = 1.0, pos=np.array([0, 0, 0.0]), rotmat=np.identity(3), # inertiatensor=np.array([[80833.3, 0.0, 0.0], [0.0, 68333.3, 0.0], [0.0, 0.0, 14166.7]])) # rbd.linearv = np.array([0,0,0]) # rbd.angularw = np.array([1, 1, 1]) # # this_dir, this_filename = os.path.split(__file__) # model_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "models", "box.egg")) # model = loader.loadModel(model_filepath) # model.reparentTo(base.render) import csv
def genmnp_nm(self, hrp5nrobot, jawwidthrgt=None, jawwidthlft=None, plotcolor=[.5, .5, .5, .3]): """ generate a panda3d nodepath for the hrp5robot mnp indicates this function generates a mesh nodepath nm indicates the robots include no material since I didn't remove the materials on the hrp5 model, this function is essentially the same as genmnp_nm :param hrp5robot: the Hrp5Robot object, see hrp5.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20170613 """ identmat4 = Mat4.identMat() # body self.__hrp5nbody_nodepath.setMat(identmat4) self.__hrp5nbody_nodepath.setZ(self.__hrp5nmnp, 0) # chest 0 self.__hrp5nchest0_nodepath.setMat(identmat4) self.__hrp5nchest0_nodepath.setZ(self.__hrp5nmnp, 274) # chest 1 self.__hrp5nchest1_nodepath.setMat(identmat4) self.__hrp5nchest1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # chest 2 hrp5nchest2_rotmat = pg.cvtMat4(hrp5nrobot.base['rotmat']) self.__hrp5nchest2_nodepath.setMat(hrp5nchest2_rotmat) self.__hrp5nchest2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # head 0 self.__hrp5nhead0_nodepath.setMat(identmat4) self.__hrp5nhead0_nodepath.setH(hrp5nrobot.initjnts[0]) self.__hrp5nhead0_nodepath.setX(32) self.__hrp5nhead0_nodepath.setZ(521) # head 1 self.__hrp5nhead1_nodepath.setP(hrp5nrobot.initjnts[1]) self.__hrp5nhead1_nodepath.setX(40) self.__hrp5nhead1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 0 hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'], hrp5nrobot.rgtarm[1]['linkpos']) self.__hrp5nrgtarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj0_rotmat) self.__hrp5nrgtarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 1 hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'], hrp5nrobot.rgtarm[2]['linkpos']) self.__hrp5nrgtarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj1_rotmat) self.__hrp5nrgtarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 2 hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'], hrp5nrobot.rgtarm[3]['linkpos']) self.__hrp5nrgtarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj2_rotmat) self.__hrp5nrgtarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 3 hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'], hrp5nrobot.rgtarm[4]['linkpos']) self.__hrp5nrgtarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj3_rotmat) self.__hrp5nrgtarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 4 hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'], hrp5nrobot.rgtarm[5]['linkpos']) self.__hrp5nrgtarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj4_rotmat) self.__hrp5nrgtarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 5 hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'], hrp5nrobot.rgtarm[6]['linkpos']) self.__hrp5nrgtarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj5_rotmat) self.__hrp5nrgtarmlj5_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 6 hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'], hrp5nrobot.rgtarm[7]['linkpos']) self.__hrp5nrgtarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj6_rotmat) self.__hrp5nrgtarmlj6_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 7 hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'], hrp5nrobot.rgtarm[8]['linkpos']) self.__hrp5nrgtarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj7_rotmat) self.__hrp5nrgtarmlj7_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 0 hrp5nlftarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[1]['rotmat'], hrp5nrobot.lftarm[1]['linkpos']) self.__hrp5nlftarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj0_rotmat) self.__hrp5nlftarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 1 hrp5nlftarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[2]['rotmat'], hrp5nrobot.lftarm[2]['linkpos']) self.__hrp5nlftarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj1_rotmat) self.__hrp5nlftarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 2 hrp5nlftarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[3]['rotmat'], hrp5nrobot.lftarm[3]['linkpos']) self.__hrp5nlftarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj2_rotmat) self.__hrp5nlftarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 3 hrp5nlftarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[4]['rotmat'], hrp5nrobot.lftarm[4]['linkpos']) self.__hrp5nlftarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj3_rotmat) self.__hrp5nlftarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 4 hrp5nlftarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[5]['rotmat'], hrp5nrobot.lftarm[5]['linkpos']) self.__hrp5nlftarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj4_rotmat) self.__hrp5nlftarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 5 hrp5nlftarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[6]['rotmat'], hrp5nrobot.lftarm[6]['linkpos']) self.__hrp5nlftarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj5_rotmat) self.__hrp5nlftarmlj5_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 6 hrp5nlftarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[7]['rotmat'], hrp5nrobot.lftarm[7]['linkpos']) self.__hrp5nlftarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj6_rotmat) self.__hrp5nlftarmlj6_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 7 hrp5nlftarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[8]['rotmat'], hrp5nrobot.lftarm[8]['linkpos']) self.__hrp5nlftarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj7_rotmat) self.__hrp5nlftarmlj7_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgthand hrp5nrobotrgtarmlj9_rotmat = pg.cvtMat4( hrp5nrobot.rgtarm[9]['rotmat'], hrp5nrobot.rgtarm[9]['linkpos']) self.hrp5nrobotrgthnd.setMat(self.__hrp5nmnp, hrp5nrobotrgtarmlj9_rotmat) if jawwidthrgt is not None: self.hrp5nrobotrgthnd.setJawwidth(jawwidthrgt) # lfthand hrp5nrobotlftarmlj9_rotmat = pg.cvtMat4( hrp5nrobot.lftarm[9]['rotmat'], hrp5nrobot.lftarm[9]['linkpos']) self.hrp5nrobotlfthnd.setMat(self.__hrp5nmnp, hrp5nrobotlftarmlj9_rotmat) if jawwidthlft is not None: self.hrp5nrobotlfthnd.setJawwidth(jawwidthlft) return copy.deepcopy(self.__hrp5nmnp)
def genmnplist(ur5dual, handpkg, jawwidthrgt=None, jawwidthlft=None): """ generate a list of panda3d nodepath for the ur5dual 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 ur5dual: :return: a list of mesh nodepaths author: weiwei date: 20170608 """ 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 [[ ur5rgtbase_nodepath, ur5rgtshoulder_nodepath, ur5rgtupperarm_nodepath, ur5rgtforearm_nodepath, ur5rgtwrist1_nodepath, ur5rgtwrist2_nodepath, ur5rgtwrist3_nodepath, ur5robotrgthnd.handnp ], [ ur5lftbase_nodepath, ur5lftshoulder_nodepath, ur5lftupperarm_nodepath, ur5lftforearm_nodepath, ur5lftwrist1_nodepath, ur5lftwrist2_nodepath, ur5lftwrist3_nodepath, ur5robotlfthnd.handnp ], []]
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(ur3dual, handpkg, jawwidthrgt = None, jawwidthlft = None): """ generate a panda3d nodepath for the ur3dual mnp indicates this function generates a mesh nodepath :param ur3dual: :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20161202 """ 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 ur3mnp
def genmnp_nm(self,hrp5nrobot, jawwidthrgt = None, jawwidthlft = None, plotcolor=[.5,.5,.5,.3]): """ generate a panda3d nodepath for the hrp5robot mnp indicates this function generates a mesh nodepath nm indicates the robots include no material since I didn't remove the materials on the hrp5 model, this function is essentially the same as genmnp_nm :param hrp5robot: the Hrp5Robot object, see hrp5.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20170613 """ identmat4 = Mat4.identMat() # body self.__hrp5nbody_nodepath.setMat(identmat4) self.__hrp5nbody_nodepath.setZ(self.__hrp5nmnp, 0) # chest 0 self.__hrp5nchest0_nodepath.setMat(identmat4) self.__hrp5nchest0_nodepath.setZ(self.__hrp5nmnp, 274) # chest 1 self.__hrp5nchest1_nodepath.setMat(identmat4) self.__hrp5nchest1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # chest 2 hrp5nchest2_rotmat = pg.cvtMat4(hrp5nrobot.base['rotmat']) self.__hrp5nchest2_nodepath.setMat(hrp5nchest2_rotmat) self.__hrp5nchest2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # head 0 self.__hrp5nhead0_nodepath.setMat(identmat4) self.__hrp5nhead0_nodepath.setH(hrp5nrobot.initjnts[0]) self.__hrp5nhead0_nodepath.setX(32) self.__hrp5nhead0_nodepath.setZ(521) # head 1 self.__hrp5nhead1_nodepath.setP(hrp5nrobot.initjnts[1]) self.__hrp5nhead1_nodepath.setX(40) self.__hrp5nhead1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 0 hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'], hrp5nrobot.rgtarm[1]['linkpos']) self.__hrp5nrgtarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj0_rotmat) self.__hrp5nrgtarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 1 hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'], hrp5nrobot.rgtarm[2]['linkpos']) self.__hrp5nrgtarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj1_rotmat) self.__hrp5nrgtarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 2 hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'], hrp5nrobot.rgtarm[3]['linkpos']) self.__hrp5nrgtarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj2_rotmat) self.__hrp5nrgtarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 3 hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'], hrp5nrobot.rgtarm[4]['linkpos']) self.__hrp5nrgtarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj3_rotmat) self.__hrp5nrgtarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 4 hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'], hrp5nrobot.rgtarm[5]['linkpos']) self.__hrp5nrgtarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj4_rotmat) self.__hrp5nrgtarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 5 hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'], hrp5nrobot.rgtarm[6]['linkpos']) self.__hrp5nrgtarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj5_rotmat) self.__hrp5nrgtarmlj5_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 6 hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'], hrp5nrobot.rgtarm[7]['linkpos']) self.__hrp5nrgtarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj6_rotmat) self.__hrp5nrgtarmlj6_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgtarm 7 hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'], hrp5nrobot.rgtarm[8]['linkpos']) self.__hrp5nrgtarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj7_rotmat) self.__hrp5nrgtarmlj7_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 0 hrp5nlftarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[1]['rotmat'], hrp5nrobot.lftarm[1]['linkpos']) self.__hrp5nlftarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj0_rotmat) self.__hrp5nlftarmlj0_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 1 hrp5nlftarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[2]['rotmat'], hrp5nrobot.lftarm[2]['linkpos']) self.__hrp5nlftarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj1_rotmat) self.__hrp5nlftarmlj1_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 2 hrp5nlftarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[3]['rotmat'], hrp5nrobot.lftarm[3]['linkpos']) self.__hrp5nlftarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj2_rotmat) self.__hrp5nlftarmlj2_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 3 hrp5nlftarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[4]['rotmat'], hrp5nrobot.lftarm[4]['linkpos']) self.__hrp5nlftarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj3_rotmat) self.__hrp5nlftarmlj3_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 4 hrp5nlftarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[5]['rotmat'], hrp5nrobot.lftarm[5]['linkpos']) self.__hrp5nlftarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj4_rotmat) self.__hrp5nlftarmlj4_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 5 hrp5nlftarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[6]['rotmat'], hrp5nrobot.lftarm[6]['linkpos']) self.__hrp5nlftarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj5_rotmat) self.__hrp5nlftarmlj5_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 6 hrp5nlftarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[7]['rotmat'], hrp5nrobot.lftarm[7]['linkpos']) self.__hrp5nlftarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj6_rotmat) self.__hrp5nlftarmlj6_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # lftarm 7 hrp5nlftarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[8]['rotmat'], hrp5nrobot.lftarm[8]['linkpos']) self.__hrp5nlftarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj7_rotmat) self.__hrp5nlftarmlj7_nodepath.setColor(plotcolor[0], plotcolor[1], plotcolor[2], plotcolor[3]) # rgthand hrp5nrobotrgtarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[9]['rotmat'], hrp5nrobot.rgtarm[9]['linkpos']) self.hrp5nrobotrgthnd.setMat(self.__hrp5nmnp, hrp5nrobotrgtarmlj9_rotmat) if jawwidthrgt is not None: self.hrp5nrobotrgthnd.setJawwidth(jawwidthrgt) # lfthand hrp5nrobotlftarmlj9_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[9]['rotmat'], hrp5nrobot.lftarm[9]['linkpos']) self.hrp5nrobotlfthnd.setMat(self.__hrp5nmnp, hrp5nrobotlftarmlj9_rotmat) if jawwidthlft is not None: self.hrp5nrobotlfthnd.setJawwidth(jawwidthlft) return copy.deepcopy(self.__hrp5nmnp)
def genmnp(self, robot, jawwidthrgt=0, jawwidthlft=0, toggleendcoord=False, togglejntscoord=False, name='robotmesh'): """ generate a panda3d nodepath for the hrp5robot mnp indicates this function generates a mesh nodepath :param robot: the Hrp5Robot object, see hrp5.py :param toggleendcoord: whether to plot end coordinate systems :param togglejntscoord: whether to plot joints coordinate systems :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20180203 """ robotmesh_nodepath = NodePath(name) # body hrp5pbody_rotmat = pg.cvtMat4(robot.rgtarm[0]['rotmat'], robot.rgtarm[0]['linkpos']) self.__hrp5pbody_nodepath.reparentTo(robotmesh_nodepath) #ADDED # self.__hrp5pchest0_nodepath.setMat(hrp5pbody_rotmat) # self.__hrp5pchest1_nodepath.setMat(hrp5pbody_rotmat) # ADDED self.__hrp5pchest2_nodepath.setMat(hrp5pbody_rotmat) # self.__hrp5phead0_nodepath.setMat(hrp5pbody_rotmat) # self.__hrp5phead1_nodepath.setMat(hrp5pbody_rotmat) self.__hrp5pbody_nodepath.setColor(0.2, 0.2, 0.2, 1) # rgtarm 0 hrp5prgtarmlj0_rotmat = pg.cvtMat4(robot.rgtarm[1]['rotmat'], robot.rgtarm[1]['linkpos']) self.__hrp5prgtarmlj0_nodepath.setMat(hrp5prgtarmlj0_rotmat) self.__hrp5prgtarmlj0_nodepath.setColor(0.2, 0.2, 0.2, 1) # rgtarm 1 hrp5prgtarmlj1_rotmat = pg.cvtMat4(robot.rgtarm[2]['rotmat'], robot.rgtarm[2]['linkpos']) self.__hrp5prgtarmlj1_nodepath.setMat(hrp5prgtarmlj1_rotmat) self.__hrp5prgtarmlj1_nodepath.setColor(.5, .5, .5, 1) # rgtarm 2 hrp5prgtarmlj2_rotmat = pg.cvtMat4(robot.rgtarm[3]['rotmat'], robot.rgtarm[3]['linkpos']) self.__hrp5prgtarmlj2_nodepath.setMat(hrp5prgtarmlj2_rotmat) self.__hrp5prgtarmlj2_nodepath.setColor(.5, .5, .5, 1) # rgtarm 3 hrp5prgtarmlj3_rotmat = pg.cvtMat4(robot.rgtarm[4]['rotmat'], robot.rgtarm[4]['linkpos']) self.__hrp5prgtarmlj3_nodepath.setMat(hrp5prgtarmlj3_rotmat) self.__hrp5prgtarmlj3_nodepath.setColor(.5, .5, .5, 1) # rgtarm 4 hrp5prgtarmlj4_rotmat = pg.cvtMat4(robot.rgtarm[5]['rotmat'], robot.rgtarm[5]['linkpos']) self.__hrp5prgtarmlj4_nodepath.setMat(hrp5prgtarmlj4_rotmat) self.__hrp5prgtarmlj4_nodepath.setColor(.5, .5, .5, 1) # rgtarm 5 hrp5prgtarmlj5_rotmat = pg.cvtMat4(robot.rgtarm[6]['rotmat'], robot.rgtarm[6]['linkpos']) self.__hrp5prgtarmlj5_nodepath.setMat(hrp5prgtarmlj5_rotmat) self.__hrp5prgtarmlj5_nodepath.setColor(.5, .5, .5, 1) # rgtarm 6 hrp5prgtarmlj6_rotmat = pg.cvtMat4(robot.rgtarm[7]['rotmat'], robot.rgtarm[7]['linkpos']) self.__hrp5prgtarmlj6_nodepath.setMat(hrp5prgtarmlj6_rotmat) self.__hrp5prgtarmlj6_nodepath.setColor(.5, .5, .5, 1) # rgtarm 7 hrp5prgtarmlj7_rotmat = pg.cvtMat4(robot.rgtarm[8]['rotmat'], robot.rgtarm[8]['linkpos']) self.__hrp5prgtarmlj7_nodepath.setMat(hrp5prgtarmlj7_rotmat) self.__hrp5prgtarmlj7_nodepath.setColor(.5, .5, .5, 1) self.__hrp5prgtarmlj0_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5prgtarmlj1_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5prgtarmlj2_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5prgtarmlj3_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5prgtarmlj4_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5prgtarmlj5_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5prgtarmlj6_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5prgtarmlj7_nodepath.reparentTo(robotmesh_nodepath) # lftarm 0 hrp5plftarmlj0_rotmat = pg.cvtMat4(robot.lftarm[1]['rotmat'], robot.lftarm[1]['linkpos']) self.__hrp5plftarmlj0_nodepath.setMat(hrp5plftarmlj0_rotmat) self.__hrp5plftarmlj0_nodepath.setColor(0.2, 0.2, 0.2, 1) # lftarm 1 hrp5plftarmlj1_rotmat = pg.cvtMat4(robot.lftarm[2]['rotmat'], robot.lftarm[2]['linkpos']) self.__hrp5plftarmlj1_nodepath.setMat(hrp5plftarmlj1_rotmat) self.__hrp5plftarmlj1_nodepath.setColor(.5, .5, .5, 1) # lftarm 2 hrp5plftarmlj2_rotmat = pg.cvtMat4(robot.lftarm[3]['rotmat'], robot.lftarm[3]['linkpos']) self.__hrp5plftarmlj2_nodepath.setMat(hrp5plftarmlj2_rotmat) self.__hrp5plftarmlj2_nodepath.setColor(.5, .5, .5, 1) # lftarm 3 hrp5plftarmlj3_rotmat = pg.cvtMat4(robot.lftarm[4]['rotmat'], robot.lftarm[4]['linkpos']) self.__hrp5plftarmlj3_nodepath.setMat(hrp5plftarmlj3_rotmat) self.__hrp5plftarmlj3_nodepath.setColor(.5, .5, .5, 1) # lftarm 4 hrp5plftarmlj4_rotmat = pg.cvtMat4(robot.lftarm[5]['rotmat'], robot.lftarm[5]['linkpos']) self.__hrp5plftarmlj4_nodepath.setMat(hrp5plftarmlj4_rotmat) self.__hrp5plftarmlj4_nodepath.setColor(.5, .5, .5, 1) # lftarm 5 hrp5plftarmlj5_rotmat = pg.cvtMat4(robot.lftarm[6]['rotmat'], robot.lftarm[6]['linkpos']) self.__hrp5plftarmlj5_nodepath.setMat(hrp5plftarmlj5_rotmat) self.__hrp5plftarmlj5_nodepath.setColor(.5, .5, .5, 1) # lftarm 6 hrp5plftarmlj6_rotmat = pg.cvtMat4(robot.lftarm[7]['rotmat'], robot.lftarm[7]['linkpos']) self.__hrp5plftarmlj6_nodepath.setMat(hrp5plftarmlj6_rotmat) self.__hrp5plftarmlj6_nodepath.setColor(.5, .5, .5, 1) # lftarm 7 hrp5plftarmlj7_rotmat = pg.cvtMat4(robot.lftarm[8]['rotmat'], robot.lftarm[8]['linkpos']) self.__hrp5plftarmlj7_nodepath.setMat(hrp5plftarmlj7_rotmat) self.__hrp5plftarmlj7_nodepath.setColor(.5, .5, .5, 1) self.__hrp5plftarmlj0_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5plftarmlj1_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5plftarmlj2_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5plftarmlj3_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5plftarmlj4_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5plftarmlj5_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5plftarmlj6_nodepath.reparentTo(robotmesh_nodepath) self.__hrp5plftarmlj7_nodepath.reparentTo(robotmesh_nodepath) # endcoord if toggleendcoord: self.pggen.plotAxis(robotmesh_nodepath, pandamat4=pg.cvtMat4( robot.rgtarm[-1]['rotmat'], robot.rgtarm[-1]['linkend'])) self.pggen.plotAxis(robotmesh_nodepath, pandamat4=pg.cvtMat4( robot.lftarm[-1]['rotmat'], robot.lftarm[-1]['linkend'])) # toggle all coord if togglejntscoord: for i in range(1, 8): self.pggen.plotAxis(robotmesh_nodepath, pandamat4=pg.cvtMat4( robot.rgtarm[i - 1]['rotmat'], robot.rgtarm[i]['linkpos'])) self.pggen.plotAxis(robotmesh_nodepath, pandamat4=pg.cvtMat4( robot.lftarm[i - 1]['rotmat'], robot.lftarm[i]['linkpos'])) if self.rgthnd is not None: self.rgthnd.setMat( pg.cvtMat4(robot.rgtarm[-1]['rotmat'], robot.rgtarm[-1]['linkpos'])) self.rgthnd.setJawwidth(jawwidthrgt) self.rgthnd.reparentTo(robotmesh_nodepath) if self.lfthnd is not None: self.lfthnd.setMat( pg.cvtMat4(robot.lftarm[-1]['rotmat'], robot.lftarm[-1]['linkpos'])) self.lfthnd.setJawwidth(jawwidthlft) self.lfthnd.reparentTo(robotmesh_nodepath) robotlegmesh_nodepath = self.genlegmnp(robot, toggleendcoord=toggleendcoord, togglejntscoord=togglejntscoord) robotlegmesh_nodepath.reparentTo(robotmesh_nodepath) return copy.deepcopy(robotmesh_nodepath)
def removeHndcc(self, base, discretesize=8): """ Handcc means hand collision detection :param discretesize: the number of hand orientations :return: author: weiwei date: 20161212, tsukuba """ # isplotted = 0 # if self.rtq85plotlist: # for rtq85plotnode in self.rtq85plotlist: # rtq85plotnode.removeNode() # self.rtq85plotlist = [] self.gripcontacts = [] self.griprotmats = [] self.gripjawwidth = [] self.gripcontactnormals = [] plotoffsetfp = 6 self.counter = 0 while self.counter < self.facetpairs.shape[0]: print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1) # print self.gripcontactpairs_precc facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]): for angleid in range(discretesize): cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1] cctnormal0 = self.gripcontactpairnormals_precc[self.counter][j][0] cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]] tmphand = self.hand # tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) # save initial hand pose initmat = tmphand.getMat() fgrdist = np.linalg.norm((cctpnt0 - cctpnt1)) tmphand.setJawwidth(fgrdist) tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2]) rotax = [0, 1, 0] rotangle = 360.0 / discretesize * angleid rotmat = rm.rodrigues(rotax, rotangle) tmphand.setMat(pandageom.cvtMat4(rotmat) * tmphand.getMat()) axx = tmphand.getMat().getRow3(0) # 130 is the distance from hndbase to fingertip cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]]) tmphand.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2])) # collision detection hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp, base.render) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): self.gripcontacts.append(contactpair) self.griprotmats.append(tmphand.getMat()) self.gripjawwidth.append(fgrdist) self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j]) # pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1]) # pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]), # tmprtq85.getMat(), length=30, thickness=2) # tmprtq85.setColor([.5, .5, .5, 1]) # tmprtq85.reparentTo(base.render) # self.rtq85plotlist.append(tmprtq85) # isplotted = 1 # reset initial hand pose tmphand.setMat(initmat) self.counter+=1 self.counter = 0
def genlegmnp(self, robot, toggleendcoord=True, togglejntscoord=False, name='legmnp'): """ this function is additional to genmnp it is an additional function to include legs :param robot: :return: leg mesh nodepath author: weiwei date: 20180203 """ robotlegmesh_nodepath = NodePath(name) # rgtleg 0 hrp5prgtleglj0_rotmat = pg.cvtMat4(robot.rgtleg[1]['rotmat'], robot.rgtleg[1]['linkpos']) self.__hrp5prgtleglj0_nodepath.setMat(hrp5prgtleglj0_rotmat) self.__hrp5prgtleglj0_nodepath.setColor(0.2, 0.2, 0.2, 1) # rgtleg 1 hrp5prgtleglj1_rotmat = pg.cvtMat4(robot.rgtleg[2]['rotmat'], robot.rgtleg[2]['linkpos']) self.__hrp5prgtleglj1_nodepath.setMat(hrp5prgtleglj1_rotmat) self.__hrp5prgtleglj1_nodepath.setColor(.5, .5, .5, 1) # rgtleg 2 hrp5prgtleglj2_rotmat = pg.cvtMat4(robot.rgtleg[3]['rotmat'], robot.rgtleg[3]['linkpos']) self.__hrp5prgtleglj2_nodepath.setMat(hrp5prgtleglj2_rotmat) self.__hrp5prgtleglj2_nodepath.setColor(.5, .5, .5, 1) # rgtleg 3 hrp5prgtleglj3_rotmat = pg.cvtMat4(robot.rgtleg[4]['rotmat'], robot.rgtleg[4]['linkpos']) self.__hrp5prgtleglj3_nodepath.setMat(hrp5prgtleglj3_rotmat) self.__hrp5prgtleglj3_nodepath.setColor(.5, .5, .5, 1) # rgtleg 4 hrp5prgtleglj4_rotmat = pg.cvtMat4(robot.rgtleg[5]['rotmat'], robot.rgtleg[5]['linkpos']) self.__hrp5prgtleglj4_nodepath.setMat(hrp5prgtleglj4_rotmat) self.__hrp5prgtleglj4_nodepath.setColor(.5, .5, .5, 1) # rgtleg 5 hrp5prgtleglj5_rotmat = pg.cvtMat4(robot.rgtleg[6]['rotmat'], robot.rgtleg[6]['linkpos']) self.__hrp5prgtleglj5_nodepath.setMat(hrp5prgtleglj5_rotmat) self.__hrp5prgtleglj5_nodepath.setColor(.5, .5, .5, 1) self.__hrp5prgtleglj0_nodepath.reparentTo(robotlegmesh_nodepath) self.__hrp5prgtleglj1_nodepath.reparentTo(robotlegmesh_nodepath) self.__hrp5prgtleglj2_nodepath.reparentTo(robotlegmesh_nodepath) self.__hrp5prgtleglj3_nodepath.reparentTo(robotlegmesh_nodepath) self.__hrp5prgtleglj4_nodepath.reparentTo(robotlegmesh_nodepath) self.__hrp5prgtleglj5_nodepath.reparentTo(robotlegmesh_nodepath) # lftleg 0 hrp5plftleglj0_rotmat = pg.cvtMat4(robot.lftleg[1]['rotmat'], robot.lftleg[1]['linkpos']) self.__hrp5plftleglj0_nodepath.setMat(hrp5plftleglj0_rotmat) self.__hrp5plftleglj0_nodepath.setColor(0.2, 0.2, 0.2, 1) # lftleg 1 hrp5plftleglj1_rotmat = pg.cvtMat4(robot.lftleg[2]['rotmat'], robot.lftleg[2]['linkpos']) self.__hrp5plftleglj1_nodepath.setMat(hrp5plftleglj1_rotmat) self.__hrp5plftleglj1_nodepath.setColor(.5, .5, .5, 1) # lftleg 2 hrp5plftleglj2_rotmat = pg.cvtMat4(robot.lftleg[3]['rotmat'], robot.lftleg[3]['linkpos']) self.__hrp5plftleglj2_nodepath.setMat(hrp5plftleglj2_rotmat) self.__hrp5plftleglj2_nodepath.setColor(.5, .5, .5, 1) # lftleg 3 hrp5plftleglj3_rotmat = pg.cvtMat4(robot.lftleg[4]['rotmat'], robot.lftleg[4]['linkpos']) self.__hrp5plftleglj3_nodepath.setMat(hrp5plftleglj3_rotmat) self.__hrp5plftleglj3_nodepath.setColor(.5, .5, .5, 1) # lftleg 4 hrp5plftleglj4_rotmat = pg.cvtMat4(robot.lftleg[5]['rotmat'], robot.lftleg[5]['linkpos']) self.__hrp5plftleglj4_nodepath.setMat(hrp5plftleglj4_rotmat) self.__hrp5plftleglj4_nodepath.setColor(.5, .5, .5, 1) # lftleg 5 hrp5plftleglj5_rotmat = pg.cvtMat4(robot.lftleg[6]['rotmat'], robot.lftleg[6]['linkpos']) self.__hrp5plftleglj5_nodepath.setMat(hrp5plftleglj5_rotmat) self.__hrp5plftleglj5_nodepath.setColor(.5, .5, .5, 1) self.__hrp5plftleglj0_nodepath.reparentTo(robotlegmesh_nodepath) self.__hrp5plftleglj1_nodepath.reparentTo(robotlegmesh_nodepath) self.__hrp5plftleglj2_nodepath.reparentTo(robotlegmesh_nodepath) self.__hrp5plftleglj3_nodepath.reparentTo(robotlegmesh_nodepath) self.__hrp5plftleglj4_nodepath.reparentTo(robotlegmesh_nodepath) self.__hrp5plftleglj5_nodepath.reparentTo(robotlegmesh_nodepath) # endcoord if toggleendcoord: self.pggen.plotAxis(robotlegmesh_nodepath, pandamat4=pg.cvtMat4( robot.rgtleg[-1]['rotmat'], robot.rgtleg[-1]['linkend'])) self.pggen.plotAxis(robotlegmesh_nodepath, pandamat4=pg.cvtMat4( robot.lftleg[-1]['rotmat'], robot.lftleg[-1]['linkend'])) # toggle all coord if togglejntscoord: for i in range(1, 6): self.pggen.plotAxis(robotlegmesh_nodepath, pandamat4=pg.cvtMat4( robot.rgtleg[i - 1]['rotmat'], robot.rgtleg[i]['linkpos'])) self.pggen.plotAxis(robotlegmesh_nodepath, pandamat4=pg.cvtMat4( robot.lftleg[i - 1]['rotmat'], robot.lftleg[i]['linkpos'])) return robotlegmesh_nodepath
def saveToDB(self, positionlist, gdb, discretesize=4): """ :param positionlist: a list of positions to place the object one the table :param discretesize: the discretization of rotation angles around z axis :return: author: weiwei date: 20161215, osaka """ # save discretiezed angle sql = "SELECT * FROM angle" result = gdb.execute(sql) if len(result) == 0: sql = "INSERT INTO angle(value) VALUES " for i in range(discretesize): sql += "(" + str(360 * i * 1.0 / discretesize) + "), " sql = sql[:-2] + ";" gdb.execute(sql) else: print "Angles already set!" # save tabletopplacements sql = "SELECT idstartgoal FROM startgoal,freetabletopplacement,object WHERE \ startgoal.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \ freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: # 1) select the freetabletopplacement sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \ FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \ AND object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: raise ValueError("Plan the freetabletopplacement first!") result = np.asarray(result) idfreelist = [int(x) for x in result[:, 0]] rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]] # 2) select the angle sql = "SELECT angle.idangle,angle.value FROM angle" result = np.asarray(gdb.execute(sql)) idanglelist = [int(x) for x in result[:, 0]] anglevaluelist = [float(x) for x in result[:, 1]] # 3) save to database sql = "INSERT INTO startgoal(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES " for ttoppos in positionlist: ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2]) for idfree, rotmatfree in zip(idfreelist, rotmatfreelist): for idangle, anglevalue in zip(idanglelist, anglevaluelist): rotangle = anglevalue rotmat = rm.rodrigues([0, 0, 1], rotangle) rotmat4 = pg.cvtMat4(rotmat, ttoppos) varrotmat = rotmatfree * rotmat4 sql += "('%s', '%s', %d, %d), " % \ (dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree) sql = sql[:-2] + ";" gdb.execute(sql) else: print "startgoal already exist!" print "Save to DB done!"
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
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)
# hrp5robot.movearmfkr(armjntsgoal, armid) # hrp5plot.plotstick(base.render, hrp5robot) # 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])
def removeHndccShow(self, base, discretesize=8): """ Handcc means hand collision detection This one is developed for demonstration This function should be called after executing removeHndcc :param discretesize: the number of hand orientations :return: delayTime author: weiwei date: 20161212, tsukuba """ # isplotted = 0 if self.rtq85plotlist: for rtq85plotnode in self.rtq85plotlist: rtq85plotnode.removeNode() self.rtq85plotlist = [] self.gripcontacts = [] self.griprotmats = [] self.gripjawwidth = [] self.gripcontactnormals = [] plotoffsetfp = 6 if self.counter2 == 0: self.counter += 1 if self.counter >= self.facetpairs.shape[0]: self.counter = 0 self.counter2 += 1 if self.counter2 >= discretesize: self.counter2 = 0 print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1) facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] for j, contactpair in enumerate( self.gripcontactpairs_precc[self.counter]): if j == 0: print j, contactpair # for angleid in range(discretesize): angleid = self.counter2 cctpnt0 = contactpair[ 0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[ 1] + plotoffsetfp * self.facetnormals[facetidx1] cctnormal0 = self.gripcontactpairnormals_precc[ self.counter][j][0] cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]] tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) # save initial hand pose fgrdist = np.linalg.norm((cctpnt0 - cctpnt1)) # TODO setting jawwidth to 80 is enough # since fgrpcc already detects inner collisions tmprtq85.setJawwidth(fgrdist) tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2]) rotax = [0, 1, 0] rotangle = 360.0 / discretesize * angleid rotmat = rm.rodrigues(rotax, rotangle) tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat()) axx = tmprtq85.getMat().getRow3(0) # 130 is the distance from hndbase to fingertip cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array( [axx[0], axx[1], axx[2]]) tmprtq85.setPos( Point3(cctcenter[0], cctcenter[1], cctcenter[2])) # collision detection self.hndbullnode = cd.genCollisionMeshMultiNp( tmprtq85.rtq85np, base.render) result = self.bulletworld.contactTest(self.hndbullnode) if not result.getNumContacts(): self.gripcontacts.append(contactpair) self.griprotmats.append(tmprtq85.getMat()) self.gripjawwidth.append(fgrdist) self.gripcontactnormals.append( self.gripcontactpairnormals_precc[self.counter][j]) # pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1]) # pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]), # tmprtq85.getMat(), length=30, thickness=2) tmprtq85.setColor([1, 1, 1, .3]) tmprtq85.reparentTo(base.render) self.rtq85plotlist.append(tmprtq85) # isplotted = 1 else: for contact in result.getContacts(): # cp = contact.getManifoldPoint() # pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1)) # pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1)) tmprtq85.setColor([.5, 0, 0, .3]) tmprtq85.reparentTo(base.render) self.rtq85plotlist.append(tmprtq85)
def genHrp5mnp(hrp5robot, jawwidthrgt = None, jawwidthlft = None): """ generate a panda3d nodepath for the hrp5robot mnp indicates this function generates a mesh nodepath :param hrp5robot: the Hrp5Robot object, see hrp5.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20161202 """ hrp5mnp = NodePath("hrp5mnp") this_dir, this_filename = os.path.split(__file__) # chest0-2, head1 (neck is not plotted) hrp5chest0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_chest0.egg")) hrp5chest1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_chest1.egg")) hrp5chest2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_chest2.egg")) hrp5head1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_head1.egg")) hrp5chest0_model = loader.loadModel(hrp5chest0_filepath) hrp5chest1_model = loader.loadModel(hrp5chest1_filepath) hrp5chest2_model = loader.loadModel(hrp5chest2_filepath) hrp5head1_model = loader.loadModel(hrp5head1_filepath) hrp5chest0_nodepath = NodePath("hrp5chest0") hrp5chest1_nodepath = NodePath("hrp5chest1") hrp5chest2_nodepath = NodePath("hrp5chest2") hrp5head1_nodepath = NodePath("hrp5head1") hrp5chest0_model.instanceTo(hrp5chest0_nodepath) hrp5chest0_rotmat = pg.cvtMat4(hrp5robot.base['rotmat']) hrp5chest0_nodepath.setMat(hrp5chest0_rotmat) hrp5chest0_nodepath.setZ(223) hrp5chest1_model.instanceTo(hrp5chest1_nodepath) hrp5chest1_nodepath.setColor(.7,.7,.2,1) hrp5chest2_model.instanceTo(hrp5chest2_nodepath) hrp5chest2_nodepath.setX(-68) hrp5chest2_nodepath.setColor(.7,.7,.7,1) hrp5head1_model.instanceTo(hrp5head1_nodepath) hrp5head1_nodepath.setH(hrp5robot.initjnts[0]) hrp5head1_nodepath.setP(hrp5robot.initjnts[1]) hrp5head1_nodepath.setX(43) hrp5head1_nodepath.setZ(440) hrp5head1_nodepath.setColor(.5,.5,.5,1) hrp5chest0_nodepath.reparentTo(hrp5mnp) hrp5chest1_nodepath.reparentTo(hrp5chest0_nodepath) hrp5chest2_nodepath.reparentTo(hrp5chest0_nodepath) hrp5head1_nodepath.reparentTo(hrp5chest0_nodepath) # rgtarm hrp5rgtarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rscap.egg")) hrp5rgtarmlj0_model = loader.loadModel(hrp5rgtarmlj0_filepath) hrp5rgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath") hrp5rgtarmlj0_model.instanceTo(hrp5rgtarmlj0_nodepath) hrp5rgtarmlj0_rotmat = pg.cvtMat4(hrp5robot.rgtarm[1]['rotmat'], hrp5robot.rgtarm[1]['linkpos']) hrp5rgtarmlj0_nodepath.setMat(hrp5rgtarmlj0_rotmat) hrp5rgtarmlj0_nodepath.setColor(.5,.5,.5,1) hrp5rgtarmlj0_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink0.egg")) hrp5rgtarmlj1_model = loader.loadModel(hrp5rgtarmlj1_filepath) hrp5rgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath") hrp5rgtarmlj1_model.instanceTo(hrp5rgtarmlj1_nodepath) hrp5rgtarmlj1_rotmat = pg.cvtMat4(hrp5robot.rgtarm[2]['rotmat'], hrp5robot.rgtarm[2]['linkpos']) hrp5rgtarmlj1_nodepath.setMat(hrp5rgtarmlj1_rotmat) hrp5rgtarmlj1_nodepath.setColor(.5,.5,.5,1) hrp5rgtarmlj1_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink1.egg")) hrp5rgtarmlj2_model = loader.loadModel(hrp5rgtarmlj2_filepath) hrp5rgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath") hrp5rgtarmlj2_model.instanceTo(hrp5rgtarmlj2_nodepath) hrp5rgtarmlj2_rotmat = pg.cvtMat4(hrp5robot.rgtarm[3]['rotmat'], hrp5robot.rgtarm[3]['linkpos']) hrp5rgtarmlj2_nodepath.setMat(hrp5rgtarmlj2_rotmat) hrp5rgtarmlj2_nodepath.setColor(.5,.5,.5,1) hrp5rgtarmlj2_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink2.egg")) hrp5rgtarmlj3_model = loader.loadModel(hrp5rgtarmlj3_filepath) hrp5rgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath") hrp5rgtarmlj3_model.instanceTo(hrp5rgtarmlj3_nodepath) hrp5rgtarmlj3_rotmat = pg.cvtMat4(hrp5robot.rgtarm[4]['rotmat'], hrp5robot.rgtarm[4]['linkpos']) hrp5rgtarmlj3_nodepath.setMat(hrp5rgtarmlj3_rotmat) hrp5rgtarmlj3_nodepath.setColor(.5,.5,.5,1) hrp5rgtarmlj3_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink3.egg")) hrp5rgtarmlj4_model = loader.loadModel(hrp5rgtarmlj4_filepath) hrp5rgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath") hrp5rgtarmlj4_model.instanceTo(hrp5rgtarmlj4_nodepath) hrp5rgtarmlj4_rotmat = pg.cvtMat4(hrp5robot.rgtarm[5]['rotmat'], hrp5robot.rgtarm[5]['linkpos']) hrp5rgtarmlj4_nodepath.setMat(hrp5rgtarmlj4_rotmat) hrp5rgtarmlj4_nodepath.setColor(.5,.5,.5,1) hrp5rgtarmlj4_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj5_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink4.egg")) hrp5rgtarmlj5_model = loader.loadModel(hrp5rgtarmlj5_filepath) hrp5rgtarmlj5_nodepath = NodePath("nxtrgtarmlj5_nodepath") hrp5rgtarmlj5_model.instanceTo(hrp5rgtarmlj5_nodepath) hrp5rgtarmlj5_rotmat = pg.cvtMat4(hrp5robot.rgtarm[6]['rotmat'], hrp5robot.rgtarm[6]['linkpos']) hrp5rgtarmlj5_nodepath.setMat(hrp5rgtarmlj5_rotmat) hrp5rgtarmlj5_nodepath.setColor(.5,.5,.5,1) hrp5rgtarmlj5_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj6_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink5.egg")) hrp5rgtarmlj6_model = loader.loadModel(hrp5rgtarmlj6_filepath) hrp5rgtarmlj6_nodepath = NodePath("nxtrgtarmlj6_nodepath") hrp5rgtarmlj6_model.instanceTo(hrp5rgtarmlj6_nodepath) hrp5rgtarmlj6_rotmat = pg.cvtMat4(hrp5robot.rgtarm[7]['rotmat'], hrp5robot.rgtarm[7]['linkpos']) hrp5rgtarmlj6_nodepath.setMat(hrp5rgtarmlj6_rotmat) hrp5rgtarmlj6_nodepath.setColor(.5,.5,.5,1) hrp5rgtarmlj6_nodepath.reparentTo(hrp5mnp) # hrp5rgtarmlj7_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp5egg", "hrp5_rlink6.egg")) hrp5rgtarmlj7_model = loader.loadModel(hrp5rgtarmlj7_filepath) hrp5rgtarmlj7_nodepath = NodePath("nxtrgtarmlj7_nodepath") hrp5rgtarmlj7_model.instanceTo(hrp5rgtarmlj7_nodepath) hrp5rgtarmlj7_rotmat = pg.cvtMat4(hrp5robot.rgtarm[8]['rotmat'], hrp5robot.rgtarm[8]['linkpos']) hrp5rgtarmlj7_nodepath.setMat(hrp5rgtarmlj7_rotmat) hrp5rgtarmlj7_nodepath.setColor(.5,.5,.5,1) hrp5rgtarmlj7_nodepath.reparentTo(hrp5mnp) # # # lftarm # nxtlftarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj0.egg")) # nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath) # nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath") # nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath) # nxtlftarmlj0_rotmat = pg.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos']) # nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat) # nxtlftarmlj0_nodepath.reparentTo(nxtmnp) # # nxtlftarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj1.egg")) # nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath) # nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath") # nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath) # nxtlftarmlj1_rotmat = pg.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos']) # nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat) # nxtlftarmlj1_nodepath.reparentTo(nxtmnp) # # nxtlftarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj2.egg")) # nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath) # nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath") # nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath) # nxtlftarmlj2_rotmat = pg.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos']) # nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat) # nxtlftarmlj2_nodepath.reparentTo(nxtmnp) # # nxtlftarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj3.egg")) # nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath) # nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath") # nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath) # nxtlftarmlj3_rotmat = pg.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos']) # nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat) # nxtlftarmlj3_nodepath.reparentTo(nxtmnp) # # nxtlftarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj4.egg")) # nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath) # nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath") # nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath) # nxtlftarmlj4_rotmat = pg.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos']) # nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat) # nxtlftarmlj4_nodepath.reparentTo(nxtmnp) # rgthnd hrp5robotrgthnd = rtq85.Rtq85() hrp5robotrgtarmlj9_rotmat = pg.cvtMat4(hrp5robot.rgtarm[9]['rotmat'], hrp5robot.rgtarm[9]['linkpos']) # pg.plotAxisSelf(hrp5mnp, hrp5robot.rgtarm[9]['linkend'], hrp5robotrgtarmlj9_rotmat) hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat) hrp5robotrgthnd.reparentTo(hrp5mnp) if jawwidthrgt is not None: hrp5robotrgthnd.setJawwidth(jawwidthrgt) # lfthnd # hrp5robotlfthnd = rtq85.Rtq85() # hrp5robotlftarmlj9_rotmat = pg.cvtMat4(hrp5robot.lftarm[9]['rotmat'], hrp5robot.lftarm[9]['linkpos']) # pg.plotAxisSelf(hrp5mnp, hrp5robot.lftarm[9]['linkend'], hrp5robotlftarmlj9_rotmat) # hrp5robotlfthnd.setMat(hrp5robotlftarmlj9_rotmat) # hrp5robotlfthnd.reparentTo(hrp5mnp) # if jawwidthlft is not None: # hrp5robotlfthnd.setJawwidth(jawwidthlft) return hrp5mnp
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)
def genmnp_nm(nxtrobot, handpkg, plotcolor=[.5,.5,.5,.3], jawwidthrgt = None, jawwidthlft = None): """ generate a panda3d nodepath for the nxtrobot mnp indicates this function generates a mesh nodepath nm means no material :param nxtrobot: the NxtRobot object, see nextage.py :param plotcolor: the color of the model, alpha allowed :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20161109 """ nxtmnp = NodePath("nxtmnp") this_dir, this_filename = os.path.split(__file__) # mainbody, waist, body, head (neck is not plotted) nxtwaist_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_waist_nm.egg")) nxtbody_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_body_nm.egg")) nxthead_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_head_nm.egg")) nxtwaist_model = loader.loadModel(nxtwaist_filepath) nxtbody_model = loader.loadModel(nxtbody_filepath) nxthead_model = loader.loadModel(nxthead_filepath) nxtwaist_nodepath = NodePath("nxtwaist_nm") nxtbody_nodepath = NodePath("nxtbody_nm") nxthead_nodepath = NodePath("nxthead_nm") nxtwaist_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtwaist_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtbody_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtbody_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxthead_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxthead_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtwaist_model.instanceTo(nxtwaist_nodepath) nxtwaist_rotmat = pandageom.cvtMat4(nxtrobot.base['rotmat']) nxtwaist_nodepath.setMat(nxtwaist_rotmat) nxtbody_model.instanceTo(nxtbody_nodepath) nxthead_model.instanceTo(nxthead_nodepath) nxthead_nodepath.setH(nxtrobot.initjnts[1]) nxthead_nodepath.setP(nxtrobot.initjnts[2]) nxthead_nodepath.setZ(569.5) nxtwaist_nodepath.reparentTo(nxtmnp) nxtbody_nodepath.reparentTo(nxtwaist_nodepath) nxthead_nodepath.reparentTo(nxtwaist_nodepath) # rgtarm nxtrgtarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj0_nm.egg")) nxtrgtarmlj0_model = loader.loadModel(nxtrgtarmlj0_filepath) nxtrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath") nxtrgtarmlj0_model.instanceTo(nxtrgtarmlj0_nodepath) nxtrgtarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[1]['rotmat'], nxtrobot.rgtarm[1]['linkpos']) nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat) nxtrgtarmlj0_nodepath.reparentTo(nxtmnp) nxtrgtarmlj0_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtrgtarmlj0_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtrgtarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj1_nm.egg")) nxtrgtarmlj1_model = loader.loadModel(nxtrgtarmlj1_filepath) nxtrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath") nxtrgtarmlj1_model.instanceTo(nxtrgtarmlj1_nodepath) nxtrgtarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[2]['rotmat'], nxtrobot.rgtarm[2]['linkpos']) nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat) nxtrgtarmlj1_nodepath.reparentTo(nxtmnp) nxtrgtarmlj1_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtrgtarmlj1_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtrgtarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj2_nm.egg")) nxtrgtarmlj2_model = loader.loadModel(nxtrgtarmlj2_filepath) nxtrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath") nxtrgtarmlj2_model.instanceTo(nxtrgtarmlj2_nodepath) nxtrgtarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[3]['rotmat'], nxtrobot.rgtarm[3]['linkpos']) nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat) nxtrgtarmlj2_nodepath.reparentTo(nxtmnp) nxtrgtarmlj2_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtrgtarmlj2_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtrgtarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj3_nm.egg")) nxtrgtarmlj3_model = loader.loadModel(nxtrgtarmlj3_filepath) nxtrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath") nxtrgtarmlj3_model.instanceTo(nxtrgtarmlj3_nodepath) nxtrgtarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[4]['rotmat'], nxtrobot.rgtarm[4]['linkpos']) nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat) nxtrgtarmlj3_nodepath.reparentTo(nxtmnp) nxtrgtarmlj3_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtrgtarmlj3_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtrgtarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_rgtarm_lj4_nm.egg")) nxtrgtarmlj4_model = loader.loadModel(nxtrgtarmlj4_filepath) nxtrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath") nxtrgtarmlj4_model.instanceTo(nxtrgtarmlj4_nodepath) nxtrgtarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[5]['rotmat'], nxtrobot.rgtarm[5]['linkpos']) nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat) nxtrgtarmlj4_nodepath.reparentTo(nxtmnp) nxtrgtarmlj4_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtrgtarmlj4_nodepath.setTransparency(TransparencyAttrib.MAlpha) # lftarm nxtlftarmlj0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj0_nm.egg")) nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath) nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath") nxtlftarmlj0_model.instanceTo(nxtlftarmlj0_nodepath) nxtlftarmlj0_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[1]['rotmat'], nxtrobot.lftarm[1]['linkpos']) nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat) nxtlftarmlj0_nodepath.reparentTo(nxtmnp) nxtlftarmlj0_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtlftarmlj0_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtlftarmlj1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj1_nm.egg")) nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath) nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath") nxtlftarmlj1_model.instanceTo(nxtlftarmlj1_nodepath) nxtlftarmlj1_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[2]['rotmat'], nxtrobot.lftarm[2]['linkpos']) nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat) nxtlftarmlj1_nodepath.reparentTo(nxtmnp) nxtlftarmlj1_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtlftarmlj1_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtlftarmlj2_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj2_nm.egg")) nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath) nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath") nxtlftarmlj2_model.instanceTo(nxtlftarmlj2_nodepath) nxtlftarmlj2_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[3]['rotmat'], nxtrobot.lftarm[3]['linkpos']) nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat) nxtlftarmlj2_nodepath.reparentTo(nxtmnp) nxtlftarmlj2_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtlftarmlj2_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtlftarmlj3_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj3_nm.egg")) nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath) nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath") nxtlftarmlj3_model.instanceTo(nxtlftarmlj3_nodepath) nxtlftarmlj3_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[4]['rotmat'], nxtrobot.lftarm[4]['linkpos']) nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat) nxtlftarmlj3_nodepath.reparentTo(nxtmnp) nxtlftarmlj3_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtlftarmlj3_nodepath.setTransparency(TransparencyAttrib.MAlpha) nxtlftarmlj4_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "nxtegg/nomat", "nxt_lftarm_lj4_nm.egg")) nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath) nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath") nxtlftarmlj4_model.instanceTo(nxtlftarmlj4_nodepath) nxtlftarmlj4_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[5]['rotmat'], nxtrobot.lftarm[5]['linkpos']) nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat) nxtlftarmlj4_nodepath.reparentTo(nxtmnp) nxtlftarmlj4_nodepath.setColor(plotcolor[0],plotcolor[1],plotcolor[2],plotcolor[3]) nxtlftarmlj4_nodepath.setTransparency(TransparencyAttrib.MAlpha) # rgthnd nxtrgthnd = handpkg.newHandNM('rgt', hndcolor=plotcolor) nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.rgtarm[6]['rotmat'], nxtrobot.rgtarm[6]['linkpos']) nxtrgthnd.setMat(nxtlftarmlj5_rotmat) nxtrgthnd.reparentTo(nxtmnp) if jawwidthrgt is not None: nxtrgthnd.setJawwidth(jawwidthrgt) # lfthnd nxtlfthnd = handpkg.newHandNM('lft', hndcolor=plotcolor) nxtlftarmlj5_rotmat = pandageom.cvtMat4(nxtrobot.lftarm[6]['rotmat'], nxtrobot.lftarm[6]['linkpos']) nxtlfthnd.setMat(nxtlftarmlj5_rotmat) nxtlfthnd.reparentTo(nxtmnp) if jawwidthlft is not None: nxtlfthnd.setJawwidth(jawwidthlft) nxtmnp.setTransparency(TransparencyAttrib.MAlpha) return nxtmnp
def __init__(self, jawwidth=85, hndcolor=None): ''' load the robotiq85 model, set jawwidth and return a nodepath the rtq85 gripper is composed of a parallelism and a fixed triangle, the parallelism: 1.905-1.905; 5.715-5.715; 70/110 degree the triangle: 4.75 (finger) 5.715 (inner knuckle) 3.175 (outer knuckle) NOTE: the setColor function is only useful when the models dont have any materials ## input pandabase: the showbase() object jawwidth: the distance between fingertips ## output rtq85np: the nodepath of this rtq85 hand author: weiwei date: 20160627 ''' self.rtq85np = NodePath("rtq85hnd") self.handnp = self.rtq85np self.jawwidth = jawwidth this_dir, this_filename = os.path.split(__file__) rtq85basepath = Filename.fromOsSpecific( os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_base_link_nm.egg")) rtq85fingerpath = Filename.fromOsSpecific( os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_finger_link_nm.egg")) rtq85fingertippath = Filename.fromOsSpecific( os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_finger_tip_link_nm.egg")) rtq85innerknucklepath = Filename.fromOsSpecific( os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_inner_knuckle_link_nm.egg")) rtq85knucklepath = Filename.fromOsSpecific( os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_knuckle_link_nm.egg")) rtq85base = NodePath("rtq85base") rtq85lknuckle = NodePath("rtq85lknuckle") rtq85rknuckle = NodePath("rtq85rknuckle") rtq85lfgr = NodePath("rtq85lfgr") rtq85rfgr = NodePath("rtq85rfgr") rtq85ilknuckle = NodePath("rtq85ilknuckle") rtq85irknuckle = NodePath("rtq85irknuckle") rtq85lfgrtip = NodePath("rtq85lfgrtip") rtq85rfgrtip = NodePath("rtq85rfgrtip") # loader is a global variable defined by panda3d rtq85_basel = loader.loadModel(rtq85basepath) rtq85_fingerl = loader.loadModel(rtq85fingerpath) rtq85_fingertipl = loader.loadModel(rtq85fingertippath) rtq85_innerknucklel = loader.loadModel(rtq85innerknucklepath) rtq85_knucklel = loader.loadModel(rtq85knucklepath) # base rtq85_basel.instanceTo(rtq85base) rtq85base.setPos(0, 0, 0) if hndcolor is None: # rtq85base.setColor(.2,.2,.2,1) pass else: rtq85base.setColor(hndcolor[0], hndcolor[1], hndcolor[2], hndcolor[3]) rtq85base.setTransparency(TransparencyAttrib.MAlpha) # left and right outer knuckle rtq85_knucklel.instanceTo(rtq85lknuckle) rtq85lknuckle.setPos(-30.60114443, 54.90451627, 0) rtq85lknuckle.setHpr(0, 0, 180) if hndcolor is None: # rtq85lknuckle.setColor(.5,.5,.5,1) pass else: rtq85lknuckle.setColor(hndcolor[0], hndcolor[1], hndcolor[2], hndcolor[3]) rtq85lknuckle.setTransparency(TransparencyAttrib.MAlpha) rtq85lknuckle.reparentTo(rtq85base) rtq85_knucklel.instanceTo(rtq85rknuckle) rtq85rknuckle.setPos(30.60114443, 54.90451627, 0) rtq85rknuckle.setHpr(0, 0, 0) if hndcolor is None: # rtq85rknuckle.setColor(.5,.5,.5,1) pass else: rtq85rknuckle.setColor(hndcolor[0], hndcolor[1], hndcolor[2], hndcolor[3]) rtq85rknuckle.setTransparency(TransparencyAttrib.MAlpha) rtq85rknuckle.reparentTo(rtq85base) # left and right finger rtq85_fingerl.instanceTo(rtq85lfgr) rtq85lfgr.setPos(31.48504435, -4.08552455, 0) if hndcolor is None: # rtq85lfgr.setColor(0.2,0.2,0.2,1) pass else: rtq85lfgr.setColor(hndcolor[0], hndcolor[1], hndcolor[2], hndcolor[3]) rtq85lfgr.setTransparency(TransparencyAttrib.MAlpha) rtq85lfgr.reparentTo(rtq85lknuckle) rtq85_fingerl.instanceTo(rtq85rfgr) rtq85rfgr.setPos(31.48504435, -4.08552455, 0) if hndcolor is None: # rtq85rfgr.setColor(0.2,0.2,0.2,1) pass else: rtq85rfgr.setColor(hndcolor[0], hndcolor[1], hndcolor[2], hndcolor[3]) rtq85rfgr.setTransparency(TransparencyAttrib.MAlpha) rtq85rfgr.reparentTo(rtq85rknuckle) # left and right inner knuckle rtq85_innerknucklel.instanceTo(rtq85ilknuckle) rtq85ilknuckle.setPos(-12.7, 61.42, 0) rtq85ilknuckle.setHpr(0, 0, 180) if hndcolor is None: # rtq85ilknuckle.setColor(.5,.5,.5,1) pass else: rtq85ilknuckle.setColor(hndcolor[0], hndcolor[1], hndcolor[2], hndcolor[3]) rtq85ilknuckle.setTransparency(TransparencyAttrib.MAlpha) rtq85ilknuckle.reparentTo(rtq85base) rtq85_innerknucklel.instanceTo(rtq85irknuckle) rtq85irknuckle.setPos(12.7, 61.42, 0) rtq85irknuckle.setHpr(0, 0, 0) if hndcolor is None: # rtq85irknuckle.setColor(.5,.5,.5,1) pass else: rtq85irknuckle.setColor(hndcolor[0], hndcolor[1], hndcolor[2], hndcolor[3]) rtq85irknuckle.setTransparency(TransparencyAttrib.MAlpha) rtq85irknuckle.reparentTo(rtq85base) # left and right fgr tip rtq85_fingertipl.instanceTo(rtq85lfgrtip) rtq85lfgrtip.setPos(37.59940821, 43.03959807, 0) if hndcolor is None: # rtq85lfgrtip.setColor(.5,.5,.5,1) pass else: rtq85lfgrtip.setColor(hndcolor[0], hndcolor[1], hndcolor[2], hndcolor[3]) rtq85lfgrtip.setTransparency(TransparencyAttrib.MAlpha) rtq85lfgrtip.reparentTo(rtq85ilknuckle) rtq85_fingertipl.instanceTo(rtq85rfgrtip) rtq85rfgrtip.setPos(37.59940821, 43.03959807, 0) rtq85rfgrtip.setHpr(0, 0, 0) if hndcolor is None: # rtq85rfgrtip.setColor(.5,.5,.5,1) pass else: rtq85rfgrtip.setColor(hndcolor[0], hndcolor[1], hndcolor[2], hndcolor[3]) rtq85rfgrtip.setTransparency(TransparencyAttrib.MAlpha) rtq85rfgrtip.reparentTo(rtq85irknuckle) # rotate to x, y, z coordinates (this one rotates the base, not the self.rtq85np) rtq85base.setMat( pandageom.cvtMat4(rm.rodrigues([0, 0, 1], 90)) * rtq85base.getMat()) rtq85base.reparentTo(self.rtq85np) self.setJawwidth(jawwidth) self.__jawwidthopen = 85.0 self.__jawwidthclosed = 0.0
import numpy as np import pandaplotutils.pandactrl as pc import pandaplotutils.pandageom as pg import utils.robotmath as rm base = pc.World(camp=[1000, -300, 700], lookatp=[0, 0, 0], w=1000, h=1000) pggen = pg.PandaGeomGen() pggen.plotAxis(base.render) pggen.plotBox(base.render, pos=[0, 0, 0], x=100, y=100, z=50, rgba=[1, 1, .3, 1]) # rotmat = rm.rodrigues([1, 1, 1], 30) pandarotmat = pg.cvtMat4(rotmat, np.array([0, 0, 0])) pggen.plotAxis(base.render, pandamat4=pandarotmat) base.run()
def removeHndcc(self, base, discretesize=8): """ Handcc means hand collision detection :param discretesize: the number of hand orientations :return: author: weiwei date: 20161212, tsukuba """ # isplotted = 0 # if self.rtq85plotlist: # for rtq85plotnode in self.rtq85plotlist: # rtq85plotnode.removeNode() # self.rtq85plotlist = [] self.gripcontacts = [] self.griprotmats = [] self.gripjawwidth = [] self.gripcontactnormals = [] plotoffsetfp = 6 self.counter = 0 while self.counter < self.facetpairs.shape[0]: # print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1) # print self.gripcontactpairs_precc facetpair = self.facetpairs[self.counter] facetidx0 = facetpair[0] facetidx1 = facetpair[1] for j, contactpair in enumerate( self.gripcontactpairs_precc[self.counter]): for angleid in range(discretesize): cctpnt0 = contactpair[ 0] + plotoffsetfp * self.facetnormals[facetidx0] cctpnt1 = contactpair[ 1] + plotoffsetfp * self.facetnormals[facetidx1] cctnormal0 = self.gripcontactpairnormals_precc[ self.counter][j][0] cctnormal1 = [ -cctnormal0[0], -cctnormal0[1], -cctnormal0[2] ] tmphand = self.hand # tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1]) # save initial hand pose initmat = tmphand.getMat() fgrdist = np.linalg.norm((cctpnt0 - cctpnt1)) tmphand.setJawwidth(fgrdist) tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2]) rotax = [0, 1, 0] rotangle = 360.0 / discretesize * angleid rotmat = rm.rodrigues(rotax, rotangle) tmphand.setMat( pandageom.cvtMat4(rotmat) * tmphand.getMat()) axx = tmphand.getMat().getRow3(0) # 130 is the distance from hndbase to fingertip cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array( [axx[0], axx[1], axx[2]]) tmphand.setPos( Point3(cctcenter[0], cctcenter[1], cctcenter[2])) # collision detection hndbullnode = cd.genCollisionMeshMultiNp( tmphand.handnp, base.render) result = self.bulletworld.contactTest(hndbullnode) if not result.getNumContacts(): self.gripcontacts.append(contactpair) self.griprotmats.append(tmphand.getMat()) self.gripjawwidth.append(fgrdist) self.gripcontactnormals.append( self.gripcontactpairnormals_precc[self.counter][j]) # pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1]) # pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]), # tmprtq85.getMat(), length=30, thickness=2) # tmprtq85.setColor([.5, .5, .5, 1]) # tmprtq85.reparentTo(base.render) # self.rtq85plotlist.append(tmprtq85) # isplotted = 1 # reset initial hand pose tmphand.setMat(initmat) self.counter += 1 self.counter = 0
def genmnp_nm(hrp2krobot, handpkg, jawwidthrgt = None, jawwidthlft = None): """ generate a panda3d nodepath for the hrp2krobot mnp indicates this function generates a mesh nodepath :param hrp2krobot: the Hrp5Robot object, see hrp5.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20170630 """ hrp2kmnp = NodePath("hrp2kmnp") this_dir, this_filename = os.path.split(__file__) # chest0-2, head1 (neck is not plotted) hrp2kbody_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp2kegg", "body_nm.egg")) hrp2kchest0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp2kegg", "chest0_nm.egg")) hrp2kchest1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp2kegg", "chest1_nm.egg")) hrp2khead0_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp2kegg", "head0_nm.egg")) hrp2khead1_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "hrp2kegg", "head1_nm.egg")) hrp2kbody_model = loader.loadModel(hrp2kbody_filepath) hrp2kchest0_model = loader.loadModel(hrp2kchest0_filepath) hrp2kchest1_model = loader.loadModel(hrp2kchest1_filepath) hrp2khead0_model = loader.loadModel(hrp2khead0_filepath) hrp2khead1_model = loader.loadModel(hrp2khead1_filepath) hrp2kbody_nodepath = NodePath("hrp2kbody") hrp2kchest0_nodepath = NodePath("hrp2kchest0") hrp2kchest1_nodepath = NodePath("hrp2kchest1") hrp2khead0_nodepath = NodePath("hrp2khead0") hrp2khead1_nodepath = NodePath("hrp2khead1") # body hrp2kbody_model.instanceTo(hrp2kbody_nodepath) hrp2kbody_rotmat = Mat4.identMat() hrp2kbody_nodepath.setMat(hrp2kbody_rotmat) hrp2kbody_nodepath.setPos(Vec3(-32, 0, -349.2)) hrp2kbody_nodepath.reparentTo(hrp2kmnp) # chest hrp2kchest0_model.instanceTo(hrp2kchest0_nodepath) hrp2kchest0_rotmat = pg.cvtMat4(hrp2krobot.base['rotmat']) hrp2kchest0_nodepath.setMat(hrp2kchest0_rotmat) hrp2kchest0_nodepath.reparentTo(hrp2kmnp) hrp2kchest1_model.instanceTo(hrp2kchest1_nodepath) hrp2kchest1_rotmat = Mat4.identMat() hrp2kchest1_nodepath.setMat(hrp2kchest1_rotmat) hrp2kchest1_nodepath.reparentTo(hrp2kchest0_nodepath) hrp2khead0_model.instanceTo(hrp2khead0_nodepath) hrp2khead0_nodepath.setH(hrp2krobot.initjnts[0]) hrp2khead0_nodepath.reparentTo(hrp2kchest1_nodepath) hrp2khead0_nodepath.setX(-7) hrp2khead0_nodepath.setZ(297.3) hrp2khead1_model.instanceTo(hrp2khead1_nodepath) hrp2khead1_nodepath.setP(hrp2krobot.initjnts[1]) hrp2khead1_nodepath.reparentTo(hrp2khead0_nodepath) # rgtarm hrp2krgtarmlj0_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "rarm0_nm.egg")) hrp2krgtarmlj0_model = loader.loadModel(hrp2krgtarmlj0_filepath) hrp2krgtarmlj0_nodepath = NodePath("hrp2krgtarmlj0_nodepath") hrp2krgtarmlj0_model.instanceTo(hrp2krgtarmlj0_nodepath) hrp2krgtarmlj0_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[1]['rotmat'], hrp2krobot.rgtarm[1]['linkpos']) hrp2krgtarmlj0_nodepath.setMat(hrp2krgtarmlj0_rotmat) hrp2krgtarmlj0_nodepath.reparentTo(hrp2kmnp) # # hrp2krgtarmlj1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "rarm1_nm.egg")) hrp2krgtarmlj1_model = loader.loadModel(hrp2krgtarmlj1_filepath) hrp2krgtarmlj1_nodepath = NodePath("hrp2krgtarmlj1_nodepath") hrp2krgtarmlj1_model.instanceTo(hrp2krgtarmlj1_nodepath) hrp2krgtarmlj1_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[2]['rotmat'], hrp2krobot.rgtarm[2]['linkpos']) hrp2krgtarmlj1_nodepath.setMat(hrp2krgtarmlj1_rotmat) hrp2krgtarmlj1_nodepath.reparentTo(hrp2kmnp) # # hrp2krgtarmlj2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "rarm2_nm.egg")) hrp2krgtarmlj2_model = loader.loadModel(hrp2krgtarmlj2_filepath) hrp2krgtarmlj2_nodepath = NodePath("hrp2krgtarmlj2_nodepath") hrp2krgtarmlj2_model.instanceTo(hrp2krgtarmlj2_nodepath) hrp2krgtarmlj2_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[3]['rotmat'], hrp2krobot.rgtarm[3]['linkpos']) hrp2krgtarmlj2_nodepath.setMat(hrp2krgtarmlj2_rotmat) hrp2krgtarmlj2_nodepath.reparentTo(hrp2kmnp) # # hrp2krgtarmlj3_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "rarm3_nm.egg")) hrp2krgtarmlj3_model = loader.loadModel(hrp2krgtarmlj3_filepath) hrp2krgtarmlj3_nodepath = NodePath("hrp2krgtarmlj3_nodepath") hrp2krgtarmlj3_model.instanceTo(hrp2krgtarmlj3_nodepath) hrp2krgtarmlj3_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[4]['rotmat'], hrp2krobot.rgtarm[4]['linkpos']) hrp2krgtarmlj3_nodepath.setMat(hrp2krgtarmlj3_rotmat) hrp2krgtarmlj3_nodepath.reparentTo(hrp2kmnp) # # hrp2krgtarmlj4_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "rarm4_nm.egg")) hrp2krgtarmlj4_model = loader.loadModel(hrp2krgtarmlj4_filepath) hrp2krgtarmlj4_nodepath = NodePath("hrp2krgtarmlj4_nodepath") hrp2krgtarmlj4_model.instanceTo(hrp2krgtarmlj4_nodepath) hrp2krgtarmlj4_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[5]['rotmat'], hrp2krobot.rgtarm[5]['linkpos']) hrp2krgtarmlj4_nodepath.setMat(hrp2krgtarmlj4_rotmat) hrp2krgtarmlj4_nodepath.reparentTo(hrp2kmnp) # # hrp2krgtarmlj5_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "rarm5_nm.egg")) hrp2krgtarmlj5_model = loader.loadModel(hrp2krgtarmlj5_filepath) hrp2krgtarmlj5_nodepath = NodePath("hrp2krgtarmlj5_nodepath") hrp2krgtarmlj5_model.instanceTo(hrp2krgtarmlj5_nodepath) hrp2krgtarmlj5_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[6]['rotmat'], hrp2krobot.rgtarm[6]['linkpos']) hrp2krgtarmlj5_nodepath.setMat(hrp2krgtarmlj5_rotmat) hrp2krgtarmlj5_nodepath.reparentTo(hrp2kmnp) # # hrp2krgtarmlj6_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "rarm6_nm.egg")) hrp2krgtarmlj6_model = loader.loadModel(hrp2krgtarmlj6_filepath) hrp2krgtarmlj6_nodepath = NodePath("hrp2krgtarmlj6_nodepath") hrp2krgtarmlj6_model.instanceTo(hrp2krgtarmlj6_nodepath) hrp2krgtarmlj6_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[7]['rotmat'], hrp2krobot.rgtarm[7]['linkpos']) hrp2krgtarmlj6_nodepath.setMat(hrp2krgtarmlj6_rotmat) hrp2krgtarmlj6_nodepath.reparentTo(hrp2kmnp) # # # lftarm hrp2klftarmlj0_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "larm0_nm.egg")) hrp2klftarmlj0_model = loader.loadModel(hrp2klftarmlj0_filepath) hrp2klftarmlj0_nodepath = NodePath("hrp2klftarmlj0_nodepath") hrp2klftarmlj0_model.instanceTo(hrp2klftarmlj0_nodepath) hrp2klftarmlj0_rotmat = pg.cvtMat4(hrp2krobot.lftarm[1]['rotmat'], hrp2krobot.lftarm[1]['linkpos']) hrp2klftarmlj0_nodepath.setMat(hrp2klftarmlj0_rotmat) hrp2klftarmlj0_nodepath.reparentTo(hrp2kmnp) # # hrp2klftarmlj1_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "larm1_nm.egg")) hrp2klftarmlj1_model = loader.loadModel(hrp2klftarmlj1_filepath) hrp2klftarmlj1_nodepath = NodePath("hrp2klftarmlj1_nodepath") hrp2klftarmlj1_model.instanceTo(hrp2klftarmlj1_nodepath) hrp2klftarmlj1_rotmat = pg.cvtMat4(hrp2krobot.lftarm[2]['rotmat'], hrp2krobot.lftarm[2]['linkpos']) hrp2klftarmlj1_nodepath.setMat(hrp2klftarmlj1_rotmat) hrp2klftarmlj1_nodepath.reparentTo(hrp2kmnp) # # hrp2klftarmlj2_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "larm2_nm.egg")) hrp2klftarmlj2_model = loader.loadModel(hrp2klftarmlj2_filepath) hrp2klftarmlj2_nodepath = NodePath("hrp2klftarmlj2_nodepath") hrp2klftarmlj2_model.instanceTo(hrp2klftarmlj2_nodepath) hrp2klftarmlj2_rotmat = pg.cvtMat4(hrp2krobot.lftarm[3]['rotmat'], hrp2krobot.lftarm[3]['linkpos']) hrp2klftarmlj2_nodepath.setMat(hrp2klftarmlj2_rotmat) hrp2klftarmlj2_nodepath.reparentTo(hrp2kmnp) # # hrp2klftarmlj3_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "larm3_nm.egg")) hrp2klftarmlj3_model = loader.loadModel(hrp2klftarmlj3_filepath) hrp2klftarmlj3_nodepath = NodePath("hrp2klftarmlj3_nodepath") hrp2klftarmlj3_model.instanceTo(hrp2klftarmlj3_nodepath) hrp2klftarmlj3_rotmat = pg.cvtMat4(hrp2krobot.lftarm[4]['rotmat'], hrp2krobot.lftarm[4]['linkpos']) hrp2klftarmlj3_nodepath.setMat(hrp2klftarmlj3_rotmat) hrp2klftarmlj3_nodepath.reparentTo(hrp2kmnp) # # hrp2klftarmlj4_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "larm4_nm.egg")) hrp2klftarmlj4_model = loader.loadModel(hrp2klftarmlj4_filepath) hrp2klftarmlj4_nodepath = NodePath("hrp2klftarmlj4_nodepath") hrp2klftarmlj4_model.instanceTo(hrp2klftarmlj4_nodepath) hrp2klftarmlj4_rotmat = pg.cvtMat4(hrp2krobot.lftarm[5]['rotmat'], hrp2krobot.lftarm[5]['linkpos']) hrp2klftarmlj4_nodepath.setMat(hrp2klftarmlj4_rotmat) hrp2klftarmlj4_nodepath.reparentTo(hrp2kmnp) # # hrp2klftarmlj5_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "larm5_nm.egg")) hrp2klftarmlj5_model = loader.loadModel(hrp2klftarmlj5_filepath) hrp2klftarmlj5_nodepath = NodePath("hrp2klftarmlj5_nodepath") hrp2klftarmlj5_model.instanceTo(hrp2klftarmlj5_nodepath) hrp2klftarmlj5_rotmat = pg.cvtMat4(hrp2krobot.lftarm[6]['rotmat'], hrp2krobot.lftarm[6]['linkpos']) hrp2klftarmlj5_nodepath.setMat(hrp2klftarmlj5_rotmat) hrp2klftarmlj5_nodepath.reparentTo(hrp2kmnp) # # hrp2klftarmlj6_filepath = Filename.fromOsSpecific( os.path.join(this_dir, "hrp2kegg", "larm6_nm.egg")) hrp2klftarmlj6_model = loader.loadModel(hrp2klftarmlj6_filepath) hrp2klftarmlj6_nodepath = NodePath("hrp2klftarmlj6_nodepath") hrp2klftarmlj6_model.instanceTo(hrp2klftarmlj6_nodepath) hrp2klftarmlj6_rotmat = pg.cvtMat4(hrp2krobot.lftarm[7]['rotmat'], hrp2krobot.lftarm[7]['linkpos']) hrp2klftarmlj6_nodepath.setMat(hrp2klftarmlj6_rotmat) hrp2klftarmlj6_nodepath.reparentTo(hrp2kmnp) # # rgthnd hrp2krobotrgthnd = handpkg.newHandNM('rgt') hrp2krobotrgtarmlj7_rotmat = pg.cvtMat4(hrp2krobot.rgtarm[7]['rotmat'], hrp2krobot.rgtarm[7]['linkpos']) # pg.plotAxisSelf(hrp2kmnp, hrp2krobot.rgtarm[9]['linkend'], hrp2krobotrgtarmlj9_rotmat) hrp2krobotrgthnd.reparentTo(hrp2kmnp) hrp2krobotrgthnd.setMat(hrp2krobotrgtarmlj7_rotmat) pos = hrp2krobotrgthnd.getPos() linklength = np.linalg.norm(hrp2krobot.rgtarm[7]['linkvec']) hrp2krobotrgthnd.setPos(pos-hrp2krobotrgtarmlj7_rotmat.getRow3(0)*(linklength-135)) if jawwidthrgt is not None: hrp2krobotrgthnd.setJawwidth(jawwidthrgt) # # lfthnd hrp2krobotlfthnd = handpkg.newHandNM('lft') hrp2krobotlftarmlj7_rotmat = pg.cvtMat4(hrp2krobot.lftarm[7]['rotmat'], hrp2krobot.lftarm[7]['linkpos']) # pg.plotAxisSelf(hrp2kmnp, hrp2krobot.lftarm[9]['linkend'], hrp2krobotlftarmlj9_rotmat) hrp2krobotlfthnd.reparentTo(hrp2kmnp) hrp2krobotlfthnd.setMat(hrp2krobotlftarmlj7_rotmat) pos = hrp2krobotlfthnd.getPos() linklength = np.linalg.norm(hrp2krobot.rgtarm[7]['linkvec']) hrp2krobotlfthnd.setPos(pos-hrp2krobotlftarmlj7_rotmat.getRow3(0)*(linklength-135)) if jawwidthlft is not None: hrp2krobotlfthnd.setJawwidth(jawwidthlft) hrp2kmnp.setTransparency(TransparencyAttrib.MAlpha) return hrp2kmnp
def saveToDB(self, positionlist, gdb, discretesize=4.0): """ :param positionlist: a list of positions to place the object one the table :param discretesize: the discretization of rotation angles around z axis :return: author: weiwei date: 20161215, osaka """ # save discretiezed angle sql = "SELECT * FROM angle" result = gdb.execute(sql) if len(result) == 0: sql = "INSERT INTO angle(value) VALUES " for i in range(discretesize): sql += "(" + str(360 * i * 1.0 / discretesize) + "), " sql = sql[:-2] + ";" gdb.execute(sql) else: print "Angles already set!" # save tabletopplacements sql = "SELECT idtabletopplacements FROM tabletopplacements,freetabletopplacement,object WHERE \ tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \ freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: # 1) select the freetabletopplacement sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \ FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \ AND object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: raise ValueError("Plan the freetabletopplacement first!") result = np.asarray(result) idfreelist = [int(x) for x in result[:, 0]] rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]] # 2) select the angle sql = "SELECT angle.idangle,angle.value FROM angle" result = np.asarray(gdb.execute(sql)) idanglelist = [int(x) for x in result[:, 0]] anglevaluelist = [float(x) for x in result[:, 1]] # 3) save to database sql = "INSERT INTO tabletopplacements(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES " for ttoppos in positionlist: ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2]) for idfree, rotmatfree in zip(idfreelist, rotmatfreelist): for idangle, anglevalue in zip(idanglelist, anglevaluelist): rotangle = anglevalue rotmat = rm.rodrigues([0, 0, 1], rotangle) # rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0, # rotmat[1][0], rotmat[1][1], rotmat[1][2], 0, # rotmat[2][0], rotmat[2][1], rotmat[2][2], 0, # ttoppos[0], ttoppos[1], ttoppos[2], 1) rotmat4 = pg.cvtMat4(rotmat, ttoppos) varrotmat = rotmatfree * rotmat4 sql += "('%s', '%s', %d, %d), " % \ (dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree) sql = sql[:-2] + ";" gdb.execute(sql) else: print "Tabletopplacements already exist!" # save tabletopgrips idhand = gdb.loadIdHand(self.handname) sql = "SELECT tabletopgrips.idtabletopgrips FROM tabletopgrips,freeairgrip,object WHERE \ tabletopgrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \ freeairgrip.idobject=object.idobject AND object.name LIKE '%s' AND \ freeairgrip.idhand = %d" % (self.dbobjname, idhand) result = gdb.execute(sql) if len(result) == 0: sql = "SELECT freetabletopplacement.idfreetabletopplacement \ FROM freetabletopplacement,object WHERE \ freetabletopplacement.idobject = object.idobject AND \ object.name LIKE '%s'" % self.dbobjname result = gdb.execute(sql) if len(result) == 0: raise ValueError("Plan the freetabletopplacement first!") for idfree in result: idfree = int(idfree[0]) sql = "SELECT tabletopplacements.idtabletopplacements, \ tabletopplacements.tabletopposition, angle.value,\ freetabletopgrip.contactpoint0, freetabletopgrip.contactpoint1, \ freetabletopgrip.contactnormal0, freetabletopgrip.contactnormal1, \ freetabletopgrip.rotmat, freetabletopgrip.jawwidth, freetabletopgrip.idfreeairgrip \ FROM tabletopplacements,freetabletopplacement,freetabletopgrip,freeairgrip,angle WHERE \ tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \ tabletopplacements.idangle = angle.idangle AND \ freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \ freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \ freeairgrip.idhand = %d AND \ freetabletopplacement.idfreetabletopplacement = %d" % ( idhand, idfree) result1 = gdb.execute(sql) if len(result1) == 0: # no grasp availalbe? continue # sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, \ # contactnormal1, rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES " if len(result1) > 20000: result1 = result1[0::int(len(result1) / 20000.0)] result1 = np.asarray(result1) idtabletopplacementslist = [int(x) for x in result1[:, 0]] tabletoppositionlist = [dc.strToV3(x) for x in result1[:, 1]] rotanglelist = [float(x) for x in result1[:, 2]] freegripcontactpoint0list = [ dc.strToV3(x) for x in result1[:, 3] ] freegripcontactpoint1list = [ dc.strToV3(x) for x in result1[:, 4] ] freegripcontactnormal0list = [ dc.strToV3(x) for x in result1[:, 5] ] freegripcontactnormal1list = [ dc.strToV3(x) for x in result1[:, 6] ] freegriprotmatlist = [dc.strToMat4(x) for x in result1[:, 7]] freegripjawwidthlist = [float(x) for x in result1[:, 8]] freegripidlist = [int(x) for x in result1[:, 9]] for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \ freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \ tabletoppositionlist, rotanglelist, freegripcontactpoint0list, freegripcontactpoint1list, \ freegripcontactnormal0list, freegripcontactnormal1list, freegriprotmatlist, freegripjawwidthlist, \ freegripidlist): rotmat = rm.rodrigues([0, 0, 1], rotangle) # rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0, # rotmat[1][0], rotmat[1][1], rotmat[1][2], 0, # rotmat[2][0], rotmat[2][1], rotmat[2][2], 0, # ttoppos[0], ttoppos[1], ttoppos[2], 1) rotmat4 = pg.cvtMat4(rotmat, ttoppos) ttpcct0 = rotmat4.xformPoint(cct0) ttpcct1 = rotmat4.xformPoint(cct1) ttpcctn0 = rotmat4.xformVec(cctn0) ttpcctn1 = rotmat4.xformVec(cctn1) ttpgriprotmat = freegriprotmat * rotmat4 sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \ rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES \ ('%s', '%s', '%s', '%s', '%s', '%s', %d, %d) " % \ (dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \ dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements) gdb.execute(sql) else: print "Tabletopgrips already exist!" print "Save to DB done!"
def genmnp_list(self, hrp5nrobot, jawwidthrgt=None, jawwidthlft=None): """ generate a panda3d nodepath for the hrp5robot mnp indicates this function generates a mesh nodepath return [[right arm mnp list], [leftarm mnp list], [body mnp list]] where one arm mnp list is ordered from base to end-effector this function is designed for collision detection :param hrp5robot: the Hrp5Robot object, see hrp5.py :return: a nodepath which is ready to be plotted using plotmesh author: weiwei date: 20170613 """ identmat4 = Mat4.identMat() # body self.__hrp5nbody_nodepath.setMat(identmat4) self.__hrp5nbody_nodepath.setZ(self.__hrp5nmnp, 0) # chest 0 self.__hrp5nchest0_nodepath.setMat(identmat4) self.__hrp5nchest0_nodepath.setZ(self.__hrp5nmnp, 274) # chest 1 self.__hrp5nchest1_nodepath.setMat(identmat4) self.__hrp5nchest1_nodepath.setColor(.7, .7, .2, 1) # chest 2 hrp5nchest2_rotmat = pg.cvtMat4(hrp5nrobot.base['rotmat']) self.__hrp5nchest2_nodepath.setMat(hrp5nchest2_rotmat) self.__hrp5nchest2_nodepath.setColor(.7, .7, .7, 1) # head 0 self.__hrp5nhead0_nodepath.setMat(identmat4) self.__hrp5nhead0_nodepath.setH(hrp5nrobot.initjnts[0]) self.__hrp5nhead0_nodepath.setX(32) self.__hrp5nhead0_nodepath.setZ(521) # head 1 self.__hrp5nhead1_nodepath.setP(hrp5nrobot.initjnts[1]) self.__hrp5nhead1_nodepath.setX(40) self.__hrp5nhead1_nodepath.setColor(.5, .5, .5, 1) # rgtarm 0 hrp5nrgtarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[1]['rotmat'], hrp5nrobot.rgtarm[1]['linkpos']) self.__hrp5nrgtarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj0_rotmat) self.__hrp5nrgtarmlj0_nodepath.setColor(.5, .5, .5, 1) # rgtarm 1 hrp5nrgtarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[2]['rotmat'], hrp5nrobot.rgtarm[2]['linkpos']) self.__hrp5nrgtarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj1_rotmat) self.__hrp5nrgtarmlj1_nodepath.setColor(.5, .5, .5, 1) # rgtarm 2 hrp5nrgtarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[3]['rotmat'], hrp5nrobot.rgtarm[3]['linkpos']) self.__hrp5nrgtarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj2_rotmat) self.__hrp5nrgtarmlj2_nodepath.setColor(.5, .5, .5, 1) # rgtarm 3 hrp5nrgtarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[4]['rotmat'], hrp5nrobot.rgtarm[4]['linkpos']) self.__hrp5nrgtarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj3_rotmat) self.__hrp5nrgtarmlj3_nodepath.setColor(.5, .5, .5, 1) # rgtarm 4 hrp5nrgtarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[5]['rotmat'], hrp5nrobot.rgtarm[5]['linkpos']) self.__hrp5nrgtarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj4_rotmat) self.__hrp5nrgtarmlj4_nodepath.setColor(.5, .5, .5, 1) # rgtarm 5 hrp5nrgtarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[6]['rotmat'], hrp5nrobot.rgtarm[6]['linkpos']) self.__hrp5nrgtarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj5_rotmat) self.__hrp5nrgtarmlj5_nodepath.setColor(.5, .5, .5, 1) # rgtarm 6 hrp5nrgtarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[7]['rotmat'], hrp5nrobot.rgtarm[7]['linkpos']) self.__hrp5nrgtarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj6_rotmat) self.__hrp5nrgtarmlj6_nodepath.setColor(.5, .5, .5, 1) # rgtarm 7 hrp5nrgtarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.rgtarm[8]['rotmat'], hrp5nrobot.rgtarm[8]['linkpos']) self.__hrp5nrgtarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nrgtarmlj7_rotmat) self.__hrp5nrgtarmlj7_nodepath.setColor(.5, .5, .5, 1) # lftarm 0 hrp5nlftarmlj0_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[1]['rotmat'], hrp5nrobot.lftarm[1]['linkpos']) self.__hrp5nlftarmlj0_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj0_rotmat) self.__hrp5nlftarmlj0_nodepath.setColor(.5, .5, .5, 1) # lftarm 1 hrp5nlftarmlj1_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[2]['rotmat'], hrp5nrobot.lftarm[2]['linkpos']) self.__hrp5nlftarmlj1_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj1_rotmat) self.__hrp5nlftarmlj1_nodepath.setColor(.5, .5, .5, 1) # lftarm 2 hrp5nlftarmlj2_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[3]['rotmat'], hrp5nrobot.lftarm[3]['linkpos']) self.__hrp5nlftarmlj2_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj2_rotmat) self.__hrp5nlftarmlj2_nodepath.setColor(.5, .5, .5, 1) # lftarm 3 hrp5nlftarmlj3_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[4]['rotmat'], hrp5nrobot.lftarm[4]['linkpos']) self.__hrp5nlftarmlj3_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj3_rotmat) self.__hrp5nlftarmlj3_nodepath.setColor(.5, .5, .5, 1) # lftarm 4 hrp5nlftarmlj4_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[5]['rotmat'], hrp5nrobot.lftarm[5]['linkpos']) self.__hrp5nlftarmlj4_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj4_rotmat) self.__hrp5nlftarmlj4_nodepath.setColor(.5, .5, .5, 1) # lftarm 5 hrp5nlftarmlj5_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[6]['rotmat'], hrp5nrobot.lftarm[6]['linkpos']) self.__hrp5nlftarmlj5_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj5_rotmat) self.__hrp5nlftarmlj5_nodepath.setColor(.5, .5, .5, 1) # lftarm 6 hrp5nlftarmlj6_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[7]['rotmat'], hrp5nrobot.lftarm[7]['linkpos']) self.__hrp5nlftarmlj6_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj6_rotmat) self.__hrp5nlftarmlj6_nodepath.setColor(.5, .5, .5, 1) # lftarm 7 hrp5nlftarmlj7_rotmat = pg.cvtMat4(hrp5nrobot.lftarm[8]['rotmat'], hrp5nrobot.lftarm[8]['linkpos']) self.__hrp5nlftarmlj7_nodepath.setMat(self.__hrp5nmnp, hrp5nlftarmlj7_rotmat) self.__hrp5nlftarmlj7_nodepath.setColor(.5, .5, .5, 1) # rgthand hrp5nrobotrgtarmlj9_rotmat = pg.cvtMat4( hrp5nrobot.rgtarm[9]['rotmat'], hrp5nrobot.rgtarm[9]['linkpos']) self.hrp5nrobotrgthnd.setMat(self.__hrp5nmnp, hrp5nrobotrgtarmlj9_rotmat) if jawwidthrgt is not None: self.hrp5nrobotrgthnd.setJawwidth(jawwidthrgt) # lfthand hrp5nrobotlftarmlj9_rotmat = pg.cvtMat4( hrp5nrobot.lftarm[9]['rotmat'], hrp5nrobot.lftarm[9]['linkpos']) self.hrp5nrobotlfthnd.setMat(self.__hrp5nmnp, hrp5nrobotlftarmlj9_rotmat) if jawwidthlft is not None: self.hrp5nrobotlfthnd.setJawwidth(jawwidthlft) return copy.deepcopy([ [ self.__hrp5nrgtarmlj0_nodepath, self.__hrp5nrgtarmlj1_nodepath, self.__hrp5nrgtarmlj2_nodepath, self.__hrp5nrgtarmlj3_nodepath, self.__hrp5nrgtarmlj4_nodepath, self.__hrp5nrgtarmlj5_nodepath, self.__hrp5nrgtarmlj6_nodepath, self.__hrp5nrgtarmlj7_nodepath, self.hrp5nrobotrgthnd.handnp ], [ self.__hrp5nlftarmlj0_nodepath, self.__hrp5nlftarmlj1_nodepath, self.__hrp5nlftarmlj2_nodepath, self.__hrp5nlftarmlj3_nodepath, self.__hrp5nlftarmlj4_nodepath, self.__hrp5nlftarmlj5_nodepath, self.__hrp5nlftarmlj6_nodepath, self.__hrp5nlftarmlj7_nodepath, self.hrp5nrobotlfthnd.handnp ], [self.__hrp5nbody_nodepath] ])