tabletopgrips.idtabletopplacements=%d" % idtabletopplacements
result = gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .1])
tmphnd.setMat(hndrotmat)
tmphnd.setJawwidth(hndjawwidth)
tmphnd.reparentTo(base.render)
if __name__ == '__main__':
nxtrobot = nxt.NxtRobot()
hrp5robot = hrp5.Hrp5Robot()
hrp5n = hrp5n.Hrp5NRobot()
hrp2k = hrp2k.Hrp2KRobot()
base = pandactrl.World(camp=[1000, 400, 1000], lookatp=[400, 0, 0])
#this_dir, this_filename = os.path.split(__file__)
# objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "ttube.stl")
# objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "tool.stl")
#objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "tool2.stl")
# done 20170307
# objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "planewheel.stl")
# objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "planelowerbody.stl")
# done 20170313
# objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "planefrontstay.stl")
# objpath = os.path.join(os.path.split(this_dir)[0]+os.sep, "grip", "objects", "planerearstay.stl")
this_dir = "E:/project/manipulation/regrasp_onworkcell/dropsimulation"
# from robotsim.nextage import nxt
# from robotsim.nextage import nxtplot
# from manipulation.grip.robotiq85 import rtq85nm
from manipulation.grip.hrp5three import hrp5threenm
# use the following two sentences to examine the scene
# loadPrcFileData("", "want-directtools #t")
# loadPrcFileData("", "want-tk #t")
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
# 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)
