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]
from robotsim.ur3dual import ur3dualmesh from robotsim.ur3dual import ur3dualball from robotsim.nextage import nxt from robotsim.nextage import nxtmesh from robotsim.nextage import nxtball from pandaplotutils import pandactrl import bldgfsettingnear import numpy as np import copy import utils.robotmath as rm import os import environment.collisionmodel as cm base = pandactrl.World(camp=[2700, 300, 2700], lookatp=[0, 0, 1000]) env = bldgfsettingnear.Env(base) env.reparentTo(base.render) obscmlist = env.getstationaryobslist() # for obscm in obscmlist: # 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()
pass else: smoother = sm.Smoother(objcm) path = smoother.pathsmoothing(path, planner) endtime = time.clock() runtime = endtime - starttime print("Run time =", runtime) return path, runtime if __name__=="__main__": # base = pc.World(camp=[0, 800, 0], lookatp=[0, 0, 0], up=[0, 0, 1], fov=40, w=1920, h=1080) base = pc.World(camp=[800, 0, 0], lookatp=[0, 0, 0], up=[0, 0, 1], fov=40, w=1920, h=1080) # base = pc.World(camp=[200, 800, 200], lookatp=[0, 0, 0], up=[0, 0, 1], fov=40, w=1920, h=1080) env = bldgfsettingnear.Env() # self.env.reparentTo(base.render) # objname = "new_LSHAPE.stl" objname = "tenon.STL" # objname = "twostairpeg_handle.STL" objcm = env.loadobj(objname) # groove = env.loadobj("new_GROOVE.stl") groove = env.loadobj("tenonhole.STL") # groove = env.loadobj("twostairhole_board.STL") posG = base.pg.npToMat4(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), np.array([0, 0, 0])) setPosRPY(groove, posG) groove.setColor(0, 0, 1, .4) # groove.showLocalFrame() groove.reparentTo(base.render)