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