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]
env = bunrisettingfree.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 = ur3dual.Ur3DualRobot() robotball = ur3dualball.Ur3DualBall() rgthnd = rtq85.Rtq85(jawwidth=85) lfthnd = rtq85.Rtq85(jawwidth=85) robotmesh = ur3dualmesh.Ur3DualMesh(rgthand=rgthnd, lfthand=lfthnd) robotnp = robotmesh.genmnp(robot, jawwidthrgt=85.0, jawwidthlft=0.0) # robotnp.reparentTo(base.render) armname = 'rgt' cdchecker = cdck.CollisionCheckerBall(robotball) ctcallback = ctcb.CtCallback(base, robot, cdchecker, armname=armname) smoother = sm.Smoother() robot.goinitpose() # robotnp = robotmesh.genmnp(robot, jawwidthrgt = 85.0, jawwidthlft=0.0) # robotnp.reparentTo(base.render) # base.run()
import manipulation.grip.robotiq85.rtq85nm as rtq85nm 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")
import pandaplotutils.pandactrl as pandactrl from direct.filter.CommonFilters import CommonFilters import manipulation.grip.robotiq85.rtq85 as rtq85 from robotsim.ur3dual import ur3dual from robotsim.ur3dual import ur3dualmesh import utiltools.robotmath as rm # loadPrcFileData("", "want-directtools #t") # loadPrcFileData("", "want-tk #t") base = pandactrl.World(camp=[3000, 3000, 3000], lookatp=[0, 0, 1300]) rgthnd = rtq85.Rtq85() lfthnd = rtq85.Rtq85() ur3dualrobot = ur3dual.Ur3DualRobot(position=[0, 500, 0], rotmat=rm.rodrigues([0, 0, 1], 70)) # goalpos = np.array([200,120,1410]) # goalrot = np.array([[1,0,0],[0,0,-1],[0,1,0]]) # goal = ur3dualrobot.numik(goalpos, goalrot, armname = 'lft') # goal = np.array([-46.251876352032305, -41.418654079396184, 94.34173209615693, -15.917387039376576, 92.07172082393664, -25.134340575525133]) goal = np.array([ -209.13573775, -42.61307312, -283.86285256, 30.51538756, -231.85631837, -218.24860474 ]) ur3dualrobot.movearmfk(goal, armname='rgt') # ur3dualrobot.gozeropose() ur3dualrobotball = Ur3DualBall() # bcndict = ur3dualrobotball.genfullbcndict(ur3dualrobot) # bcndict = ur3dualrobotball.gensemibcndict(ur3dualrobot) # bcndict = ur3dualrobotball.genholdbcndict(ur3dualrobot, armname='rgt') # bcndict = ur3dualrobotball.genfullactivebcndict(ur3dualrobot)
return armjnts_degree if __name__ == '__main__': import math3d import robotsim.ur3dual.ur3dual as u3d import robotsim.ur3dual.ur3dualmesh as u3dm import pandaplotutils.pandactrl as pc from manipulation.grip.robotiq85 import rtq85 base = pc.World(camp=[3000, 0, 3000], lookatp=[0, 0, 700]) rgthand = rtq85.Rtq85() lfthand = rtq85.Rtq85() ur3dualrobot = u3d.Ur3DualRobot() ur3dualrobot.goinitpose() ur3u = Ur3DualUrx() initpose = ur3dualrobot.initjnts initrgt = initpose[3:9] initlft = initpose[9:15] ur3u.movejntssgl(initrgt, armname='rgt') ur3u.movejntssgl(initlft, armname='lft') # goalrgt = copy.deepcopy(initrgt) # goalrgt[0] = goalrgt[0]-10.0 # goalrgt1 = copy.deepcopy(initrgt) # goalrgt1[0] = goalrgt1[0]-5.0 # goallft = copy.deepcopy(initlft) # goallft[0] = goallft[0]+10.0
obscm.showcn() objcm = env.loadobj("bunnysim.stl") objcm.setColor(.2, .5, 0, 1) objcm.setPos(400, -200, 1200) objcm.reparentTo(base.render) objcm.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) objcm3 = cm.CollisionModel( trimesh.primitives.Box(box_center=[500, 100, 1500], box_extents=[200, 50, 100])) objcm3.setColor(1, 0, 0, 1) objcm3.reparentTo(base.render) objcm3.showcn() hndfa = rtq85.Robotiq85Factory() rgthnd = hndfa.genHand() lfthnd = hndfa.genHand() rbt = robot.Ur3DualRobot(rgthnd=rgthnd, lfthnd=lfthnd) rbtmg = robotmesh.Ur3DualMesh() rbtmnp = rbtmg.genmnp(rbt, toggleendcoord=True) rbtmnp.reparentTo(base.render) base.run()
#max torque limit friction_torque = 0.9 #Nm if __name__ == "__main__": base = pandactrl.World(camp=[2700, 300, 2700], lookatp=[0, 0, 1000]) #set the env and its collision models env = bsf.Env() env.reparentTo(base.render) obscmlist = env.getstationaryobslist() #set the hand and robot hndfa = rtq85.Robotiq85Factory() rgthnd = hndfa.genHand() lfthnd = hndfa.genHand() rbt = robotsim.Ur3DualRobot(rgthnd, lfthnd) rbtmg = robotmesh.Ur3DualMesh() #robot initial start and tranformed start for the task startpos = np.array([600, 400, 1550]) startrot = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]]) startrot = rot_transform(startrot, 180) #to visualize the starting pose jnts = rbt.numik(startpos, startrot, armname="lft") rbt.movearmfk(jnts, armname="lft") # rbtmg.genmnp(rbt).reparentTo(base.render) #load the object and set its pose #objects are "small_3mm", "small_10mm", "big" obj = loadobj.Objsim(base, name="small_10mm") startposobj = np.array([600 + obj.length / 2, 400, 1550]) # 1350 #500
import numpy as np from panda3d.core import * 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) hndfa = rtq85.Robotiq85Factory() rgthnd = hndfa.genHand() lfthnd = hndfa.genHand() robotsim = ur3dualsim.Ur3DualRobot(rgthnd, lfthnd) robotmeshgen = ur3dualsimmesh.Ur3DualMesh(rgthand=rgthnd, lfthand=lfthnd) robotmesh = robotmeshgen.genmnp(robotsim, toggleendcoord=False) robotmesh.reparentTo(base.render) base.run()