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]
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
ur3u.movejntssgl(jntlft_calib, armid='lft')
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)
# bcndict = ur3dualrobotball.gensemiactivebcndict(ur3dualrobot)
bcndict = ur3dualrobotball.genholdactivebcndict(ur3dualrobot,
armname='rgt')
ur3dualrobotball.showcn(base, bcndict)
ur3dualrobotmeshgen = ur3dualmesh.Ur3DualMesh(lfthand=lfthnd,
rgthand=rgthnd)
ur3dualrobotmnp = ur3dualrobotmeshgen.genmnp(ur3dualrobot,
togglejntscoord=True)
ur3dualrobotmnp.reparentTo(base.render)
ur3dualrobotsnp = ur3dualrobotmeshgen.gensnp(ur3dualrobot)
ur3dualrobotsnp.reparentTo(base.render)
base.pggen.plotAxis(base.render)
base.run()
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()
from panda3d.core import *
from panda3d.bullet import BulletSphereShape
import pandaplotutils.pandageom as pandageom
import pandaplotutils.pandactrl as pandactrl
# from direct.filter.CommonFilters import CommonFilters
# from robotsim.ur5dual import ur5dual
# from robotsim.ur5dual import ur5dualplot
from robotsim.ur3dual import ur3dual
from robotsim.ur3dual import ur3dualmesh
from robotsim.ur3dual import ur3dualball
# from robotsim.nextage import nxt
# from robotsim.nextage import nxtplot
# from manipulation.grip.robotiq85 import rtq85nm
# from manipulation.grip.hrp5three import hrp5threenm
base = pandactrl.World(camp=[3000, 0, 3000], lookatp=[0, 0, 700])
robotpose = [-135.0, 5.0, -180.0, -45.0, -90.0, 0]
robot = ur3dual.Ur3DualRobot()
robot.movearmfk(robotpose)
robotmeshgen = ur3dualmesh.Ur3DualMesh()
robotball = ur3dualball.Ur3DualBall()
robotmnp = robotmeshgen.genmnp(robot)
robotmnp.reparentTo(base.render)
bcnlist = robotball.genbcndict(robot)
robotball.showbcn(base, bcnlist)
cdchecker = CollisionCheckerBall(robotball)
print cdchecker.isSelfCollided(robot)
base.run()
# show in panda3d
from direct.showbase.ShowBase import ShowBase
from panda3d.core import *
from panda3d.bullet import BulletSphereShape
import pandaplotutils.pandageom as pandageom
import pandaplotutils.pandactrl as pandactrl
# from direct.filter.CommonFilters import CommonFilters
from robotsim.ur3dual import ur3dual
from robotsim.ur3dual import ur3dualmesh
from manipulation.grip.robotiq85 import rtq85
base = pandactrl.World(camp=[3000, 0, 3000], lookatp=[0, 0, 700])
robot = ur3dual.Ur3DualRobot()
robotmesh = ur3dualmesh.Ur3DualMesh()
rgthnd = rtq85.Rtq85()
lfthnd = rtq85.Rtq85()
robotmnp = robotmesh.genmnp(robot)
robotmnp.reparentTo(base.render)
base.pggen.plotAxis(base.render)
cdchecker = CollisionChecker(robotmesh)
print(cdchecker.isSelfCollided(robot))
bullcldrnp = base.render.attachNewNode("bulletcollider")
debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(False)
