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()
