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]
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()
robot.goinitpose() if isselfcollided: return True else: return False if __name__ == '__main__': import robotsim.nextage.nxtplot as nxtplot base = pandactrl.World() robot = nxt.NxtRobot() handpkg = rtq85nm robotmesh = nxtmesh.NxtMesh(handpkg) robotball = nxtball.NxtBall() # cdchecker = cdck.CollisionChecker(robotmesh) cdchecker = cdck.CollisionCheckerBall(robotball) start = [50.0, 0.0, -143.0, 0.0, 0.0, 0.0] goal = [-15.0, 0.0, -143.0, 0.0, 0.0, 0.0] # plot init and goal robot.movearmfk(armjnts=start, armid='rgt') robotmesh.genmnp(robot).reparentTo(base.render) robot.movearmfk(armjnts=goal, armid='rgt') robotmesh.genmnp(robot).reparentTo(base.render) jointlimits = [[robot.rgtarm[1]['rngmin'], robot.rgtarm[1]['rngmax']], [robot.rgtarm[2]['rngmin'], robot.rgtarm[2]['rngmax']], [robot.rgtarm[3]['rngmin'], robot.rgtarm[3]['rngmax']],
# obscm.showcn() # load obj collision model using env objcm = env.loadobj("bunnysim.stl") # another example -- load obj collision model independently # this_dir, this_filename = os.path.split(__file__) # objname = "tool_motordriver.stl" # objpath = os.path.join(this_dir, "objects", objname) # objcm = cm.CollisionModel(objpath) robot = nxt.NxtRobot() robotball = nxtball.NxtBall() rgthnd = rtq85.Rtq85(jawwidth=85, ftsensoroffset=0) lfthnd = rtq85.Rtq85(jawwidth=85, ftsensoroffset=0) robotmesh = nxtmesh.NxtMesh(rgthand=rgthnd, lfthand=lfthnd) robotnp = robotmesh.genmnp(robot, jawwidthrgt=85.0, jawwidthlft=0.0) # robotnp.reparentTo(base.render) armname = 'rgt' smoother = sm.Smoother() robot.goinitpose() # robotnp = robotmesh.genmnp(robot, jawwidthrgt = 85.0, jawwidthlft=0.0) # robotnp.reparentTo(base.render) # base.run() starttreesamplerate = 25 endtreesamplerate = 30 objstpos = np.array([354.5617667638, -256.0889005661, 1090.5]) objstrot = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]).T objgpos = np.array([304.5617667638, -277.1026725769, 1101.0])
if __name__ == "__main__": base = pandactrl.World(camp=[3500, -300, 1500], lookatp=[0, 0, 1000]) env = bldgfsettingnear.Env(base) env.reparentTo(base.render) objpos_lft = [] objrot_lft = [] objpos_rgt = [] objrot_rgt = [] nxtrobot = nxt.NxtRobot() robotball = nxtball.NxtBall() lfthand = rtq85.Rtq85(jawwidth=85, ftsensoroffset=0) rgthand = rtq85.Rtq85(jawwidth=85, ftsensoroffset=0) robotmesh = nxtmesh.NxtMesh(lfthand=lfthand, rgthand=rgthand) nxtmesh = robotmesh.genmnp(nxtrobot, jawwidthrgt=85.0, jawwidthlft=0.0, togglejntscoord=False) # nxtmesh.reparentTo(base.render) armname = 'rgt' nxtball = nxtball.NxtBall() cdchecker = cdck.CollisionCheckerBall(nxtball) ctcallback = ctcb.CtCallback(base, nxtrobot, cdchecker, ctchecker=None, armname=armname) smoother = sm.Smoother() starttreesamplerate = 25 endtreesamplerate = 30 pandamat4 = Mat4() pandamat4.setRow(3, Vec3(0, 0, 250)) nxtrobot.goinitpose()
#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 largeboard0 = cm.CollisionModel(largeboardpath, radius=3,