def updateshow(objms, numikrms, jawwidth, nxtmnp, objmnp, counter,
nxtrobot, handpkg, objpath, task):
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if objmnp[0] is not None:
objmnp[0].detachNode()
print counter[0]
print numikrms[counter[0]]
nxtrobot.movearmfkr(numikrms[counter[0]])
#move robot
alljnts = [numikrms[counter[0]][0], 0, 0]
alljnts.extend([i for i in numikrms[counter[0]][1]])
alljnts.extend([i for i in nxtrobot.initjnts[9:]])
nxtrobot.movealljnts(alljnts)
nxtmnp[0] = nxtplot.genmnp(nxtrobot,
handpkg,
jawwidthrgt=jawwidth[counter[0]])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
objmnp[0] = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objmnp[0].setMat(objms[counter[0]])
objmnp[0].reparentTo(base.render)
if counter[0] >= 1:
if base.inputmgr.keyMap['space'] is False:
pass
else:
c2e = counter[0] - 1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in nxtrobot.initjnts[9:]])
nxts.movejnts15(alljnts)
if jawwidth[c2e] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
counter[0] += 1
base.inputmgr.keyMap['space'] = False
else:
counter[0] += 1
else:
if base.inputmgr.keyMap['space'] is True:
c2e = counter[0] - 1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in nxtrobot.initjnts[9:]])
nxts.movejnts15(alljnts)
if jawwidth[c2e] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
nxts.movejnts15(nxtrobot.initjnts)
return task.done
return task.again
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp,
obj1mnp, counter, nxtrobot, obj0path, obj1path, task):
base.win.saveScreenshot(Filename(str(counter[0]) + '.jpg'))
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if obj0mnp[0] is not None:
obj0mnp[0].detachNode()
if obj1mnp[0] is not None:
obj1mnp[0].detachNode()
print counter[0]
print numikrms[counter[0]]
alljnts = [numikrms[counter[0]][0], 0, 0]
alljnts.extend([i for i in numikrms[counter[0]][1]])
alljnts.extend([i for i in numikrms[counter[0]][2]])
nxtrobot.movealljnts(alljnts)
nxtmnp[0] = nxtplot.genmnp(nxtrobot,
handpkg,
jawwidthrgt=jawwidth[counter[0]][0],
jawwidthlft=jawwidth[counter[0]][1])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
obj0mnp[0] = pg.genObjmnp(obj0path, color=Vec4(.7, .3, 0, 1))
obj0mnp[0].setMat(objms0[counter[0]])
# pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]])
obj0mnp[0].reparentTo(base.render)
obj1mnp[0] = pg.genObjmnp(obj1path, color=Vec4(0, .3, .7, 1))
obj1mnp[0].setMat(objms1[counter[0]])
# pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]])
obj1mnp[0].reparentTo(base.render)
counter[0] += 1
else:
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
nxtrobot.goinitpose()
nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg)
nxtmnp[0].reparentTo(base.render)
# counter[0] = 0
return task.done
pass
return task.again
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp, obj1mnp, counter, nxtrobot, obj0path, obj1path, task):
base.win.saveScreenshot(Filename(str(counter[0])+'.jpg'))
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if obj0mnp[0] is not None:
obj0mnp[0].detachNode()
if obj1mnp[0] is not None:
obj1mnp[0].detachNode()
print counter[0]
print numikrms[counter[0]]
alljnts = [numikrms[counter[0]][0], 0, 0]
alljnts.extend([i for i in numikrms[counter[0]][1]])
alljnts.extend([i for i in numikrms[counter[0]][2]])
nxtrobot.movealljnts(alljnts)
nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]][0], jawwidthlft=jawwidth[counter[0]][1])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
obj0mnp[0] = pg.genObjmnp(obj0path, color = Vec4(.7,.3,0,1))
obj0mnp[0].setMat(objms0[counter[0]])
# pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]])
obj0mnp[0].reparentTo(base.render)
obj1mnp[0] = pg.genObjmnp(obj1path, color = Vec4(0,.3,.7,1))
obj1mnp[0].setMat(objms1[counter[0]])
# pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]])
obj1mnp[0].reparentTo(base.render)
counter[0] += 1
else:
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
nxtrobot.goinitpose()
nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg)
nxtmnp[0].reparentTo(base.render)
# counter[0] = 0
return task.done
pass
return task.again
def updateshow(objms, numikrms, jawwidth, nxtmnp, objmnp, counter, nxtrobot, handpkg, objpath, task):
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if objmnp[0] is not None:
objmnp[0].detachNode()
print counter[0]
print numikrms[counter[0]]
nxtrobot.movearmfkr(numikrms[counter[0]])
nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
objmnp[0] = pg.genObjmnp(objpath, color = Vec4(.7,.7,0,1))
objmnp[0].setMat(objms[counter[0]])
# pg.plotAxisSelf(base.render,objms[counter[0]].getRow3(3), objms[counter[0]])
objmnp[0].reparentTo(base.render)
counter[0] += 1
return task.again
def updateshow(objms, numikrms, jawwidth, nxtmnp, objmnp, counter,
nxtrobot, handpkg, objpath, task):
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if objmnp[0] is not None:
objmnp[0].detachNode()
print counter[0]
print numikrms[counter[0]]
nxtrobot.movearmfkr(numikrms[counter[0]])
nxtmnp[0] = nxtplot.genmnp(nxtrobot,
handpkg,
jawwidthrgt=jawwidth[counter[0]])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
objmnp[0] = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objmnp[0].setMat(objms[counter[0]])
# pg.plotAxisSelf(base.render,objms[counter[0]].getRow3(3), objms[counter[0]])
objmnp[0].reparentTo(base.render)
counter[0] += 1
return task.again
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp,
obj1mnp, counter, nxtrobot, obj0path, obj1path, task):
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if obj0mnp[0] is not None:
obj0mnp[0].detachNode()
if obj1mnp[0] is not None:
obj1mnp[0].detachNode()
alljnts = [numikrms[counter[0]][0], 0, 0]
alljnts.extend([i for i in numikrms[counter[0]][1]])
alljnts.extend([i for i in numikrms[counter[0]][2]])
nxtrobot.movealljnts(alljnts)
nxtmnp[0] = nxtplot.genmnp(nxtrobot,
handpkg,
jawwidthrgt=jawwidth[counter[0]][0],
jawwidthlft=jawwidth[counter[0]][1])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
obj0mnp[0] = pg.genObjmnp(obj0path, color=Vec4(.7, .3, 0, 1))
obj0mnp[0].setMat(objms0[counter[0]])
# pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]])
obj0mnp[0].reparentTo(base.render)
obj1mnp[0] = pg.genObjmnp(obj1path, color=Vec4(0, .3, .7, 1))
obj1mnp[0].setMat(objms1[counter[0]])
# pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]])
obj1mnp[0].reparentTo(base.render)
if counter[0] >= 1:
print base.inputmgr.keyMap['space']
if base.inputmgr.keyMap['space'] is False:
pass
else:
c2e = counter[0] - 1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in numikrms[c2e][2]])
nxts.movejnts15(alljnts)
if jawwidth[c2e][0] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
if jawwidth[c2e][1] < 85:
rtq85ls.openhandto(0)
else:
rtq85ls.openhandto(85)
counter[0] += 1
base.inputmgr.keyMap['space'] = False
else:
counter[0] += 1
else:
if base.inputmgr.keyMap['space'] is True:
c2e = counter[0] - 1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in numikrms[c2e][2]])
# nxts.movejnts15(alljnts)
if jawwidth[c2e][0] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
if jawwidth[c2e][1] < 85:
rtq85ls.openhandto(0)
else:
rtq85ls.openhandto(85)
# if nxtmnp[0] is not None:
# nxtmnp[0].detachNode()
# nxtrobot.goinitpose()
# nxtmnp[0] = nxtplot.genNxtmnp(nxtrobot)
# nxtmnp[0].reparentTo(base.render)
# counter[0] = 0
return task.done
return task.again
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()
def updateshow(objms0, objms1, numikrms, jawwidth, nxtmnp, obj0mnp, obj1mnp, counter, nxtrobot, obj0path, obj1path, task):
if counter[0] < len(numikrms):
if nxtmnp[0] is not None:
nxtmnp[0].detachNode()
if obj0mnp[0] is not None:
obj0mnp[0].detachNode()
if obj1mnp[0] is not None:
obj1mnp[0].detachNode()
alljnts = [numikrms[counter[0]][0], 0, 0]
alljnts.extend([i for i in numikrms[counter[0]][1]])
alljnts.extend([i for i in numikrms[counter[0]][2]])
nxtrobot.movealljnts(alljnts)
nxtmnp[0] = nxtplot.genmnp(nxtrobot, handpkg, jawwidthrgt=jawwidth[counter[0]][0], jawwidthlft=jawwidth[counter[0]][1])
nxtrobot.goinitpose()
nxtmnp[0].reparentTo(base.render)
obj0mnp[0] = pg.genObjmnp(obj0path, color = Vec4(.7,.3,0,1))
obj0mnp[0].setMat(objms0[counter[0]])
# pg.plotAxisSelf(base.render,objms0[counter[0]].getRow3(3), objms[counter[0]])
obj0mnp[0].reparentTo(base.render)
obj1mnp[0] = pg.genObjmnp(obj1path, color = Vec4(0,.3,.7,1))
obj1mnp[0].setMat(objms1[counter[0]])
# pg.plotAxisSelf(base.render,objms1[counter[0]].getRow3(3), objms[counter[0]])
obj1mnp[0].reparentTo(base.render)
if counter[0] >= 1:
print base.inputmgr.keyMap['space']
if base.inputmgr.keyMap['space'] is False:
pass
else:
c2e = counter[0]-1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in numikrms[c2e][2]])
nxts.movejnts15(alljnts)
if jawwidth[c2e][0] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
if jawwidth[c2e][1] < 85:
rtq85ls.openhandto(0)
else:
rtq85ls.openhandto(85)
counter[0] += 1
base.inputmgr.keyMap['space'] = False
else:
counter[0] += 1
else:
if base.inputmgr.keyMap['space'] is True:
c2e = counter[0]-1
alljnts = [numikrms[c2e][0], 0, 0]
alljnts.extend([i for i in numikrms[c2e][1]])
alljnts.extend([i for i in numikrms[c2e][2]])
# nxts.movejnts15(alljnts)
if jawwidth[c2e][0] < 85:
rtq85rs.openhandto(0)
else:
rtq85rs.openhandto(85)
if jawwidth[c2e][1] < 85:
rtq85ls.openhandto(0)
else:
rtq85ls.openhandto(85)
# if nxtmnp[0] is not None:
# nxtmnp[0].detachNode()
# nxtrobot.goinitpose()
# nxtmnp[0] = nxtplot.genNxtmnp(nxtrobot)
# nxtmnp[0].reparentTo(base.render)
# counter[0] = 0
return task.done
return task.again
base = pandactrl.World(camp=[3000, 0, 3000], lookatp=[0, 0, 700])
# robot = ur5dual.Ur5DualRobot()
# handpkg = rtq85nm
# robotplot = ur5dualplot
# robot = hrp5n.Hrp5NRobot()
# handpkg = hrp5threenm
# robotmesh = hrp5nmesh.Hrp5NMesh(handpkg)
# robotmnp = robotmesh.genmnp(robot)
# robotmnp.reparentTo(base.render)
robot = nxt.NxtRobot()
handpkg = rtq85nm
#robotplot = nxtplot
robotmnp = nxtplot.genmnp(robot, handpkg)
robotmnp.reparentTo(base.render)
robotmesh = 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))
#obstacle list
this_dir = "E:/project/manipulation/regrasp_onworkcell/dropsimulation"