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