def __init__(self, objname, markerid=5):
self.markerid = markerid
self.objname = objname
self.gdb = db.GraspDB(database="nxt",
user="******",
password="******")
self.robot = nxtsim.NxtRobot()
self.rgthnd = sck918.Sck918(jawwidth=50,
hndcolor=(0.5, 0.5, 0.5, 1),
ftsensoroffset=0)
self.lfthnd = sck918.Sck918(jawwidth=50,
hndcolor=(0.5, 0.5, 0.5, 1),
ftsensoroffset=0)
self.robotmesh = nxtsimmesh.NxtMesh(rgthand=self.rgthnd,
lfthand=self.lfthnd)
self.robotball = nxtsimball.NxtBall()
self.robotik = nxtik
self.env = bldgfsettingnear.Env()
self.env.reparentTo(base.render)
self.objcm = self.env.loadobj(objname)
self.groove = self.env.loadobj("tenonhole_new_115.STL")
self.groove.setPos(595, 0, 973 - 76 + 75 + 80)
self.groove.setRPY(0, 90, -90)
self.groove.setColor(0, 0, 1, 0.4)
# self.groove.showLocalFrame()
self.groove.reparentTo(base.render)
self.obstaclecmlist = self.env.getstationaryobslist()
# for obstaclecdcm in self.obstaclecmlist:
# obstaclecdcm.showcn()
self.robot.goinitpose()
# self.rgtwatchpos = [400.0, -200.0, 1200.0]
# self.rgtwatchrotmat = [[0.09141122, -0.76823672, 0.63360582],
# [-0.99509199, -0.04625775, 0.08747659],
# [-0.03789355, -0.63849242, -0.76869468]]
# self.rgtwatchjs = self.robot.numik(self.rgtwatchpos, self.rgtwatchrotmat, "rgt")
#
# self.robot.movearmfk(self.rgtwatchjs, armname="rgt")
self.rbtmnp = self.robotmesh.genmnp(self.robot,
self.rgthnd.jawwidthopen,
self.lfthnd.jawwidthopen)
self.rbtmnp.reparentTo(base.render)
# uncomment the following commands to actuate the robot
self.nxtu = nxturx.NxtRobot(host="10.0.1.114:15005")
self.nxtu.goInitial()
# self.nxtu = nxturx.NxtCon()
# self.nxtu.setJointAnglesOfGroup('rarm', self.robot.initjnts[3:9], 5.0)
# self.nxtu.setJointAnglesOfGroup('larm', self.robot.initjnts[9:15], 5.0)
self.hc = hndcam.HndCam(rgtcamid=0, lftcamid=1)
# goallist, a list of 4x4 h**o numpy mat
self.goallist = []
self.objrenderlist = []
self.startobjcm = None
self.rbtmnpani = [None, None]
self.objmnpani = [None]
def __init__(self, objname, markerid=5):
self.markerid = markerid
self.objname = objname
self.gdb = db.GraspDB(database="nxt",
user="******",
password="******")
self.robot = nxtsim.NxtRobot()
self.rgthnd = rtq85nm.newHand(hndid="rgt", ftsensoroffset=0)
self.lfthnd = rtq85nm.newHand(hndid="lft", ftsensoroffset=0)
self.robotmesh = nxtsimmesh.NxtMesh(rgthand=self.rgthnd,
lfthand=self.lfthnd)
self.robotball = nxtsimball.NxtBall()
self.robotik = nxtik
self.env = bldgfsettingnear.Env()
self.env.reparentTo(base.render)
self.objcm = self.env.loadobj(objname)
self.obstaclecmlist = self.env.getstationaryobslist()
for obstaclecdcm in self.obstaclecmlist:
obstaclecdcm.showcn()
self.robot.goinitpose()
# self.rgtwatchpos = [400.0, -200.0, 1200.0]
# self.rgtwatchrotmat = [[0.09141122, -0.76823672, 0.63360582],
# [-0.99509199, -0.04625775, 0.08747659],
# [-0.03789355, -0.63849242, -0.76869468]]
# self.rgtwatchjs = self.robot.numik(self.rgtwatchpos, self.rgtwatchrotmat, "rgt")
#
# self.robot.movearmfk(self.rgtwatchjs, armname="rgt")
self.rbtmnp = self.robotmesh.genmnp(self.robot,
self.rgthnd.jawwidthopen,
self.lfthnd.jawwidthopen)
self.rbtmnp.reparentTo(base.render)
# uncomment the following commands to actuate the robot
# self.ur3u = ur3urx.Ur3DualUrx()
# self.ur3u.movejntssgl(self.robot.initjnts[3:9], 'rgt')
# self.ur3u.movejntssgl(self.robot.initjnts[9:15], 'lft')
# self.ur3u.movejntssgl(self.rgtwatchjs, 'rgt')
self.hc = cameras.HndCam(rgtcamid=0, lftcamid=1)
# goallist, a list of 4x4 h**o numpy mat
self.goallist = []
self.objrenderlist = []
self.startobjcm = None
self.rbtmnpani = [None, None]
self.objmnpani = [None]
# updateDBwithFGPairs compute the FGpairs without considering ik
# updateDBwithIK compute the ik feasible FGs
# loadIKFeasibleFGPairsFromDB load DBFGpairs and DBwithIK and compute the IK-feasible FGpairs
grids = []
for x in range(400,401,100):
for y in [0]:
for z in range(150,151,100):
grids.append([x,y,z])
fpose.genFPandGs(grids)
fpose.saveToDB()
fpose.loadFromDB()
fpose.updateDBwithFGPairs()
nxtrobot = nxt.NxtRobot()
# hrp5nrobot = hrp5n.Hrp5NRobot()
# hrp2k = hrp2k.Hrp2KRobot()
fpose.updateDBwithIK(robot=nxtrobot)
# fpose.updateDBwithIK(robot=hrp5nrobot)
# fpose.updateDBwithIK(robot=hrp2k)
# for i in range(1,len(fpose.gridsfloatingposemat4s),len(fpose.floatingposemat4)):
# fpose.plotOneFPandG(base.render, i)
fpose.loadIKFeasibleFGPairsFromDB(robot=nxtrobot)
# fpose.loadIKFeasibleFGPairsFromDB(robot=hrp5nrobot)
# fpose.loadIKFeasibleFGPairsFromDB(robot=hrp2k)
# fpose.plotOneFPandG(base.render, 0)4
fpose.plotOneFPandGPairs(base.render, 0)
# poseid = [0]
base = pandactrl.World(camp=[2700, 300, 2700], lookatp=[0, 0, 1000])
env = Env()
env.reparentTo(base.render)
objcm = env.loadobj("bunnysim.stl")
objcm.setColor(.2, .5, 0, 1)
objcm.setPos(400, -200, 1200)
objcm.reparentTo(base.render)
objcm.showcn()
obscmlist = env.getstationaryobslist()
for obscm in obscmlist:
obscm.showcn()
objpos = np.array([400, -300, 1200])
objrot = rm.rodrigues([0, 1, 0], 45)
objcm2 = env.loadobj("housing.stl")
objcm2.setColor(1, .5, 0, 1)
env.addchangableobs(base.render, objcm2, objpos, objrot)
robotsim = robotsim.NxtRobot()
rgthnd = rtq85nm.newHand(hndid="rgt", ftsensoroffset=0)
lfthnd = rtq85nm.newHand(hndid="lft", ftsensoroffset=0)
robotmeshgen = robotmesh.NxtMesh(rgthand=rgthnd, lfthand=lfthnd)
robotmesh = robotmeshgen.genmnp(robotsim, toggleendcoord=False)
robotmesh.reparentTo(base.render)
base.pggen.plotAxis(base.render, spos=Vec3(400, -300, 900))
base.pggen.plotAxis(base.render, spos=Vec3(300, 0, 1300))
base.run()
# gdb = db.GraspDB(database="nxt")
tps = ttps.TabletopPlacements(objpath, handpkg)
## build grid space
## save to db
## use the following grids for ur3d
# grids = []
# for x in range(400,401,200):
# for y in range(-400,401,800):
# grids.append([x,y,1030])
# tps.saveToDB(grids, gdb)
## use the following grids for nxt
grids = []
for x in range(400,401,200):
for y in range(-270,271,540):
grids.append([x,y,900])
tps.saveToDB(grids, gdb)
# add ik for a robot
# robsim = ur3dualsim.Ur3DualRobot()
# tps.updateDBwithIK(gdb, robsim, armname = "rgt")
# tps.updateDBwithIK(gdb, robsim, armname = "lft")
# add ik for a second robot
robsim2 = nxtsim.NxtRobot()
tps.updateDBwithIK(gdb, robsim2, armname = "rgt")
tps.updateDBwithIK(gdb, robsim2, armname = "lft")
tps.grpshow(base, gdb)
base.run()
## use wrs database for ur3d
# gdb = db.GraspDB(database="wrs")
## use nxt database for nextage!
gdb = db.GraspDB(database="nxt")
fpose = fps.FloatingPoses(objpath, gdb, handpkg, base)
## usage:
## genFPGPandSaveToDB generate floating poses and grasps and saves them
## updatgeDBwithIK
## use the following grids for ur3d
# grids = []
# for x in range(400,401,100):
# for y in [0]:
# for z in range(1430,1431,400):
# grids.append([x,y,z])
# fpose.genFPGPandSaveToDB(grids, angles=[0,180])
# robsim = ur3dualsim.Ur3DualRobot()
# fpose.updateDBwithIK(robot=robsim)
## use the following grids for nxt
grids = []
for x in range(300, 301, 100):
for y in [0]:
for z in range(1300, 1301, 400):
grids.append([x, y, z])
fpose.genFPGPandSaveToDB(grids, angles=[0, 90, 180, 270])
robsim = nxtsim.NxtRobot()
fpose.updateDBwithIK(robot=robsim)
base.run()
env = simenv.Env()
env.reparentTo(base.render)
#robot hand and arm
hndfa = rtq85.Robotiq85Factory()
rgthnd = hndfa.genHand()
lfthnd = hndfa.genHand()
rbt = robot.Ur3DualRobot(rgthnd=rgthnd, lfthnd=lfthnd)
rbtmg = robotmesh.Ur3DualMesh()
#human hand and arm
hhdfa = hhdw.HHdwFactory()
rgthmhnd = hhdfa.genHand(ftsensoroffset=0, armname="rgt")
lfthmhnd = hhdfa.genHand(ftsensoroffset=0, armname="lft")
hmn = human.NxtRobot(rgthnd=rgthmhnd,
lfthnd=lfthmhnd,
position=[1100, 0, 200],
rotmat=rm.rodrigues([0, 0, 1], 180))
hmnmg = humanmesh.NxtMesh()
#build the cabinet
cabinet = buildCabinet(base, isrendercoord=True)
cabinet.reparentTo(base.render)
cabinetpose = cabinet.getMat()
cabinetposenp4 = base.pg.mat4ToNp(cabinetpose)
__this_dir, _ = os.path.split(__file__)
largeboardpath = os.path.join(__this_dir, "objects", "largeboard.stl")
middleboardpath = os.path.join(__this_dir, "objects", "middleboard.stl")
smallboardpath = os.path.join(__this_dir, "objects", "smallboard.stl")
# the boards