def __init__(self, ompath, dotnormplan=.95, dotnoarmovlp=.95, readser=False):
self.objtrimesh = None
# the sampled points and their normals
self.objsamplepnts = None
self.objsamplenrmls = None
# the sampled points (bad samples removed)
self.objsamplepnts_ref = None
self.objsamplenrmls_ref = None
# the sampled points (bad samples removed + clustered)
self.objsamplepnts_refcls = None
self.objsamplenrmls_refcls = None
# facets is used to avoid repeated computation
self.facets = None
# facetnormals is used to plot overlapped facets with different heights
self.facetnormals = None
# facet2dbdries saves the 2d boundaries of each facet
self.facet2dbdries = None
# the contactpairs are not index by facetids but by facetpairs [facet0, facet1]
self.facetpairs = None
self.gripcontactpairs = None
self.gripcontactpairnormals = None
self.gripcontactpairfacets = None
self.gripcontactrotangles = None
# for plot
self.facetcolorarray = None
self.counter = 0
self.pggen = pandageom.PandaGeomGen()
if readser is False:
self.loadObjModel(ompath, dotnormplan=dotnormplan, dotnoarmovlp=dotnoarmovlp)
self.saveSerialized("tmpnocp.pickle")
else:
self.loadSerialized("tmpnocp.pickle", ompath)
def __init__(self, rgthand = None, lfthand = None):
"""
load models
:param rgthand: hand object
:param lfthand: hand object
the two hands could be different
author: weiwei
date: 20171213
"""
##########
### load the model files of the robots
##########
this_dir, this_filename = os.path.split(__file__)
robotwaist_filepath = os.path.join(this_dir, "ur3stl", "ur3dualbody.stl")
ur3base_filepath = os.path.join(this_dir, "ur3stl", "base.stl")
ur3upperarm_filepath = os.path.join(this_dir, "ur3stl", "upperarm.stl")
ur3shoulder_filepath = os.path.join(this_dir, "ur3stl", "shoulder.stl")
ur3forearm_filepath = os.path.join(this_dir, "ur3stl", "forearm.stl")
ur3wrist1_filepath = os.path.join(this_dir, "ur3stl", "wrist1.stl")
ur3wrist2_filepath = os.path.join(this_dir, "ur3stl", "wrist2.stl")
ur3wrist3_filepath = os.path.join(this_dir, "ur3stl", "wrist3.stl")
robotwaist_model = pg.loadstlaspandanp_fn(robotwaist_filepath)
ur3base_model = pg.loadstlaspandanp_fn(ur3base_filepath)
ur3upperarm_model = pg.loadstlaspandanp_fn(ur3upperarm_filepath)
ur3shoulder_model = pg.loadstlaspandanp_fn(ur3shoulder_filepath)
ur3forearm_model = pg.loadstlaspandanp_fn(ur3forearm_filepath)
ur3wrist1_model = pg.loadstlaspandanp_fn(ur3wrist1_filepath)
ur3wrist2_model = pg.loadstlaspandanp_fn(ur3wrist2_filepath)
ur3wrist3_model = pg.loadstlaspandanp_fn(ur3wrist3_filepath)
self.__robotwaist_nodepath = NodePath("ur3dualbody")
robotwaist_model.instanceTo(self.__robotwaist_nodepath)
self.__robotwaist_nodepath.setColor(.3, .3, .3, 1)
# rgt
self.__ur3rgtbase_nodepath = NodePath("ur3rgtbase")
self.__ur3rgtshoulder_nodepath = NodePath("ur3rgtshoulder")
self.__ur3rgtupperarm_nodepath = NodePath("ur3rgtupperarm")
self.__ur3rgtforearm_nodepath = NodePath("ur3rgtforearm")
self.__ur3rgtwrist1_nodepath = NodePath("ur3rgtwrist1")
self.__ur3rgtwrist2_nodepath = NodePath("ur3rgtwrist2")
self.__ur3rgtwrist3_nodepath = NodePath("ur3rgtwrist3")
# base egg coordinates is retracted 89.2 along z to fit parameters
ur3base_model.instanceTo(self.__ur3rgtbase_nodepath)
self.__ur3rgtbase_nodepath.setColor(.5, .5, .5, 1)
ur3shoulder_model.instanceTo(self.__ur3rgtshoulder_nodepath)
self.__ur3rgtshoulder_nodepath.setColor(.5, .7, .3, 1)
ur3upperarm_model.instanceTo(self.__ur3rgtupperarm_nodepath)
self.__ur3rgtupperarm_nodepath.setColor(.5, .5, .5, 1)
ur3forearm_model.instanceTo(self.__ur3rgtforearm_nodepath)
self.__ur3rgtforearm_nodepath.setColor(.5, .5, .5, 1)
ur3wrist1_model.instanceTo(self.__ur3rgtwrist1_nodepath)
self.__ur3rgtwrist1_nodepath.setColor(.5, .7, .3, 1)
ur3wrist2_model.instanceTo(self.__ur3rgtwrist2_nodepath)
self.__ur3rgtwrist2_nodepath.setColor(.5, .5, .5, 1)
ur3wrist3_model.instanceTo(self.__ur3rgtwrist3_nodepath)
self.__ur3rgtwrist3_nodepath.setColor(.5, .5, .5, 1)
# lft
self.__ur3lftbase_nodepath = NodePath("ur3lftbase")
self.__ur3lftshoulder_nodepath = NodePath("ur3lftshoulder")
self.__ur3lftupperarm_nodepath = NodePath("ur3lftupperarm")
self.__ur3lftforearm_nodepath = NodePath("ur3lftforearm")
self.__ur3lftwrist1_nodepath = NodePath("ur3lftwrist1")
self.__ur3lftwrist2_nodepath = NodePath("ur3lftwrist2")
self.__ur3lftwrist3_nodepath = NodePath("ur3lftwrist3")
# base egg coordinates is retracted 89.2 along z to fit parameters
ur3base_model.instanceTo(self.__ur3lftbase_nodepath)
self.__ur3lftbase_nodepath.setColor(.5, .5, .5, 1)
ur3shoulder_model.instanceTo(self.__ur3lftshoulder_nodepath)
self.__ur3lftshoulder_nodepath.setColor(.5, .7, .3, 1)
ur3upperarm_model.instanceTo(self.__ur3lftupperarm_nodepath)
self.__ur3lftupperarm_nodepath.setColor(.5, .5, .5, 1)
ur3forearm_model.instanceTo(self.__ur3lftforearm_nodepath)
self.__ur3lftforearm_nodepath.setColor(.5, .5, .5, 1)
ur3wrist1_model.instanceTo(self.__ur3lftwrist1_nodepath)
self.__ur3lftwrist1_nodepath.setColor(.5, .7, .3, 1)
ur3wrist2_model.instanceTo(self.__ur3lftwrist2_nodepath)
self.__ur3lftwrist2_nodepath.setColor(.5, .5, .5, 1)
ur3wrist3_model.instanceTo(self.__ur3lftwrist3_nodepath)
self.__ur3lftwrist3_nodepath.setColor(.5, .5, .5, 1)
##########
### load the model files of sticks
##########
self.pggen = pg.PandaGeomGen()
# hand
self.rgthnd = rgthand
self.lfthnd = lfthand
glfgrts)
lfgrcollidernp = bullcldrnp.attachNewNode(lfgrbullnode)
base.world.attachRigidBody(lfgrbullnode)
lfgrcollidernp.setCollideMask(BitMask32.allOn())
# rtq85hnd.rtq85np.find("**/rtq85lfgr").showTightBounds()
# rtq85hnd.rtq85np.showTightBounds()
base.taskMgr.add(updateworld,
"updateworld",
extraArgs=[base.world],
appendTask=True)
result = base.world.contactTestPair(ilkbullnode, lftbullnode)
print result
print result.getContacts()
import pandaplotutils.pandageom as pandageom
pggen = pandageom.PandaGeomGen()
for contact in result.getContacts():
cp = contact.getManifoldPoint()
print cp.getLocalPointA()
pggen.plotSphere(base,
pos=cp.getLocalPointA(),
radius=10,
rgba=Vec4(1, 0, 0, 1))
pggen.plotAxisSelf(base.render, spos=Vec3(0, 0, 0))
debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(False)
debugNode.showNormals(False)
import manipulation.grip.robotiq85.rtq85 as rtq85
import ur3dualball
base = pandactrl.World(camp=[3000, 0, 3000], lookatp=[0, 0, 700])
ur3dualrobot = Ur3DualRobot()
ur3dualrobot2 = Ur3DualRobot(position=np.array([1000, 0, 0]),
rotmat=rm.rodrigues([0, 0, 1], 170))
ur3dualrobot.goinitpose()
ur3dualrobot2.goinitpose()
# ur3dualrobot.gozeropose()
rgthnd = rtq85.Rtq85()
lfthnd = rtq85.Rtq85()
ur3mg = ur3dualmesh.Ur3DualMesh(rgthand=rgthnd, lfthand=lfthnd)
pgg = pandageom.PandaGeomGen()
# ur3stick = ur3mg.gensnp(ur3dualrobot)
# ur3stick.reparentTo(base.render)
#
# # from manipulation.grip.hrp5three import hrp5threenm
ur3dualmnp = ur3mg.genmnp(ur3dualrobot, togglejntscoord=False)
ur3dualmnp2 = ur3mg.genmnp(ur3dualrobot2, togglejntscoord=False)
ur3dualmnp.reparentTo(base.render)
ur3dualmnp2.reparentTo(base.render)
# ur3dualrobot.gozeropose()
# ur3dualmnp = ur3mg.genmnp(ur3dualrobot, togglejntscoord=True)
# ur3dualmnp.reparentTo(base.render)
pgg.plotAxis(base.render, Vec3(0, 0, 0))
ur3dualrobotball = ur3dualball.Ur3DualBall()
bcndict = ur3dualrobotball.genfullbcndict(ur3dualrobot)
ur3dualrobotball.showbcn(base, bcndict)
def __init__(self, rgthand=None, lfthand=None):
"""
load models
:param rgthand: hand object
:param lfthand: hand object
the two hands could be different
author: weiwei
date: 20180109
"""
##########
### load the model files of the robots
##########
this_dir, this_filename = os.path.split(__file__)
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"))
self.__nxtwaist_nodepath = NodePath("nxtwaist")
self.__nxtbody_nodepath = NodePath("nxtbody")
self.__nxthead_nodepath = NodePath("nxthead")
nxtwaist_model = loader.loadModel(nxtwaist_filepath)
nxtbody_model = loader.loadModel(nxtbody_filepath)
nxthead_model = loader.loadModel(nxthead_filepath)
nxtwaist_model.instanceTo(self.__nxtwaist_nodepath)
nxtbody_model.instanceTo(self.__nxtbody_nodepath)
nxthead_model.instanceTo(self.__nxthead_nodepath)
self.__nxtbody_nodepath.reparentTo(self.__nxtwaist_nodepath)
self.__nxthead_nodepath.setZ(569.5)
self.__nxthead_nodepath.reparentTo(self.__nxtwaist_nodepath)
# rgtarm
nxtrgtarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj0.egg"))
nxtrgtarmlj0_model = loader.loadModel(nxtrgtarmlj0_filepath)
self.__nxtrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
nxtrgtarmlj0_model.instanceTo(self.__nxtrgtarmlj0_nodepath)
nxtrgtarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj1.egg"))
nxtrgtarmlj1_model = loader.loadModel(nxtrgtarmlj1_filepath)
self.__nxtrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
nxtrgtarmlj1_model.instanceTo(self.__nxtrgtarmlj1_nodepath)
nxtrgtarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj2.egg"))
nxtrgtarmlj2_model = loader.loadModel(nxtrgtarmlj2_filepath)
self.__nxtrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
nxtrgtarmlj2_model.instanceTo(self.__nxtrgtarmlj2_nodepath)
nxtrgtarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj3.egg"))
nxtrgtarmlj3_model = loader.loadModel(nxtrgtarmlj3_filepath)
self.__nxtrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
nxtrgtarmlj3_model.instanceTo(self.__nxtrgtarmlj3_nodepath)
nxtrgtarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj4.egg"))
nxtrgtarmlj4_model = loader.loadModel(nxtrgtarmlj4_filepath)
self.__nxtrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
nxtrgtarmlj4_model.instanceTo(self.__nxtrgtarmlj4_nodepath)
# lftarm
nxtlftarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj0.egg"))
nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath)
self.__nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath")
nxtlftarmlj0_model.instanceTo(self.__nxtlftarmlj0_nodepath)
nxtlftarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj1.egg"))
nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath)
self.__nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath")
nxtlftarmlj1_model.instanceTo(self.__nxtlftarmlj1_nodepath)
nxtlftarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj2.egg"))
nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath)
self.__nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath")
nxtlftarmlj2_model.instanceTo(self.__nxtlftarmlj2_nodepath)
nxtlftarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj3.egg"))
nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath)
self.__nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath")
nxtlftarmlj3_model.instanceTo(self.__nxtlftarmlj3_nodepath)
nxtlftarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj4.egg"))
nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath)
self.__nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath")
nxtlftarmlj4_model.instanceTo(self.__nxtlftarmlj4_nodepath)
# rgthnd
self.rgthnd = rgthand
# lfthnd
self.lfthnd = lfthand
##########
### load the model files of sticks
##########
self.pggen = pg.PandaGeomGen()
def __init__(self, handpkg):
"""
load models
:param handpkg:
author: weiwei
date: 20180109
"""
self.__nxtmnp = NodePath("nxtmesh")
##########
### load the model files of the robots
##########
this_dir, this_filename = os.path.split(__file__)
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"))
self.__nxtwaist_nodepath = NodePath("nxtwaist")
self.__nxtbody_nodepath = NodePath("nxtbody")
self.__nxthead_nodepath = NodePath("nxthead")
nxtwaist_model = loader.loadModel(nxtwaist_filepath)
nxtbody_model = loader.loadModel(nxtbody_filepath)
nxthead_model = loader.loadModel(nxthead_filepath)
nxtwaist_model.instanceTo(self.__nxtwaist_nodepath)
nxtbody_model.instanceTo(self.__nxtbody_nodepath)
nxthead_model.instanceTo(self.__nxthead_nodepath)
self.__nxtwaist_nodepath.reparentTo(self.__nxtmnp)
self.__nxtbody_nodepath.reparentTo(self.__nxtwaist_nodepath)
self.__nxthead_nodepath.setZ(569.5)
self.__nxthead_nodepath.reparentTo(self.__nxtwaist_nodepath)
# rgtarm
nxtrgtarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj0.egg"))
nxtrgtarmlj0_model = loader.loadModel(nxtrgtarmlj0_filepath)
self.__nxtrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
nxtrgtarmlj0_model.instanceTo(self.__nxtrgtarmlj0_nodepath)
self.__nxtrgtarmlj0_nodepath.reparentTo(self.__nxtmnp)
nxtrgtarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj1.egg"))
nxtrgtarmlj1_model = loader.loadModel(nxtrgtarmlj1_filepath)
self.__nxtrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
nxtrgtarmlj1_model.instanceTo(self.__nxtrgtarmlj1_nodepath)
self.__nxtrgtarmlj1_nodepath.reparentTo(self.__nxtmnp)
nxtrgtarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj2.egg"))
nxtrgtarmlj2_model = loader.loadModel(nxtrgtarmlj2_filepath)
self.__nxtrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
nxtrgtarmlj2_model.instanceTo(self.__nxtrgtarmlj2_nodepath)
self.__nxtrgtarmlj2_nodepath.reparentTo(self.__nxtmnp)
nxtrgtarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj3.egg"))
nxtrgtarmlj3_model = loader.loadModel(nxtrgtarmlj3_filepath)
self.__nxtrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
nxtrgtarmlj3_model.instanceTo(self.__nxtrgtarmlj3_nodepath)
self.__nxtrgtarmlj3_nodepath.reparentTo(self.__nxtmnp)
nxtrgtarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_rgtarm_lj4.egg"))
nxtrgtarmlj4_model = loader.loadModel(nxtrgtarmlj4_filepath)
self.__nxtrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
nxtrgtarmlj4_model.instanceTo(self.__nxtrgtarmlj4_nodepath)
self.__nxtrgtarmlj4_nodepath.reparentTo(self.__nxtmnp)
# lftarm
nxtlftarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj0.egg"))
nxtlftarmlj0_model = loader.loadModel(nxtlftarmlj0_filepath)
self.__nxtlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath")
nxtlftarmlj0_model.instanceTo(self.__nxtlftarmlj0_nodepath)
self.__nxtlftarmlj0_nodepath.reparentTo(self.__nxtmnp)
nxtlftarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj1.egg"))
nxtlftarmlj1_model = loader.loadModel(nxtlftarmlj1_filepath)
self.__nxtlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath")
nxtlftarmlj1_model.instanceTo(self.__nxtlftarmlj1_nodepath)
self.__nxtlftarmlj1_nodepath.reparentTo(self.__nxtmnp)
nxtlftarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj2.egg"))
nxtlftarmlj2_model = loader.loadModel(nxtlftarmlj2_filepath)
self.__nxtlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath")
nxtlftarmlj2_model.instanceTo(self.__nxtlftarmlj2_nodepath)
self.__nxtlftarmlj2_nodepath.reparentTo(self.__nxtmnp)
nxtlftarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj3.egg"))
nxtlftarmlj3_model = loader.loadModel(nxtlftarmlj3_filepath)
self.__nxtlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath")
nxtlftarmlj3_model.instanceTo(self.__nxtlftarmlj3_nodepath)
self.__nxtlftarmlj3_nodepath.reparentTo(self.__nxtmnp)
nxtlftarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "nxtegg", "nxt_lftarm_lj4.egg"))
nxtlftarmlj4_model = loader.loadModel(nxtlftarmlj4_filepath)
self.__nxtlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath")
nxtlftarmlj4_model.instanceTo(self.__nxtlftarmlj4_nodepath)
self.__nxtlftarmlj4_nodepath.reparentTo(self.__nxtmnp)
# rgthnd
self.nxtrgthnd = handpkg.newHand('rgt')
self.nxtrgthnd.reparentTo(self.__nxtmnp)
# lfthnd
self.nxtlfthnd = handpkg.newHand('lft')
self.nxtlfthnd.reparentTo(self.__nxtmnp)
##########
### load the model files of sticks
##########
self.pggen = pg.PandaGeomGen()
def __init__(self, handpkg):
"""
load models
:param handpkg:
author: weiwei
date: 20170612
"""
self.__hrp5nmnp = NodePath("hrp5nmnp")
##########
### load the model files of the robots
##########
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",
"HRP-5P_ConceptDesign_Body.egg"))
hrp5nchest0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Chest_Link0.egg"))
hrp5nchest1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Chest_Link1.egg"))
hrp5nchest2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Chest_Link2.egg"))
hrp5nhead0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Head_Link0.egg"))
hrp5nhead1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"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)
self.__hrp5nbody_nodepath = NodePath("hrp5nbody")
self.__hrp5nchest0_nodepath = NodePath("hrp5nchest0")
self.__hrp5nchest1_nodepath = NodePath("hrp5nchest1")
self.__hrp5nchest2_nodepath = NodePath("hrp5nchest2")
self.__hrp5nhead0_nodepath = NodePath("hrp5nhead0")
self.__hrp5nhead1_nodepath = NodePath("hrp5nhead1")
hrp5nbody_model.instanceTo(self.__hrp5nbody_nodepath)
hrp5nchest0_model.instanceTo(self.__hrp5nchest0_nodepath)
hrp5nchest1_model.instanceTo(self.__hrp5nchest1_nodepath)
hrp5nchest2_model.instanceTo(self.__hrp5nchest2_nodepath)
hrp5nhead0_model.instanceTo(self.__hrp5nhead0_nodepath)
hrp5nhead1_model.instanceTo(self.__hrp5nhead1_nodepath)
self.__hrp5nbody_nodepath.reparentTo(self.__hrp5nmnp)
self.__hrp5nchest0_nodepath.reparentTo(self.__hrp5nbody_nodepath)
self.__hrp5nchest1_nodepath.reparentTo(self.__hrp5nchest0_nodepath)
self.__hrp5nchest2_nodepath.reparentTo(self.__hrp5nchest1_nodepath)
self.__hrp5nhead0_nodepath.reparentTo(self.__hrp5nchest2_nodepath)
self.__hrp5nhead1_nodepath.reparentTo(self.__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)
self.__hrp5nrgtarmlj0_nodepath = NodePath("nxtrgtarmlj0_nodepath")
hrp5nrgtarmlj0_model.instanceTo(self.__hrp5nrgtarmlj0_nodepath)
self.__hrp5nrgtarmlj0_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nrgtarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Rarm_Link0.egg"))
hrp5nrgtarmlj1_model = loader.loadModel(hrp5nrgtarmlj1_filepath)
self.__hrp5nrgtarmlj1_nodepath = NodePath("nxtrgtarmlj1_nodepath")
hrp5nrgtarmlj1_model.instanceTo(self.__hrp5nrgtarmlj1_nodepath)
self.__hrp5nrgtarmlj1_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nrgtarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Rarm_Link1.egg"))
hrp5nrgtarmlj2_model = loader.loadModel(hrp5nrgtarmlj2_filepath)
self.__hrp5nrgtarmlj2_nodepath = NodePath("nxtrgtarmlj2_nodepath")
hrp5nrgtarmlj2_model.instanceTo(self.__hrp5nrgtarmlj2_nodepath)
self.__hrp5nrgtarmlj2_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nrgtarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Rarm_Link2.egg"))
hrp5nrgtarmlj3_model = loader.loadModel(hrp5nrgtarmlj3_filepath)
self.__hrp5nrgtarmlj3_nodepath = NodePath("nxtrgtarmlj3_nodepath")
hrp5nrgtarmlj3_model.instanceTo(self.__hrp5nrgtarmlj3_nodepath)
self.__hrp5nrgtarmlj3_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nrgtarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Rarm_Link3.egg"))
hrp5nrgtarmlj4_model = loader.loadModel(hrp5nrgtarmlj4_filepath)
self.__hrp5nrgtarmlj4_nodepath = NodePath("nxtrgtarmlj4_nodepath")
hrp5nrgtarmlj4_model.instanceTo(self.__hrp5nrgtarmlj4_nodepath)
self.__hrp5nrgtarmlj4_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nrgtarmlj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Rarm_Link4.egg"))
hrp5nrgtarmlj5_model = loader.loadModel(hrp5nrgtarmlj5_filepath)
self.__hrp5nrgtarmlj5_nodepath = NodePath("nxtrgtarmlj5_nodepath")
hrp5nrgtarmlj5_model.instanceTo(self.__hrp5nrgtarmlj5_nodepath)
self.__hrp5nrgtarmlj5_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nrgtarmlj6_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Rarm_Link5.egg"))
hrp5nrgtarmlj6_model = loader.loadModel(hrp5nrgtarmlj6_filepath)
self.__hrp5nrgtarmlj6_nodepath = NodePath("nxtrgtarmlj6_nodepath")
hrp5nrgtarmlj6_model.instanceTo(self.__hrp5nrgtarmlj6_nodepath)
self.__hrp5nrgtarmlj6_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nrgtarmlj7_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Rarm_Link6.egg"))
hrp5nrgtarmlj7_model = loader.loadModel(hrp5nrgtarmlj7_filepath)
self.__hrp5nrgtarmlj7_nodepath = NodePath("nxtrgtarmlj7_nodepath")
hrp5nrgtarmlj7_model.instanceTo(self.__hrp5nrgtarmlj7_nodepath)
self.__hrp5nrgtarmlj7_nodepath.reparentTo(self.__hrp5nmnp)
# lftarm
hrp5nlftarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Lscap_Link0.egg"))
hrp5nlftarmlj0_model = loader.loadModel(hrp5nlftarmlj0_filepath)
self.__hrp5nlftarmlj0_nodepath = NodePath("nxtlftarmlj0_nodepath")
hrp5nlftarmlj0_model.instanceTo(self.__hrp5nlftarmlj0_nodepath)
self.__hrp5nlftarmlj0_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nlftarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Larm_Link0.egg"))
hrp5nlftarmlj1_model = loader.loadModel(hrp5nlftarmlj1_filepath)
self.__hrp5nlftarmlj1_nodepath = NodePath("nxtlftarmlj1_nodepath")
hrp5nlftarmlj1_model.instanceTo(self.__hrp5nlftarmlj1_nodepath)
self.__hrp5nlftarmlj1_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nlftarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Larm_Link1.egg"))
hrp5nlftarmlj2_model = loader.loadModel(hrp5nlftarmlj2_filepath)
self.__hrp5nlftarmlj2_nodepath = NodePath("nxtlftarmlj2_nodepath")
hrp5nlftarmlj2_model.instanceTo(self.__hrp5nlftarmlj2_nodepath)
self.__hrp5nlftarmlj2_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nlftarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Larm_Link2.egg"))
hrp5nlftarmlj3_model = loader.loadModel(hrp5nlftarmlj3_filepath)
self.__hrp5nlftarmlj3_nodepath = NodePath("nxtlftarmlj3_nodepath")
hrp5nlftarmlj3_model.instanceTo(self.__hrp5nlftarmlj3_nodepath)
self.__hrp5nlftarmlj3_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nlftarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Larm_Link3.egg"))
hrp5nlftarmlj4_model = loader.loadModel(hrp5nlftarmlj4_filepath)
self.__hrp5nlftarmlj4_nodepath = NodePath("nxtlftarmlj4_nodepath")
hrp5nlftarmlj4_model.instanceTo(self.__hrp5nlftarmlj4_nodepath)
self.__hrp5nlftarmlj4_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nlftarmlj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Larm_Link4.egg"))
hrp5nlftarmlj5_model = loader.loadModel(hrp5nlftarmlj5_filepath)
self.__hrp5nlftarmlj5_nodepath = NodePath("nxtlftarmlj5_nodepath")
hrp5nlftarmlj5_model.instanceTo(self.__hrp5nlftarmlj5_nodepath)
self.__hrp5nlftarmlj5_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nlftarmlj6_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Larm_Link5.egg"))
hrp5nlftarmlj6_model = loader.loadModel(hrp5nlftarmlj6_filepath)
self.__hrp5nlftarmlj6_nodepath = NodePath("nxtlftarmlj6_nodepath")
hrp5nlftarmlj6_model.instanceTo(self.__hrp5nlftarmlj6_nodepath)
self.__hrp5nlftarmlj6_nodepath.reparentTo(self.__hrp5nmnp)
#
hrp5nlftarmlj7_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5negg",
"HRP-5P_ConceptDesign_Larm_Link6.egg"))
hrp5nlftarmlj7_model = loader.loadModel(hrp5nlftarmlj7_filepath)
self.__hrp5nlftarmlj7_nodepath = NodePath("nxtlftarmlj7_nodepath")
hrp5nlftarmlj7_model.instanceTo(self.__hrp5nlftarmlj7_nodepath)
self.__hrp5nlftarmlj7_nodepath.reparentTo(self.__hrp5nmnp)
# rgthnd
self.hrp5nrobotrgthnd = handpkg.newHand('rgt')
self.hrp5nrobotrgthnd.reparentTo(self.__hrp5nmnp)
# lfthnd
self.hrp5nrobotlfthnd = handpkg.newHand('lft')
self.hrp5nrobotlfthnd.reparentTo(self.__hrp5nmnp)
##########
### load the model files of sticks
##########
self.pggen = pg.PandaGeomGen()
import numpy as np import pandaplotutils.pandactrl as pc import pandaplotutils.pandageom as pg import utils.robotmath as rm import trimesh import os from panda3d.core import Filename, TransparencyAttrib base = pc.World(camp=[1000, -300, 700], lookatp=[0, 10, 0], w=1000, h=1000) pggen = pg.PandaGeomGen() pggen.plotAxis(base.render) this_dir, this_filename = os.path.split(__file__) objpath = os.path.join(this_dir, "sandpart.stl") # objtrimesh=trimesh.load_mesh(objpath) # # # objmnp = pg.trimesh2Panda(objtrimesh) objmnp = pg.genObjmnp(objpath, color=(0, 0, 0, 1)) # # objmnp.reparentTo(base.render) # # eggpath = Filename.fromOsSpecific(os.path.join(this_dir, "bzcube.egg")) # objmnp2 = loader.loadModel(eggpath) # objmnp2.setScale(5.0) # objmnp2.setColor(0,0,1, 0.7) # objmnp2.setTransparency(TransparencyAttrib.MAlpha) # objmnp2.reparentTo(base.render) base.run()
def __init__(self, rgthand=None, lfthand=None):
"""
load models
:param handpkg:
author: weiwei
date: 20180203
"""
##########
### load the model files of the robots
##########
this_dir, this_filename = os.path.split(__file__)
# chest0-2, head1 (neck is not plotted)
hrp5pbody_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "base.egg"))
hrp5pchest0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "chest0.egg"))
hrp5pchest1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "chest1.egg"))
hrp5pchest2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "chest2.egg"))
hrp5phead0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "head0.egg"))
hrp5phead1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "head1.egg"))
hrp5pbody_model = loader.loadModel(hrp5pbody_filepath)
hrp5pchest0_model = loader.loadModel(hrp5pchest0_filepath)
hrp5pchest1_model = loader.loadModel(hrp5pchest1_filepath)
hrp5pchest2_model = loader.loadModel(hrp5pchest2_filepath)
hrp5phead0_model = loader.loadModel(hrp5phead0_filepath)
hrp5phead1_model = loader.loadModel(hrp5phead1_filepath)
self.__hrp5pbody_nodepath = NodePath("hrp5pbody")
self.__hrp5pchest0_nodepath = NodePath("hrp5pchest0")
self.__hrp5pchest1_nodepath = NodePath("hrp5pchest1")
self.__hrp5pchest2_nodepath = NodePath("hrp5pchest2")
self.__hrp5phead0_nodepath = NodePath("hrp5phead0")
self.__hrp5phead1_nodepath = NodePath("hrp5phead1")
hrp5pbody_model.instanceTo(self.__hrp5pbody_nodepath)
hrp5pchest0_model.instanceTo(self.__hrp5pchest0_nodepath)
hrp5pchest1_model.instanceTo(self.__hrp5pchest1_nodepath)
hrp5pchest2_model.instanceTo(self.__hrp5pchest2_nodepath)
hrp5phead0_model.instanceTo(self.__hrp5phead0_nodepath)
hrp5phead1_model.instanceTo(self.__hrp5phead1_nodepath)
self.__hrp5pchest0_nodepath.reparentTo(self.__hrp5pbody_nodepath)
self.__hrp5pchest0_nodepath.setPos(55.0, 0.0, 274.0)
self.__hrp5pchest1_nodepath.reparentTo(self.__hrp5pchest0_nodepath)
self.__hrp5pchest2_nodepath.reparentTo(self.__hrp5pchest1_nodepath)
self.__hrp5phead0_nodepath.reparentTo(self.__hrp5pchest2_nodepath)
self.__hrp5phead0_nodepath.setPos(32.0, 0.0, 549.0)
self.__hrp5phead1_nodepath.reparentTo(self.__hrp5phead0_nodepath)
# rgtarm
hrp5prgtarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rscap.egg"))
hrp5prgtarmlj0_model = loader.loadModel(hrp5prgtarmlj0_filepath)
self.__hrp5prgtarmlj0_nodepath = NodePath("rscap")
hrp5prgtarmlj0_model.instanceTo(self.__hrp5prgtarmlj0_nodepath)
#
hrp5prgtarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rlink0.egg"))
hrp5prgtarmlj1_model = loader.loadModel(hrp5prgtarmlj1_filepath)
self.__hrp5prgtarmlj1_nodepath = NodePath("rlink0")
hrp5prgtarmlj1_model.instanceTo(self.__hrp5prgtarmlj1_nodepath)
#
hrp5prgtarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rlink1.egg"))
hrp5prgtarmlj2_model = loader.loadModel(hrp5prgtarmlj2_filepath)
self.__hrp5prgtarmlj2_nodepath = NodePath("rlink1")
hrp5prgtarmlj2_model.instanceTo(self.__hrp5prgtarmlj2_nodepath)
#
hrp5prgtarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rlink2.egg"))
hrp5prgtarmlj3_model = loader.loadModel(hrp5prgtarmlj3_filepath)
self.__hrp5prgtarmlj3_nodepath = NodePath("rlink2")
hrp5prgtarmlj3_model.instanceTo(self.__hrp5prgtarmlj3_nodepath)
#
hrp5prgtarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rlink3.egg"))
hrp5prgtarmlj4_model = loader.loadModel(hrp5prgtarmlj4_filepath)
self.__hrp5prgtarmlj4_nodepath = NodePath("rlink3")
hrp5prgtarmlj4_model.instanceTo(self.__hrp5prgtarmlj4_nodepath)
#
hrp5prgtarmlj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rlink4.egg"))
hrp5prgtarmlj5_model = loader.loadModel(hrp5prgtarmlj5_filepath)
self.__hrp5prgtarmlj5_nodepath = NodePath("rlink4")
hrp5prgtarmlj5_model.instanceTo(self.__hrp5prgtarmlj5_nodepath)
#
hrp5prgtarmlj6_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rlink5.egg"))
hrp5prgtarmlj6_model = loader.loadModel(hrp5prgtarmlj6_filepath)
self.__hrp5prgtarmlj6_nodepath = NodePath("rlink5")
hrp5prgtarmlj6_model.instanceTo(self.__hrp5prgtarmlj6_nodepath)
#
hrp5prgtarmlj7_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rlink6.egg"))
hrp5prgtarmlj7_model = loader.loadModel(hrp5prgtarmlj7_filepath)
self.__hrp5prgtarmlj7_nodepath = NodePath("rlink6")
hrp5prgtarmlj7_model.instanceTo(self.__hrp5prgtarmlj7_nodepath)
# lftarm
hrp5plftarmlj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "lscap.egg"))
hrp5plftarmlj0_model = loader.loadModel(hrp5plftarmlj0_filepath)
self.__hrp5plftarmlj0_nodepath = NodePath("lscap")
hrp5plftarmlj0_model.instanceTo(self.__hrp5plftarmlj0_nodepath)
#
hrp5plftarmlj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "llink0.egg"))
hrp5plftarmlj1_model = loader.loadModel(hrp5plftarmlj1_filepath)
self.__hrp5plftarmlj1_nodepath = NodePath("llink0")
hrp5plftarmlj1_model.instanceTo(self.__hrp5plftarmlj1_nodepath)
#
hrp5plftarmlj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "llink1.egg"))
hrp5plftarmlj2_model = loader.loadModel(hrp5plftarmlj2_filepath)
self.__hrp5plftarmlj2_nodepath = NodePath("llink1")
hrp5plftarmlj2_model.instanceTo(self.__hrp5plftarmlj2_nodepath)
#
hrp5plftarmlj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "llink2.egg"))
hrp5plftarmlj3_model = loader.loadModel(hrp5plftarmlj3_filepath)
self.__hrp5plftarmlj3_nodepath = NodePath("llink2")
hrp5plftarmlj3_model.instanceTo(self.__hrp5plftarmlj3_nodepath)
#
hrp5plftarmlj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "llink3.egg"))
hrp5plftarmlj4_model = loader.loadModel(hrp5plftarmlj4_filepath)
self.__hrp5plftarmlj4_nodepath = NodePath("llink3")
hrp5plftarmlj4_model.instanceTo(self.__hrp5plftarmlj4_nodepath)
#
hrp5plftarmlj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "llink4.egg"))
hrp5plftarmlj5_model = loader.loadModel(hrp5plftarmlj5_filepath)
self.__hrp5plftarmlj5_nodepath = NodePath("llink4")
hrp5plftarmlj5_model.instanceTo(self.__hrp5plftarmlj5_nodepath)
#
hrp5plftarmlj6_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "llink5.egg"))
hrp5plftarmlj6_model = loader.loadModel(hrp5plftarmlj6_filepath)
self.__hrp5plftarmlj6_nodepath = NodePath("llink5")
hrp5plftarmlj6_model.instanceTo(self.__hrp5plftarmlj6_nodepath)
#
hrp5plftarmlj7_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "llink6.egg"))
hrp5plftarmlj7_model = loader.loadModel(hrp5plftarmlj7_filepath)
self.__hrp5plftarmlj7_nodepath = NodePath("llink6")
hrp5plftarmlj7_model.instanceTo(self.__hrp5plftarmlj7_nodepath)
# hand
self.rgthnd = rgthand
self.lfthnd = lfthand
# rgtleg
hrp5prgtleglj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rleg0.egg"))
hrp5prgtleglj0_model = loader.loadModel(hrp5prgtleglj0_filepath)
self.__hrp5prgtleglj0_nodepath = NodePath("rleg0")
hrp5prgtleglj0_model.instanceTo(self.__hrp5prgtleglj0_nodepath)
#
hrp5prgtleglj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rleg1.egg"))
hrp5prgtleglj1_model = loader.loadModel(hrp5prgtleglj1_filepath)
self.__hrp5prgtleglj1_nodepath = NodePath("rleg1")
hrp5prgtleglj1_model.instanceTo(self.__hrp5prgtleglj1_nodepath)
#
hrp5prgtleglj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rleg2.egg"))
hrp5prgtleglj2_model = loader.loadModel(hrp5prgtleglj2_filepath)
self.__hrp5prgtleglj2_nodepath = NodePath("rleg2")
hrp5prgtleglj2_model.instanceTo(self.__hrp5prgtleglj2_nodepath)
#
hrp5prgtleglj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rleg3.egg"))
hrp5prgtleglj3_model = loader.loadModel(hrp5prgtleglj3_filepath)
self.__hrp5prgtleglj3_nodepath = NodePath("rleg3")
hrp5prgtleglj3_model.instanceTo(self.__hrp5prgtleglj3_nodepath)
#
hrp5prgtleglj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rleg4.egg"))
hrp5prgtleglj4_model = loader.loadModel(hrp5prgtleglj4_filepath)
self.__hrp5prgtleglj4_nodepath = NodePath("rleg4")
hrp5prgtleglj4_model.instanceTo(self.__hrp5prgtleglj4_nodepath)
#
hrp5prgtleglj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "rleg5.egg"))
hrp5prgtleglj5_model = loader.loadModel(hrp5prgtleglj5_filepath)
self.__hrp5prgtleglj5_nodepath = NodePath("rleg5")
hrp5prgtleglj5_model.instanceTo(self.__hrp5prgtleglj5_nodepath)
# lftleg
hrp5plftleglj0_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "lleg0.egg"))
hrp5plftleglj0_model = loader.loadModel(hrp5plftleglj0_filepath)
self.__hrp5plftleglj0_nodepath = NodePath("lleg0")
hrp5plftleglj0_model.instanceTo(self.__hrp5plftleglj0_nodepath)
#
hrp5plftleglj1_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "lleg1.egg"))
hrp5plftleglj1_model = loader.loadModel(hrp5plftleglj1_filepath)
self.__hrp5plftleglj1_nodepath = NodePath("lleg1")
hrp5plftleglj1_model.instanceTo(self.__hrp5plftleglj1_nodepath)
#
hrp5plftleglj2_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "lleg2.egg"))
hrp5plftleglj2_model = loader.loadModel(hrp5plftleglj2_filepath)
self.__hrp5plftleglj2_nodepath = NodePath("lleg2")
hrp5plftleglj2_model.instanceTo(self.__hrp5plftleglj2_nodepath)
#
hrp5plftleglj3_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "lleg3.egg"))
hrp5plftleglj3_model = loader.loadModel(hrp5plftleglj3_filepath)
self.__hrp5plftleglj3_nodepath = NodePath("lleg3")
hrp5plftleglj3_model.instanceTo(self.__hrp5plftleglj3_nodepath)
#
hrp5plftleglj4_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "lleg4.egg"))
hrp5plftleglj4_model = loader.loadModel(hrp5plftleglj4_filepath)
self.__hrp5plftleglj4_nodepath = NodePath("lleg4")
hrp5plftleglj4_model.instanceTo(self.__hrp5plftleglj4_nodepath)
#
hrp5plftleglj5_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "hrp5pegg", "lleg5.egg"))
hrp5plftleglj5_model = loader.loadModel(hrp5plftleglj5_filepath)
self.__hrp5plftleglj5_nodepath = NodePath("lleg5")
hrp5plftleglj5_model.instanceTo(self.__hrp5plftleglj5_nodepath)
##########
### load the model files of sticks
##########
self.pggen = pg.PandaGeomGen()
def __init__(self, base, odespace, odeworld, fgrlen = 50.0, handwidth = 20.0):
self.base = base
self.pggen = pg.PandaGeomGen()
self.handnp = NodePath("hand")
self.handlen = fgrlen
self.handwidth = handwidth
self.odespace = odespace
self.odeworld = odeworld
# palm, no geom
self.palmbody = OdeBody(self.odeworld)
palmmass = OdeMass()
palmmass.setSphere(.2,1)
self.palmbody.setMass(palmmass)
self.palmgeom = self.pggen.genBox(10.0, 10.0, 2.0)
self.palmgeom.reparentTo(self.base.render)
# linear joint between palm and environment
self.palmenvjnt = OdeSliderJoint(self.odeworld)
self.palmenvjnt.attachBody(self.palmbody, 0)
self.palmenvjnt.setAxis(0,0,-1)
self.palmenvjnt.setParamVel(0)
self.palmenvjnt.setParamFMax(10000000)
# fgr0
self.fgr0body = OdeBody(self.odeworld)
fgr0mass = OdeMass()
fgr0mass.setSphere(.1,1)
self.fgr0body.setMass(palmmass)
self.fgr0geom = self.pggen.genBox(10.0, 2.0, self.handlen)
self.fgr0geom.reparentTo(self.base.render)
fgr0trimesh = OdeTriMeshData(self.fgr0geom, True)
fgr0geom = OdeTriMeshGeom(self.odespace, fgr0trimesh)
fgr0geom.setBody(self.fgr0body)
# linear joint between fgr0 and palm
self.fgr0palmjnt = OdeSliderJoint(self.odeworld)
self.fgr0palmjnt.attach(self.fgr0body, self.palmbody)
self.fgr0palmjnt.setAxis(0,1,0)
self.fgr0palmjnt.setParamVel(0)
self.fgr0palmjnt.setParamFMax(10000000)
# fgr1
self.fgr1body = OdeBody(self.odeworld)
fgr1mass = OdeMass()
fgr1mass.setSphere(.1,1)
self.fgr1body.setMass(palmmass)
self.fgr1geom = self.pggen.genBox(10.0, 2.0, self.handlen)
self.fgr1geom.reparentTo(self.base.render)
fgr1trimesh = OdeTriMeshData(self.fgr1geom, True)
fgr1geom = OdeTriMeshGeom(self.odespace, fgr1trimesh)
fgr1geom.setBody(self.fgr1body)
# linear joint between fgr1 and palm
self.fgr1palmjnt = OdeSliderJoint(self.odeworld)
self.fgr1palmjnt.attach(self.fgr1body, self.palmbody)
self.fgr1palmjnt.setAxis(0,-1,0)
self.fgr1palmjnt.setParamVel(0)
self.fgr1palmjnt.setParamFMax(10000000)
self.bopenhand = False
self.bclosehand = False
self.palmbody.setPosition(0,0,1000.0)
self.fgr0body.setPosition(0,0,1000.0)
self.fgr1body.setPosition(0,0,1000.0)
self.update()
self.resetgrip()
def __init__(self, camp=[2000,500,2000], lookatp=[0,0,250], up = [0,0,1], fov = 40, w = 2000, h = 1500):
"""
:param camp:
:param lookatp:
:param fov:
:param w: width of window
:param h: height of window
"""
super(World, self).__init__()
self.setBackgroundColor(1, 1, 1)
# set up lens
lens = PerspectiveLens()
lens.setFov(fov)
lens.setNearFar(1, 50000)
self.disableMouse()
self.cam.setPos(camp[0], camp[1], camp[2])
self.cam.lookAt(Point3(lookatp[0], lookatp[1], lookatp[2]), Vec3(up[0], up[1], up[2]))
self.cam.node().setLens(lens)
# set up slight
ablight = AmbientLight("ambientlight")
ablight.setColor(Vec4(0.2, 0.2, 0.2, 1))
self.__ablightnode = self.cam.attachNewNode(ablight)
self.render.setLight(self.__ablightnode)
ptlight0 = PointLight("pointlight1")
ptlight0.setColor(VBase4(1, 1, 1, 1))
self.__ptlightnode0 = self.cam.attachNewNode(ptlight0)
self.__ptlightnode0.setPos(0, 0, 0)
self.render.setLight(self.__ptlightnode0)
ptlight1 = PointLight("pointlight1")
ptlight1.setColor(VBase4(.4, .4, .4, 1))
self.__ptlightnode1 = self.cam.attachNewNode(ptlight1)
self.__ptlightnode1.setPos(self.cam.getPos().length(), 0, self.cam.getPos().length())
self.render.setLight(self.__ptlightnode1)
ptlight2 = PointLight("pointlight2")
ptlight2.setColor(VBase4(.3, .3, .3, 1))
self.__ptlightnode2 = self.cam.attachNewNode(ptlight2)
self.__ptlightnode2.setPos(-self.cam.getPos().length(), 0, base.cam.getPos().length())
self.render.setLight(self.__ptlightnode2)
self.pg = pg
self.pggen = pg.PandaGeomGen()
# set up inputmanager
self.inputmgr = im.InputManager(self, lookatp, self.pggen)
thepot = []
taskMgr.add(self.cycleUpdate, "cycle update", extraArgs=[thepot], appendTask=True)
# set up rotational cam
self.lookatp = lookatp
# taskMgr.doMethodLater(.1, self.rotateCam, "rotate cam")
# set window size
props = WindowProperties()
props.setSize(w, h)
self.win.requestProperties(props)
# set up cartoon effect
self.__separation = 1
self.filters = CommonFilters(self.win, self.cam)
self.filters.setCartoonInk(separation=self.__separation)