def __init__(self, objname, markerid=5):
self.markerid = markerid
self.objname = objname
self.gdb = db.GraspDB(database="wrs",
user="******",
password="******")
self.robot = ur3dualsim.Ur3DualRobot()
self.rgthnd = rtq85nm.newHand(hndid="rgt")
self.lfthnd = rtq85nm.newHand(hndid="lft")
self.robotmesh = ur3dualsimmesh.Ur3DualMesh(rgthand=self.rgthnd,
lfthand=self.lfthnd)
self.robotball = ur3dualsimball.Ur3DualBall()
self.robotik = ur3dualik
self.env = bunrisettingfree.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]
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.groove = self.env.loadobj("groove.stl")
self.groove.setPos(495,0,973-76)
self.groove.setColor(0,1,0,1)
self.groove.reparentTo(base.render)
self.obstaclecmlist = self.env.getstationaryobslist()
# self.obstaclecmlist.append(self.groove)
# 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.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]
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()
import robotsim.ur3dual.ur3dual as ur3dualsim
import robotsim.ur3dual.ur3dualmesh as ur3dualsimmesh
import robotcon.ur3dual as ur3urx
import cv2
import cv2.aruco as aruco
if __name__=='__main__':
#load the left arm camera parameters
font = cv2.FONT_HERSHEY_SIMPLEX
cv_file = cv2.FileStorage("aruco/calib_images/left_arm_camera/left_arm_camera_calib__.yaml", cv2.FILE_STORAGE_READ)
mtx = cv_file.getNode("camera_matrix").mat()
dist = cv_file.getNode("dist_coeff").mat()
#The robot simulation model
robot = ur3dualsim.Ur3DualRobot()
rgthnd = rtq85nm.newHand(hndid = "rgt")
lfthnd = rtq85nm.newHand(hndid = "lft")
robotmesh = ur3dualsimmesh.Ur3DualMesh(rgthand = rgthnd, lfthand = lfthnd)
#move the robot simulation model to its initial pose
robot.goinitpose()
#the joints of the left arm calibration pose
jntlft_calib = [29.359462279367676, -163.2380687300504, -50.661251584499425, 175.0638136354098, -147.0334995657085, 192.5160534554379]
#move the robot simulation model left arm to the calibration pose
robot.movearmfk(jntlft_calib, "lft")
#the real robot
ur3u = ur3urx.Ur3DualUrx()
# move the real robot right arm to its initial pose
ur3u.movejntssgl(robot.initrgtjnts, armid="rgt")
#move the real robot left arm to the clibration pose