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]
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']], [robot.rgtarm[4]['rngmin'], robot.rgtarm[4]['rngmax']],