def __init__(self, directory=None): """ :param directory: """ srcpcdfilename = "vacuumhead_templatepcd.pkl" srcmeshfilename = "vacuumhead.stl" if directory is None: self.bgdepth = pickle.load( open("./databackground/bgdepth.pkl", "rb")) self.bgpcd = pickle.load(open("./databackground/bgpcd.pkl", "rb")) self.sensorhomomat = pickle.load( open("./datacalibration/calibmat.pkl", "rb")) self.srcpcdnp = pickle.load( open("./dataobjtemplate/" + srcpcdfilename, "rb")) # tstpcd, tube stand template self.srccm = cm.CollisionModel("./objects/" + srcmeshfilename) else: self.bgdepth = pickle.load( open(directory + "/databackground/bgdepth.pkl", "rb")) self.bgpcd = pickle.load( open(directory + "/databackground/bgpcd.pkl", "rb")) self.sensorhomomat = pickle.load( open(directory + "/datacalibration/calibmat.pkl", "rb")) self.srcpcdnp = pickle.load( open(directory + "/dataobjtemplate/" + srcpcdfilename, "rb")) # tstpcd, tube stand template self.srccm = cm.CollisionModel(directory + "/objects/" + srcmeshfilename) # for compatibility with locatorfixed self.objhomomat = None
def gentubes(self, homomat, elearray=None, type="dumbbell"): """ :param elearray: :param tubestand_homomat: :param eleconfidencearray: None by default :param alpha: only works when eleconfidencearray is None, it renders the array transparently :return: author: weiwei date: 20191229osaka """ if elearray is None: elearray = self.pattern tubecmlist = [] for i in range(elearray.shape[0]): for j in range(elearray.shape[1]): if elearray[i,j] == 1: if type is "dumbbell": tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 100]))) tubecm.setColor(.5,0,0,1) elif type is "cad": tubecm = self.tubebigcm elif elearray[i,j] == 2: if type is "dumbbell": tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 80]))) tubecm.setColor(0,.5,0,1) elif type is "cad": tubecm = self.tubesmall1cm elif elearray[i,j] == 3: if type is "dumbbell": tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 70]))) tubecm.setColor(0,0,.5,1) elif type is "cad": tubecm = self.tubesmall2cm elif elearray[i,j] == -1: tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 70]))) tubecm.setColor(1,1,1,1) elif elearray[i,j] == -2: tubecm = cm.CollisionModel(p3dh.gendumbbell(spos=np.array([0,0,0]), epos = np.array([0, 0, 70]))) tubecm.setColor(0,0,0,1) else: continue newtubecm = copy.deepcopy(tubecm) tubemat = copy.deepcopy(homomat) tubepos_normalized = np.array([self.tubeholecenters[i,j][0], self.tubeholecenters[i,j][1], 5]) tubepos = rm.homotransformpoint(homomat, tubepos_normalized) tubemat[:3, 3] = tubepos newtubecm.set_homomat(tubemat) tubecmlist.append(newtubecm) return tubecmlist
def __init__(self, betransparent=False): """ load obstacles model separated by category :param base: author: weiwei date: 20181205 """ self.__this_dir, _ = os.path.split(__file__) # table self.__tablepath = os.path.join(self.__this_dir, "obstacles", "yumi_tablenotop.stl") self.__tablecm = cm.CollisionModel(self.__tablepath, betransparent) self.__tablecm.setColor(.55, .55, .5, 1.0) self.__tablecm.setColor(.45, .45, .35, 1.0) # housing ## housing pillar ## TODO these models could be replaced by trimesh.primitives self.__beam2100path = os.path.join(self.__this_dir, "obstacles", "yumi_column60602100.stl") self.__beam540path = os.path.join(self.__this_dir, "obstacles", "yumi_column6060540.stl") self.__pillarrgtcm = cm.CollisionModel(self.__beam2100path) self.__pillarrgtcm.setColor(.5, .5, .55, 1.0) self.__pillarrgtcm.setPos(-327.0, -240.0, -1015.0) self.__pillarlftcm = copy.deepcopy(self.__pillarrgtcm) self.__pillarlftcm.setColor(.5, .5, .55, 1.0) self.__pillarlftcm.setPos(-327.0, 240.0, -1015.0) ## housing rgt self.__rowbackcm = cm.CollisionModel(self.__beam540path) self.__rowbackcm.setColor(.5, .5, .55, 1.0) self.__rowbackcm.setPos(-327.0, 0.0, 1085.0) self.__rowrgtcm = copy.deepcopy(self.__rowbackcm) self.__rowrgtcm.setColor(.5, .5, .55, 1.0) homomat = self.__rowrgtcm.gethomomat() homomat[:3,:3] = rm.rodrigues([0,0,1],90) self.__rowrgtcm.sethomomat(homomat) self.__rowrgtcm.setPos(-27.0, -240.0, 1085.0) self.__rowlftcm = copy.deepcopy(self.__rowbackcm) self.__rowlftcm.setColor(.5, .5, .55, 1.0) homomat = self.__rowlftcm.gethomomat() homomat[:3,:3] = rm.rodrigues([0,0,1],90) self.__rowlftcm.sethomomat(homomat) self.__rowlftcm.setPos(-27.0, 240.0, 1085.0) self.__rowfrontcm = copy.deepcopy(self.__rowbackcm) self.__rowfrontcm.setColor(.5, .5, .55, 1.0) self.__rowfrontcm.setPos(273.0, -0.0, 1085.0) self.__battached = False self.__changableobslist = []
def gentubeandstandboxcm(self, homomat, wrapheight=120, rgba=np.array([.5, .5, .5, .3])): """ gen a solid box to wrap both a stand and the tubes in it :param homomat: :return: author: weiwei date: 20191229osaka """ homomat_copy = copy.deepcopy(homomat) homomat_copy[:3, 3] = homomat_copy[:3, 3] + homomat_copy[:3, 2] * wrapheight / 2 tubeandstandboxcm = cm.CollisionModel( p3dh.genbox( np.array([self.tubestandsize[0], self.tubestandsize[1], 120]), homomat_copy)) tubeandstandboxcm.setColor(rgba[0], rgba[1], rgba[2], rgba[3]) return tubeandstandboxcm
def __init__(self): this_dir, this_filename = os.path.split(__file__) self.__name = "barrett" # open and close is defined for antipodal grip (thumb vs index+middle) self.__jawwidthopen = 100 self.__jawwidthclose = 0 self.__base = cm.CollisionModel(objinit=this_dir + "/stl/palm.stl", betransparency=True) self.__fingerprox = cm.CollisionModel(objinit=this_dir + "/stl/finger_prox.stl", betransparency=True) self.__fingermed = cm.CollisionModel(objinit=this_dir + "/stl/finger_med.stl", betransparency=True) self.__fingerdist = cm.CollisionModel(objinit=this_dir + "/stl/finger_dist.stl", betransparency=True)
def __init__(self, elearray=np.zeros((5,10)), root=".."): self.tubestandcm = cm.CollisionModel(root+"/objects/tubestand_light.stl") self.tubestandcm.setColor(0,.5,.7,1) self.tubebigcm = cm.CollisionModel(root+"/objects/tubebig_capped.stl") self.tubebigcm.setColor(.57,0,0,.7) self.tubesmall1cm = cm.CollisionModel(root+"/objects/tubena.stl") # self.tubesmall1cm.setColor(0,.57,0,.7) self.tubesmall1cm.setColor(0,0,.57,.7) self.tubesmall2cm = cm.CollisionModel(root+"/objects/tubeblue.stl") self.tubesmall2cm.setColor(0,.57,0,.7) self.pattern = elearray tubeholecenters = [] for x in [-38,-19,0,19,38]: tubeholecenters.append([]) for y in [-81, -63, -45, -27, -9, 9, 27, 45, 63, 81]: tubeholecenters[-1].append([x,y]) self.tubeholecenters = np.array(tubeholecenters) self.tubeholesize = np.array([15, 16])
def __init__(self, directory=None, homomatfilename_start="rightfixture_light_homomat1", homomatfilename_goal="rightfixture_light_homomat2"): self.__directory = directory if directory is None: self.bgdepth = pickle.load( open("./databackground/bgdepth.pkl", "rb")) self.bgpcd = pickle.load(open("./databackground/bgpcd.pkl", "rb")) self.sensorhomomat = pickle.load( open("./datacalibration/calibmat.pkl", "rb")) self.tubestandcm = cm.CollisionModel( "./objects/tubestand_light.stl") self.tubebigcm = cm.CollisionModel("./objects/tubebig_capped.stl", type="cylinder", expand_radius=2) self.tubesmallcm = cm.CollisionModel( "./objects/tubesmall_capped.stl", type="cylinder", expand_radius=2) self.tubestandhomomat_start = pickle.load( open("./datafixture/" + homomatfilename_start + ".pkl", "rb")) self.tubestandhomomat_goal = pickle.load( open("./datafixture/" + homomatfilename_goal + ".pkl", "rb")) else: self.bgdepth = pickle.load( open(directory + "/databackground/bgdepth.pkl", "rb")) self.bgpcd = pickle.load( open(directory + "/databackground/bgpcd.pkl", "rb")) self.sensorhomomat = pickle.load( open(directory + "/datacalibration/calibmat.pkl", "rb")) self.tubestandcm = cm.CollisionModel( directory + "/objects/tubestand_light.stl") self.tubebigcm = cm.CollisionModel(directory + "/objects/tubebig_capped.stl", type="cylinder", expand_radius=2) self.tubesmallcm = cm.CollisionModel( directory + "/objects/tubesmall_capped.stl", type="cylinder", expand_radius=2) self.tubestandhomomat_start = pickle.load( open("./datafixture/" + homomatfilename_start + ".pkl", "rb")) self.tubestandhomomat_goal = pickle.load( open("./datafixture/" + homomatfilename_goal + ".pkl", "rb")) # down x, right y tubeholecenters = [] for x in [-36, -18, 0, 18, 36]: tubeholecenters.append([]) for y in [ -83.25, -64.75, -46.25, -27.75, -9.25, 9.25, 27.75, 46.25, 64.75, 83.25 ]: tubeholecenters[-1].append([x, y]) self.tubeholecenters = np.array(tubeholecenters) self.tubeholesize = np.array([17, 16.5]) self.tubestandsize = np.array([97, 191]) # initialize the registered tubes, a dictionary with the template of each tube type in a list (multiple values allowed) self.registeredtubetemps = {1: [], 2: []}
def __init__(self, directory=None, standtype = "light"): """ standtype could be normal, light, ... :param directory: :param standtype: """ if standtype is "normal": tsfilename = "tubestand.stl" tspcdfilename = "tubestandtemplatepcd.pkl" # down x, right y tubeholecenters = [] for x in [-38,-19,0,19,38]: tubeholecenters.append([]) for y in [-81, -63, -45, -27, -9, 9, 27, 45, 63, 81]: tubeholecenters[-1].append([x,y]) self.tubeholecenters = np.array(tubeholecenters) self.tubeholesize = np.array([15, 16]) self.tubestandsize = np.array([96, 192]) elif standtype is "light": tsfilename = "tubestand_light.stl" tspcdfilename = "tubestand_light_templatepcd.pkl" tubeholecenters = [] for x in [-36, -18, 0, 18, 36]: tubeholecenters.append([]) for y in [-83.25, -64.75, -46.25, -27.75, -9.25, 9.25, 27.75, 46.25, 64.75, 83.25]: tubeholecenters[-1].append([x, y]) self.tubeholecenters = np.array(tubeholecenters) self.tubeholesize = np.array([17, 16.5]) self.tubestandsize = np.array([97, 191]) self.__directory = directory if directory is None: self.bgdepth = pickle.load(open("./databackground/bgdepth.pkl", "rb")) self.bgpcd = pickle.load(open("./databackground/bgpcd.pkl", "rb")) self.sensorhomomat = pickle.load(open("./datacalibration/calibmat.pkl", "rb")) self.tstpcdnp = pickle.load(open("./dataobjtemplate/"+tspcdfilename, "rb"))# tstpcd, tube stand template self.tubestandcm = cm.CollisionModel("./objects/"+tsfilename) self.tubebigcm = cm.CollisionModel("./objects/tubebig_capped.stl", type="cylinder", expand_radius=0) self.tubesmallcm = cm.CollisionModel("./objects/tubesmall_capped.stl", type="cylinder", expand_radius=0) else: self.bgdepth = pickle.load(open(directory+"/databackground/bgdepth.pkl", "rb")) self.bgpcd = pickle.load(open(directory+"/databackground/bgpcd.pkl", "rb")) self.sensorhomomat = pickle.load(open(directory+"/datacalibration/calibmat.pkl", "rb")) self.tstpcdnp = pickle.load(open(directory+"/dataobjtemplate/"+tspcdfilename, "rb"))# tstpcd, tube stand template self.tubestandcm = cm.CollisionModel(directory+"/objects/"+tsfilename) self.tubebigcm = cm.CollisionModel(directory +"/objects/tubebig_capped.stl", type="cylinder", expand_radius=0) self.tubesmallcm = cm.CollisionModel(directory +"/objects/tubesmall_capped.stl", type="cylinder", expand_radius=0) # for compatibility with locatorfixed self.tubestandhomomat = None
def findtubes(self, tubestand_homomat, tgtpcdnp, toggledebug=False): """ :param tgtpcdnp: :return: author: weiwei date: 20200317 """ elearray = np.zeros((5, 10)) eleconfidencearray = np.zeros((5, 10)) tgtpcdnp = o3dh.remove_outlier(tgtpcdnp, downsampling_voxelsize=None, nb_points=90, radius=5) # transform back to the local frame of the tubestand tgtpcdnp_normalized = rm.homotransformpointarray( rm.homoinverse(tubestand_homomat), tgtpcdnp) if toggledebug: cm.CollisionModel(tgtpcdnp_normalized).reparentTo(base.render) tscm2 = copy.deepcopy(self.tubestandcm) tscm2.reparentTo(base.render) for i in range(5): for j in range(10): holepos = self.tubeholecenters[i][j] tmppcd = self._crop_pcd_overahole(tgtpcdnp_normalized, holepos[0], holepos[1]) if len(tmppcd) > 50: if toggledebug: print( "------more than 50 raw points, start a new test------" ) tmppcdover100 = tmppcd[tmppcd[:, 2] > 100] tmppcdbelow90 = tmppcd[tmppcd[:, 2] < 90] tmppcdlist = [tmppcdover100, tmppcdbelow90] if toggledebug: print("rotate around...") rejflaglist = [False, False] allminstdlist = [[], []] newtmppcdlist = [None, None] minstdlist = [None, None] for k in range(2): if toggledebug: print("checking over 100 and below 90, now: ", j) if len(tmppcdlist[k]) < 10: rejflaglist[k] = True continue for angle in np.linspace(0, 180, 10): tmphomomat = np.eye(4) tmphomomat[:3, :3] = rm.rodrigues( tubestand_homomat[:3, 2], angle) newtmppcdlist[k] = rm.homotransformpointarray( tmphomomat, tmppcdlist[k]) minstdlist[k] = np.min( np.std(newtmppcdlist[k][:, :2], axis=0)) if toggledebug: print(minstdlist[k]) allminstdlist[k].append(minstdlist[k]) if minstdlist[k] < 1.5: rejflaglist[k] = True if toggledebug: print("rotate round done") print("minstd ", np.min(np.asarray(allminstdlist[k]))) if all(item for item in rejflaglist): continue elif all(not item for item in rejflaglist): print("CANNOT tell if the tube is big or small") raise ValueError() else: tmppcd = tmppcdbelow90 if rejflaglist[ 0] else tmppcdover100 candidatetype = 2 if rejflaglist[0] else 1 tmpangles = np.arctan2(tmppcd[:, 1], tmppcd[:, 0]) tmpangles[ tmpangles < 0] = 360 + tmpangles[tmpangles < 0] if toggledebug: print(np.std(tmpangles)) print("ACCEPTED! ID: ", i, j) elearray[i][j] = candidatetype eleconfidencearray[i][j] = 1 if toggledebug: # normalized objnp = p3dh.genpointcloudnodepath(tmppcd, pntsize=5) rgb = np.random.rand(3) objnp.setColor(rgb[0], rgb[1], rgb[2], 1) objnp.reparentTo(base.render) stick = p3dh.gendumbbell( spos=np.array([holepos[0], holepos[1], 10]), epos=np.array([holepos[0], holepos[1], 60])) stick.setColor(rgb[0], rgb[1], rgb[2], 1) stick.reparentTo(base.render) # original tmppcd_tr = rm.homotransformpointarray( tubestand_homomat, tmppcd) objnp_tr = p3dh.genpointcloudnodepath(tmppcd_tr, pntsize=5) objnp_tr.setColor(rgb[0], rgb[1], rgb[2], 1) objnp_tr.reparentTo(base.render) spos_tr = rm.homotransformpoint( tubestand_homomat, np.array([holepos[0], holepos[1], 0])) stick_tr = p3dh.gendumbbell( spos=np.array([spos_tr[0], spos_tr[1], 10]), epos=np.array([spos_tr[0], spos_tr[1], 60])) stick_tr.setColor(rgb[0], rgb[1], rgb[2], 1) stick_tr.reparentTo(base.render) # box normalized center, bounds = rm.get_aabb(tmppcd) boxextent = np.array([ bounds[0, 1] - bounds[0, 0], bounds[1, 1] - bounds[1, 0], bounds[2, 1] - bounds[2, 0] ]) boxhomomat = np.eye(4) boxhomomat[:3, 3] = center box = p3dh.genbox( extent=boxextent, homomat=boxhomomat, rgba=np.array([rgb[0], rgb[1], rgb[2], .3])) box.reparentTo(base.render) # box original center_r = rm.homotransformpoint( tubestand_homomat, center) boxhomomat_tr = copy.deepcopy(tubestand_homomat) boxhomomat_tr[:3, 3] = center_r box_tr = p3dh.genbox( extent=boxextent, homomat=boxhomomat_tr, rgba=np.array([rgb[0], rgb[1], rgb[2], .3])) box_tr.reparentTo(base.render) if toggledebug: print("------the new test is done------") return elearray, eleconfidencearray
def gencm(self, startposobj, startrotobj, startposrbt, startrotrbt, isrotated = False, iscornered = False, isdrooped = False, isinclined = False): """ make the collision model of the object, set its pose according to the robot end effector pose :param startposobj: object position :param startrotobj: object orientation :param startposrbt: robot position :param startrotrbt: robot orientation :param isrotated: is the object to be rotated 90 degrees about the vertical axis of the world frame :param iscornered:is the object grasped from its corner :param isdrooped: is the object drooped due to orientation :param isinclined: is the object at an inclined pose :return: collision model of the object with set pose """ #load the object and set its pose related to the robot pose startposobj = startposobj startrotobj = startrotobj startpos = startposrbt startrot = startrotrbt #create the collision model self.objcm = cm.CollisionModel(objinit=self.objpath) # if required to flip the object and the robot pose # startrot = rot_transform(startrot, 180) # set the object pose in hand # easy to be automated for planning if isrotated: self.torque = 9.81 * self.m * self.width / 2 / 1000 # Nm print("rotated torque is set!") print("self.torque is:", self.torque) rotz = rm.rodrigues([0, 0, 1], -90) tmp_rot = np.dot(rotz, startrotobj) startrotobj = np.dot(tmp_rot, startrotobj) startposobj[0] -= (self.length/2-self.width/2) if isinclined: incline_angle = 10 tmp_incline_rot = rm.rodrigues(startrot[:,2],incline_angle) startrot = np.dot(tmp_incline_rot,startrot) startrotobj = np.dot(tmp_incline_rot, startrotobj) objcmcopy = copy.deepcopy(self.objcm) objcmcopy.setMat(pg.npToMat4(startrotobj,startposobj)) ##for visualization of the limit # for incangle in range(0,20,10): # print (incangle) # tmp_incline_rot = rm.rodrigues(startrot[:, 2], incangle) # startrotobjinc = np.dot(tmp_incline_rot, startrotobj) # objcmcopy = copy.deepcopy(objcm) # startrotrbt = np.dot(tmp_incline_rot, startrot) # rbtangles = rbt.numik(startpos, startrotrbt, "lft") # rbt.movearmfk(rbtangles, "lft") # rbtmg.genmnp(rbt, jawwidthlft=objheight,toggleendcoord=False).reparentTo(base.render) # objcmcopy.setMat(pg.npToMat4(startrotobjinc, startposobj)) # if incangle == 10: # objcmcopy.setColor(.8, .0, .0, .5) # objcmcopy.reparentTo(base.render) # base.run() if iscornered: corner_angle = 45 rotz = rm.rodrigues([0,0,1],corner_angle) tmp_rot = np.dot(rotz,startrotobj) startrotobj = np.dot(tmp_rot,startrotobj) grasppnt = [-self.length/2, self.width/2,0,1] obj_w_t = np.eye(4) obj_w_t[:3,:3] = startrotobj obj_w_t[:3,3] = startposobj print("obj_w_t",obj_w_t) grasppnt_w = np.dot(obj_w_t,grasppnt) print(grasppnt_w) print(startpos) pos_diff = [a-b for a,b in zip(grasppnt_w,startpos)] print ("Position difference is: ", pos_diff) print("Position difference is: ", [grasppnt_w[0],grasppnt_w[1],grasppnt_w[2]]-startpos) startposobj-=pos_diff if isdrooped == True: droop_angle = -20 tmp_rot_ee_x = rm.rodrigues(startrot[:,0],droop_angle) startrotobj = np.dot(tmp_rot_ee_x,startrotobj) # grasppnt = [-objlength/2*np.cos(droop_angle), objwidth/2*np.sin(droop_angle), 0, 1] # obj_w_t = np.eye(4) # obj_w_t[:3, :3] = startrotobj # obj_w_t[:3, 3] = startposobj # grasppnt_w = np.dot(obj_w_t, grasppnt) # print(grasppnt_w) # print(startpos) # pos_diff = [a - b for a, b in zip(grasppnt_w, startpos)] # print("Position difference is: ", pos_diff) # print("Position difference is: ", [grasppnt_w[0], grasppnt_w[1], grasppnt_w[2]] - startpos) # startposobj += pos_diff self.objcm.setMat(pg.npToMat4(startrotobj,startposobj)) return self.objcm
obscmlist = env.getstationaryobslist() for obscm in obscmlist: obscm.showcn() objcm = env.loadobj("bunnysim.stl") objcm.setColor(.2, .5, 0, 1) objcm.setPos(400, -200, 100) objcm.reparentTo(base.render) objcm.showcn() objpos = np.array([400, -300, 100]) 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,100], box_extents=[200,50,100])) objcm3.setColor(1,0,0,1) objcm3.reparentTo(base.render) objcm3.showcn() hndfa = yumiintegrated.YumiIntegratedFactory() rgthnd = hndfa.genHand() lfthnd = hndfa.genHand() rbt = robot.YumiRobot(rgthnd=rgthnd, lfthnd=lfthnd) rbtmg = robotmesh.YumiMesh() rbtmnp = rbtmg.genmnp(rbt, toggleendcoord=True) rbtmnp.reparentTo(base.render) base.run()
def __init__(self, *args, **kwargs): """ load the robotiq85 model, set jaw_width :param args: :param kwargs: 'jaw_width' 0-85 'ftsensoroffset' the offset for forcesensor 'toggleframes' True, False author: weiwei date: 20160627, 20190518, 20190824osaka, 20200321osaka """ if 'jawwidthopen' in kwargs: self.__jawwidthopen = kwargs['jawwidthopen'] else: self.__jawwidthopen = 100 if 'jawwidthclose' in kwargs: self.__jawwidthclose = kwargs['jawwidthclose'] else: self.__jawwidthclose = 0 if 'jaw_width' in kwargs: self.__jawwidth = kwargs['jaw_width'] else: self.__jawwidth = self.__jawwidthopen if 'ftsensoroffset' in kwargs: self.__ftsensoroffset = kwargs['ftsensoroffset'] if 'toggleframes' in kwargs: self.__toggleframes = kwargs['toggleframes'] if 'hndbase' in kwargs: self.__hndbase = cp.deepcopy(kwargs['hndbase']) # for copy self.__hndbase_bk = cp.deepcopy(kwargs['hndbase']) if 'hndfingerprox' in kwargs: self.__finger1_prox = cp.deepcopy(kwargs['hndfingerprox']) self.__finger2_prox = cp.deepcopy(kwargs['hndfingerprox']) self.__finger3_prox = cp.deepcopy(kwargs['hndfingerprox']) # for copy self.__hndfingerprox_bk = cp.deepcopy(kwargs['hndfingerprox']) if 'hndfingermed' in kwargs: self.__finger1_med = cp.deepcopy(kwargs['hndfingermed']) self.__finger2_med = cp.deepcopy(kwargs['hndfingermed']) self.__finger3_med = cp.deepcopy(kwargs['hndfingermed']) # for copy self.__hndfingermed_bk = cp.deepcopy(kwargs['hndfingermed']) if 'hndfingerdist' in kwargs: self.__finger1_dist = cp.deepcopy(kwargs['hndfingerdist']) self.__finger2_dist = cp.deepcopy(kwargs['hndfingerdist']) self.__finger3_dist = cp.deepcopy(kwargs['hndfingerdist']) # for copy self.__hndfingerdist_bk = cp.deepcopy(kwargs['hndfingerdist']) self.__name = "barrett-bh8-282" self.__hndnp = NodePath(self.__name) self.__angle_open = self._compute_jawangle(self.__jawwidthopen) self.__angle_close = self._compute_jawangle(self.__jawwidthclose) print(self.__angle_open, self.__angle_close) baselength = 79.5 fingerlength = 100 # eetippos/eetiprot self.__eetip = np.array([ 0.0, 0.0, baselength + fingerlength ]) + np.array([0.0, 0.0, self.__ftsensoroffset]) # max length 136 # base # self.__hndbase.setrotmat(rm.rodrigues(np.array([0,0,1]), 180)) self.__hndbase.setpos(np.array([0.0, 0.0, self.__ftsensoroffset])) # ftsensor if self.__ftsensoroffset > 0: self.__ftsensor = cm.CollisionModel(tp.Cylinder( height=self.__ftsensoroffset, radius=30), name="ftsensor") self.__ftsensor.setPos(0, 0, -self.__ftsensoroffset / 2) self.__ftsensor.reparentTo(self.__hndbase) else: self.__ftsensor = None # 1,2 are the index and middle fingers, 3 is the thumb self.__finger1_prox_pos = np.array([-25, 0, 41.5]) self.__finger1_prox_rot = rm.rotmat_from_euler(0, 0, -90) self.__finger1_med_pos = np.array([50, 0, 33.9]) self.__finger1_med_rot = rm.rotmat_from_euler(90, 0, 0) self.__finger1_dist_pos = np.array([69.94, 3, 0]) self.__finger1_dist_rot = rm.rotmat_from_euler(0, 0, -3) self.__finger2_prox_pos = np.array([25, 0, 41.5]) self.__finger2_prox_rot = rm.rotmat_from_euler(0, 0, -90) self.__finger2_med_pos = np.array([50, 0, 33.9]) self.__finger2_med_rot = rm.rotmat_from_euler(90, 0, 0) self.__finger2_dist_pos = np.array([69.94, 3, 0]) self.__finger2_dist_rot = rm.rotmat_from_euler(0, 0, -3) self.__finger3_med_pos = np.array([0, 50, 75.4]) self.__finger3_med_rot = rm.rotmat_from_euler(90, 0, 90) self.__finger3_dist_pos = np.array([69.94, 3, 0]) self.__finger3_dist_rot = rm.rotmat_from_euler(0, 0, -3) # controllable angles self.__angle_main = 0.0 self.__angle_finger1 = 0.0 self.__angle_finger2 = 0.0 self.__angle_finger3 = 0.0 # angle ranges self.__am_range = {"rngmin": 0, "rngmax": 180} self.__af1_range = {"rngmin": 0, "rngmax": 140} self.__af2_range = {"rngmin": 0, "rngmax": 140} self.__af3_range = {"rngmin": 0, "rngmax": 140} # update finger positions # finger 1 finger1_prox_pos = self.__finger1_prox_pos finger1_prox_rot = np.dot( self.__finger1_prox_rot, rm.rodrigues(np.array([0, 0, 1]), -self.__angle_main)) self.__finger1_prox.sethomomat( rm.homobuild(finger1_prox_pos, finger1_prox_rot)) finger1_med_pos = finger1_prox_pos + np.dot(finger1_prox_rot, self.__finger1_med_pos) finger1_med_rot = np.dot( finger1_prox_rot, np.dot(self.__finger1_med_rot, rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger1))) self.__finger1_med.sethomomat( rm.homobuild(finger1_med_pos, finger1_med_rot)) finger1_dist_pos = finger1_med_pos + np.dot(finger1_med_rot, self.__finger1_dist_pos) finger1_dist_rot = np.dot( finger1_med_rot, np.dot(self.__finger1_dist_rot, rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger1 / 3))) self.__finger1_dist.sethomomat( rm.homobuild(finger1_dist_pos, finger1_dist_rot)) self.__finger1_prox.reparentTo(self.__hndbase) self.__finger1_med.reparentTo(self.__hndbase) self.__finger1_dist.reparentTo(self.__hndbase) # finger 2 finger2_prox_pos = self.__finger2_prox_pos finger2_prox_rot = np.dot( self.__finger2_prox_rot, rm.rodrigues(np.array([0, 0, 1]), self.__angle_main)) self.__finger2_prox.sethomomat( rm.homobuild(finger2_prox_pos, finger2_prox_rot)) finger2_med_pos = finger2_prox_pos + np.dot(finger2_prox_rot, self.__finger2_med_pos) finger2_med_rot = np.dot( finger2_prox_rot, np.dot(self.__finger2_med_rot, rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger2))) self.__finger2_med.sethomomat( rm.homobuild(finger2_med_pos, finger2_med_rot)) finger2_dist_pos = finger2_med_pos + np.dot(finger2_med_rot, self.__finger2_dist_pos) finger2_dist_rot = np.dot( finger2_med_rot, np.dot(self.__finger2_dist_rot, rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger2 / 3))) self.__finger2_dist.sethomomat( rm.homobuild(finger2_dist_pos, finger2_dist_rot)) self.__finger2_prox.reparentTo(self.__hndbase) self.__finger2_med.reparentTo(self.__hndbase) self.__finger2_dist.reparentTo(self.__hndbase) # finger 3 finger3_med_pos = self.__finger3_med_pos finger3_med_rot = np.dot( self.__finger3_med_rot, rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger3)) self.__finger3_med.sethomomat( rm.homobuild(finger3_med_pos, finger3_med_rot)) finger3_dist_pos = finger3_med_pos + np.dot(finger3_med_rot, self.__finger3_dist_pos) finger3_dist_rot = np.dot( finger3_med_rot, np.dot(self.__finger3_dist_rot, rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger3 / 3))) self.__finger3_dist.sethomomat( rm.homobuild(finger3_dist_pos, finger3_dist_rot)) # self.__finger3_med.reparentTo(self.__hndbase) self.__finger3_dist.reparentTo(self.__hndbase) self.__hndbase.reparentTo(self.__hndnp) # self.setjawwidth(self.__jawwidth) self.setDefaultColor() if self.__toggleframes: if self.__ftsensor is not None: self.__ftsensorframe = p3dh.genframe() self.__ftsensorframe.reparentTo(self.__hndnp) self.__hndframe = p3dh.genframe() self.__hndframe.reparentTo(self.__hndnp) self.__baseframe = p3dh.genframe() self.__baseframe.reparentTo(self.__hndbase.objnp) self.__finger1_prox_frame = p3dh.genframe() self.__finger1_prox_frame.reparentTo(self.__finger1_prox.objnp) self.__finger1_med_frame = p3dh.genframe() self.__finger1_med_frame.reparentTo(self.__finger1_med.objnp) self.__finger1_dist_frame = p3dh.genframe() self.__finger1_dist_frame.reparentTo(self.__finger1_dist.objnp) self.__finger2_prox_frame = p3dh.genframe() self.__finger2_prox_frame.reparentTo(self.__finger2_prox.objnp) self.__finger2_med_frame = p3dh.genframe() self.__finger2_med_frame.reparentTo(self.__finger2_med.objnp) self.__finger2_dist_frame = p3dh.genframe() self.__finger2_dist_frame.reparentTo(self.__finger2_dist.objnp) self.__finger3_med_frame = p3dh.genframe() self.__finger3_med_frame.reparentTo(self.__finger3_med.objnp) self.__finger3_dist_frame = p3dh.genframe() self.__finger3_dist_frame.reparentTo(self.__finger3_dist.objnp)
def __init__(self, betransparent=False): """ load obstacles model separated by category :param base: author: weiwei date: 20181205 """ self.__this_dir, _ = os.path.split(__file__) # body self.__bodypath = os.path.join(self.__this_dir, "obstacles", "ur3body.stl") self.__bodycm = cm.CollisionModel(self.__bodypath, betransparent) self.__bodycm.setColor(.3, .3, .3, 1.0) # table self.__tablepath = os.path.join(self.__this_dir, "obstacles", "ur3dtablefree.stl") self.__tablecm = cm.CollisionModel(self.__tablepath, betransparent) self.__tablecm.setColor(.55, .55, .5, 1.0) self.__tablecm.setPos(50.0, 0.0, 0.0) # housing ## housing pillar ## TODO these models could be replaced by trimesh.primitives self.__beam1200path = os.path.join(self.__this_dir, "obstacles", "ur3housing1200.stl") self.__beam1500path = os.path.join(self.__this_dir, "obstacles", "ur3housing1500.stl") self.__beam2100path = os.path.join(self.__this_dir, "obstacles", "ur3housing2100.stl") self.__pillarfrontrgtcm = cm.CollisionModel(self.__beam2100path) self.__pillarfrontrgtcm.setColor(.5, .5, .55, 1.0) self.__pillarfrontrgtcm.setPos(860.0 + 50.0 + 30.0, -780.0, 0.0) self.__pillarfrontlftcm = cm.CollisionModel(self.__beam2100path) self.__pillarfrontlftcm.setColor(.5, .5, .55, 1.0) self.__pillarfrontlftcm.setPos(860.0 + 50.0 + 30.0, 780.0, 0.0) self.__pillarbackrgtcm = cm.CollisionModel(self.__beam2100path) self.__pillarbackrgtcm.setColor(.5, .5, .55, 1.0) self.__pillarbackrgtcm.setPos(860 + 50 - 1230, -780.0, 0.0) self.__pillarbacklftcm = cm.CollisionModel(self.__beam2100path) self.__pillarbacklftcm.setColor(.5, .5, .55, 1.0) self.__pillarbacklftcm.setPos(860 + 50 - 1230, 780.0, 0.0) ## housing rgt self.__rowrgtbottomcm = cm.CollisionModel(self.__beam1200path) self.__rowrgtbottomcm.setColor(.5, .5, .55, 1.0) self.__rowrgtbottomcm.setPos(860 + 50 - 600, -780.0, 75.0) self.__rowrgtmiddlecm = cm.CollisionModel(self.__beam1200path) self.__rowrgtmiddlecm.setColor(.5, .5, .55, 1.0) self.__rowrgtmiddlecm.setPos(860 + 50 - 600, -780.0, 867.0) self.__rowrgttopcm = cm.CollisionModel(self.__beam1200path) self.__rowrgttopcm.setColor(.5, .5, .55, 1.0) self.__rowrgttopcm.setPos(860 + 50 - 600, -780.0, 2100.0 - 60.0) ## housing lft self.__rowlftbottomcm = cm.CollisionModel(self.__beam1200path) self.__rowlftbottomcm.setColor(.5, .5, .55, 1.0) self.__rowlftbottomcm.setPos(860 + 50 - 600, 780.0, 75.0) self.__rowlfttopcm = cm.CollisionModel(self.__beam1200path) self.__rowlfttopcm.setColor(.5, .5, .55, 1.0) self.__rowlfttopcm.setPos(860 + 50 - 600, 780.0, 2100.0 - 60.0) ## housing front self.__rowfrontbottomcm = cm.CollisionModel(self.__beam1500path) self.__rowfrontbottomcm.setColor(.5, .5, .55, 1.0) self.__rowfrontbottomcm.setPos(860 + 50 + 30, 0.0, 75.0) self.__rowfrontmiddlecm = cm.CollisionModel(self.__beam1500path) self.__rowfrontmiddlecm.setColor(.5, .5, .55, 1.0) self.__rowfrontmiddlecm.setPos(860 + 50 + 30, 0.0, 867.0) self.__rowfronttopcm = cm.CollisionModel(self.__beam1500path) self.__rowfronttopcm.setColor(.5, .5, .55, 1.0) self.__rowfronttopcm.setPos(860 + 50 + 30, 0.0, 2100.0 - 60.0) ## housing back self.__rowbackbottomcm = cm.CollisionModel(self.__beam1500path) self.__rowbackbottomcm.setColor(.5, .5, .55, 1.0) self.__rowbackbottomcm.setPos(860 + 50 - 1230, 0.0, 2100.0 - 530.0 - 60.0) self.__rowbackmiddlecm = cm.CollisionModel(self.__beam1500path) self.__rowbackmiddlecm.setColor(.5, .5, .55, 1.0) self.__rowbackmiddlecm.setPos(860 + 50 - 1230, 0.0, 2100.0 - 264.0 - 60.0) self.__rowbacktopcm = cm.CollisionModel(self.__beam1500path) self.__rowbacktopcm.setColor(.5, .5, .55, 1.0) self.__rowbacktopcm.setPos(860 + 50 - 1230, 0.0, 2100.0 - 60.0) self.__battached = False self.__changableobslist = []
# objpath = "../objects/025mug1000.stl" # objpath = "../objects/025mug3000.stl" # objpath = "../objects/025mug3000_tsdf.stl" # objpath = "../objects/035drill3000_tsdf.stl" # objpath = "../objects/072toyplanedrill3000_tsdf.stl" # objpath = "../objects/housing.stl" # objpath = "../objects/ttube.stl" # objpath = "./objects/bunny12356.stl" # objstpath = "./objects/bunny17432.stl" pathname = "./objects/" objname = ["housing", "planewheel", "tool2", "ttube", "sandpart", "planelowerbody", "planerearstay", "bunnysim"] type = ["160over", "160noover"] for objn in objname: objpath = pathname+objn+".stl" objcm = cm.CollisionModel(objinit = objpath) hndfa = hnd.Robotiq85Factory() for i in range(2): hand= hndfa.genHand() if i == 0: freegriptst = fg.Freegrip(objpath, hand, faceangle = .9, segangle = .9, refine1min=5, fpairparallel=-0.9, togglebcdcdebug=True, useoverlap=True) if i == 1: freegriptst = fg.Freegrip(objpath, hand, faceangle = .9, segangle = .9, refine1min=5, fpairparallel=-0.9, togglebcdcdebug=True, useoverlap=False) # facetsizes = [] # for j, faces in enumerate(freegriptst.facets): # rgba = [np.random.random(), np.random.random(), np.random.random(), 1] # # compute facet normal # facetnormal = np.sum(freegriptst.objtrimesh.face_normals[faces], axis=0) # facetnormal = facetnormal / np.linalg.norm(facetnormal)
import utiltools.thirdparty.p3dhelper as p3dh import utiltools.thirdparty.o3dhelper as o3dh import pickle import tro.tro_pickplaceplanner as ppp if __name__ == '__main__': yhx = robothelper.RobotHelperX(usereal=True) yhx.movetox(yhx.robot_s.initrgtjnts, armname="rgt") yhx.movetox(yhx.robot_s.initlftjnts, armname="lft") yhx.closegripperx(armname="rgt") # yhx.closegripperx(arm_name="lft") # yhx.opengripperx(arm_name="rgt") # yhx.opengripperx(arm_name="lft") objcm = cm.CollisionModel("../objects/vacuumhead.stl") # yhx = robothelper.RobotHelper(startworld=True) armjnts0 = np.array( [120.18, -41.71, -160.45, 7.16, 148.59, 95.69, -210.39]) obstaclecmlist = [] armname = "rgt" rgtinitarmjnts = yhx.robot_s.getarmjnts(armname=armname).tolist() primitivedirection_backward = np.array([0, 0, 1]) primitivedistance_backward = 150 rgtupmotion = yhx.genmovebackwardmotion(primitivedirection_backward, primitivedistance_backward, armjnts0, obstaclecmlist, armname) rgtpickmotion = rgtupmotion[::-1] rgtrrtgotobeforepick = yhx.planmotion(rgtinitarmjnts, rgtpickmotion[0], obstaclecmlist, armname)
freesuctst = fs.Freesuc() reconstructedtrimeshlist, nppcdlist = o3dh.reconstruct_surfaces_bp( nppcd, radii=(10, 12)) # for i, tmpnppcd in enumerate(nppcdlist): # p3dpcd = p3dh.genpointcloudnodepath(tmpnppcd, pntsize=1.57) # p3dpcd.reparentTo(rhx.base.render) # if i == 0: # p3dpcd.setColor(.7,0,0,1) # elif i == 1: # p3dpcd.setColor(0,0,.7,1) # elif i == 2: # p3dpcd.setColor(0,.7,0,1) # else: # p3dpcd.setColor(1,1,1,1) for i, reconstructedtrimesh in enumerate(reconstructedtrimeshlist): reconstructedmeshobjcm = cm.CollisionModel(reconstructedtrimesh) reconstructedmeshobjcm.reparentTo(rhx.base.render) if i == 0: reconstructedmeshobjcm.setColor(.7, 0, 0, 1) elif i == 1: reconstructedmeshobjcm.setColor(0, 0, .7, 1) elif i == 2: reconstructedmeshobjcm.setColor(0, .7, 0, 1) else: reconstructedmeshobjcm.setColor(1, 1, 1, 1) freesuctst.plansuctions(effa=effa, objinit=reconstructedmeshobjcm, faceangle=.85, segangle=.85, mindist=10,
yifa = yi.YumiIntegratedFactory() reconstructedtrimeshlist, nppcdlist = o3dh.reconstruct_surfaces_bp( mergednppcd, mergednppcdnrmls, radii=(7, 10)) # for i, tmpnppcd in enumerate(nppcdlist): # p3dpcd = p3dh.genpointcloudnodepath(tmpnppcd, pntsize=1.57) # p3dpcd.reparentTo(rhx.base.render) # if i == 0: # p3dpcd.setColor(.7,0,0,1) # elif i == 1: # p3dpcd.setColor(0,0,.7,1) # elif i == 2: # p3dpcd.setColor(0,.7,0,1) # else: # p3dpcd.setColor(1,1,1,1) for i, reconstructedtrimesh in enumerate(reconstructedtrimeshlist): reconstructedmeshobjcm = cm.CollisionModel(reconstructedtrimesh) # reconstructedmeshobjcm.reparentTo(rhx.base.render) # if i == 1: # reconstructedmeshobjcm.setColor(.7,0,0,1) # elif i == 0: # reconstructedmeshobjcm.setColor(0,0,.7,1) # elif i == 2: # reconstructedmeshobjcm.setColor(0,.7,0,1) # else: # reconstructedmeshobjcm.setColor(1,1,1,1) freegriptst = fg.Freegrip(reconstructedmeshobjcm, yifa.genHand(), faceangle=.9, segangle=.9, refine1min=7,
hmnmg = humanmesh.NxtMesh() #build the cabinet cabinet = buildCabinet(base, isrendercoord=True) cabinet.reparentTo(base.render) cabinetpose = cabinet.getMat() cabinetposenp4 = base.pg.mat4ToNp(cabinetpose) __this_dir, _ = os.path.split(__file__) largeboardpath = os.path.join(__this_dir, "objects", "largeboard.stl") middleboardpath = os.path.join(__this_dir, "objects", "middleboard.stl") smallboardpath = os.path.join(__this_dir, "objects", "smallboard.stl") # the boards largeboard0 = cm.CollisionModel(largeboardpath, radius=3, betransparency=True) middleboard0 = cm.CollisionModel(middleboardpath, radius=3, betransparency=True) smallboard0 = cm.CollisionModel(smallboardpath, radius=3, betransparency=True) # the obstacle boards largeboardobstacle0 = copy.deepcopy(largeboard0) largecompundrot = np.dot(rm.rodrigues([0, 1, 0], -90), rm.rodrigues([1, 0, 0], 90)) largeboardobstacle0.sethomomat( rm.homobuild( np.array([0, 195, 293.5]) + cabinetposenp4[:3, 3],
def __init__(self, betransparent=False): """ load obstacles model separated by category :param base: author: weiwei date: 20181205 """ self.__this_dir, this_filename = os.path.split(__file__) # body self.__bodypath = os.path.join(self.__this_dir, "obstacles", "ur3body.stl") self.__bodycm = cm.CollisionModel(self.__bodypath, betransparent) self.__bodycm.setColor(.3,.3,.3,1.0) # table self.__tablepath = os.path.join(self.__this_dir, "obstacles", "ur3dtable.stl") self.__tablecm = cm.CollisionModel(self.__tablepath, betransparent) self.__tablecm.setColor(.6,.6,.6,1.0) self.__tablecm.setPos(50.0, 0.0, 0.0) # housing ## housing pillar self.__beam1200path = os.path.join(self.__this_dir, "obstacles", "ur3housing1200.stl") self.__beam1500path = os.path.join(self.__this_dir, "obstacles", "ur3housing1500.stl") self.__beam2100path = os.path.join(self.__this_dir, "obstacles", "ur3housing2100.stl") self.__pillarfrontrgtcm = cm.CollisionModel(self.__beam2100path) self.__pillarfrontrgtcm.setColor(.6,.6,.6,1.0) self.__pillarfrontrgtcm.setPos(860.0+50.0+30.0, -780.0, 0.0) self.__pillarfrontlftcm = cm.CollisionModel(self.__beam2100path) self.__pillarfrontlftcm.setColor(.6,.6,.6,1.0) self.__pillarfrontlftcm.setPos(860.0+50.0+30.0, 780.0, 0.0) self.__pillarbackrgtcm = cm.CollisionModel(self.__beam2100path) self.__pillarbackrgtcm.setColor(.6,.6,.6,1.0) self.__pillarbackrgtcm.setPos(860+50-1200, -780.0, 0.0) self.__pillarbacklftcm = cm.CollisionModel(self.__beam2100path) self.__pillarbacklftcm.setColor(.6,.6,.6,1.0) self.__pillarbacklftcm.setPos(860+50-1200, 780.0, 0.0) ## housing rgt self.__rowrgtbottomcm = cm.CollisionModel(self.__beam1200path) self.__rowrgtbottomcm.setColor(.6,.6,.6,1.0) self.__rowrgtbottomcm.setPos(860+50-600, -780.0, 75.0) self.__rowrgtmiddlecm = cm.CollisionModel(self.__beam1200path) self.__rowrgtmiddlecm.setColor(.6,.6,.6,1.0) self.__rowrgtmiddlecm.setPos(860+50-600, -780.0, 867.0) self.__rowrgttopcm = cm.CollisionModel(self.__beam1200path) self.__rowrgttopcm.setColor(.6,.6,.6,1.0) self.__rowrgttopcm.setPos(860+50-600, -780.0, 2100.0-60.0) ## housing lft self.__rowlftbottomcm = cm.CollisionModel(self.__beam1200path) self.__rowlftbottomcm.setColor(.6,.6,.6,1.0) self.__rowlftbottomcm.setPos(860+50-600, 780.0, 75.0) self.__rowlfttopcm = cm.CollisionModel(self.__beam1200path) self.__rowlfttopcm.setColor(.6,.6,.6,1.0) self.__rowlfttopcm.setPos(860+50-600, 780.0, 2100.0-60.0) ## housing front self.__rowfrontbottomcm = cm.CollisionModel(self.__beam1500path) self.__rowfrontbottomcm.setColor(.6,.6,.6,1.0) self.__rowfrontbottomcm.setPos(860+50+30, 0.0, 75.0) self.__rowfrontmiddlecm = cm.CollisionModel(self.__beam1500path) self.__rowfrontmiddlecm.setColor(.6,.6,.6,1.0) self.__rowfrontmiddlecm.setPos(860+50+30, 0.0, 867.0) # self.__rowfronttopcm = cm.CollisionModel(self.__beam1500path) # self.__rowfronttopcm.setColor(.6,.6,.6,1.0) # self.__rowfronttopcm.setPos(860+50+30, 0.0, 2100.0-60.0) ## housing back self.__rowbackbottomcm = cm.CollisionModel(self.__beam1500path) self.__rowbackbottomcm.setColor(.6,.6,.6,1.0) self.__rowbackbottomcm.setPos(860+50-1200, 0.0, 2100.0-530.0-60.0) self.__rowbackmiddlecm = cm.CollisionModel(self.__beam1500path) self.__rowbackmiddlecm.setColor(.6,.6,.6,1.0) self.__rowbackmiddlecm.setPos(860+50-1200, 0.0, 2100.0-264.0-60.0) self.__rowbacktopcm = cm.CollisionModel(self.__beam1500path) self.__rowbacktopcm.setColor(.6,.6,.6,1.0) self.__rowbacktopcm.setPos(860+50-1200, 0.0, 2100.0-60.0) # assebmly frame self.__assshelfpath = os.path.join(self.__this_dir, "obstacles", "ur3dassshelf.stl") self.__asssupportpath = os.path.join(self.__this_dir, "obstacles", "asssupport.stl") self.__assbasepath = os.path.join(self.__this_dir, "obstacles", "assbase.stl") self.__assvertplanebigpath = os.path.join(self.__this_dir, "obstacles", "assvertplanebig.stl") self.__assshelfcm = cm.CollisionModel(self.__assshelfpath) self.__assshelfcm.setColor(.7,.7,.6,1.0) self.__assshelfcm.setPos(50.0, 65.0,1000.0) self.__asssupportcm = cm.CollisionModel(self.__asssupportpath) self.__asssupportcm.setColor(.2,.2,.2,1.0) self.__asssupportcm.setPos(110.0, 65.0,1190.0) self.__assbasecm = cm.CollisionModel(self.__assbasepath) self.__assbasecm.setColor(.7,.72,.75,1.0) self.__assbasecm.setPos(130.0, 65.0,1220.0) self.__assvertplanebigcm= cm.CollisionModel(self.__assvertplanebigpath) self.__assvertplanebigcm.setColor(.7,.72,.75,1.0) self.__assvertplanebigcm.setPos(330.0, 42.0,1230.0) self.__battached = False self.__changableobslist = []
def loadobj(self, name): self.__objpath = os.path.join(self.__this_dir, "objects", name) self.__objcm = cm.CollisionModel(self.__objpath, type="ball") return self.__objcm
def buildCabinet(base, cabpos=np.array([773.5, -55.17, 1035]), cabrotangle=0, isrendercoord=False): __this_dir, _ = os.path.split(__file__) cabinet = NodePath("cabinet") #build the cabinet #the middle board, 3 pieces middleboardpath = os.path.join(__this_dir, "objects", "middleboard.stl") middleboard0 = cm.CollisionModel(middleboardpath, radius=3, betransparency=True) middleboard0.setColor(1, 0, 0, 1) temprotmiddleboard = rm.rodrigues([0, 0, 1], 90) rotmiddleboardmat4 = middleboard0.getMat() rotmiddleboardnp4 = base.pg.mat4ToNp(rotmiddleboardmat4) rotmiddleboardnp4[:3, :3] = np.dot(temprotmiddleboard, rotmiddleboardnp4[:3, :3]) rotmiddleboardmat4 = base.pg.np4ToMat4(rotmiddleboardnp4) middleboard0.setMat(rotmiddleboardmat4) # temprotsmallboard = np.dot() middleboard1 = copy.deepcopy(middleboard0) middleboard1.setPos(0, 0, 288.5) middleboard2 = copy.deepcopy(middleboard0) temprotmiddleboard = rm.rodrigues([1, 0, 0], 180) rotmiddleboardnp4[:3, :3] = np.dot(temprotmiddleboard, rotmiddleboardnp4[:3, :3]) rotmiddleboardnp4[:3, 3] = np.array([0, 0, 587]) middleboard2.sethomomat(rotmiddleboardnp4) # middleboard2.setPos(0,0,577) middleboard0.setColor(.4, .4, .4, .7) #this 0 doesn't need to setpos middleboard0.reparentTo(cabinet) middleboard1.setColor(.4, .4, .4, .7) middleboard1.reparentTo(cabinet) middleboard2.setColor(.4, .4, .4, .7) middleboard2.reparentTo(cabinet) largeboardpath = os.path.join(__this_dir, "objects", "largeboard.stl") largeboard0 = cm.CollisionModel(largeboardpath, radius=3, betransparency=True) largeboard1 = copy.deepcopy(largeboard0) largecompundrot0 = np.dot(rm.rodrigues([0, 1, 0], -90), rm.rodrigues([1, 0, 0], 90)) largecompundrot1 = np.dot(rm.rodrigues([-1, 0, 0], 180), largecompundrot0) largeboard0.sethomomat( rm.homobuild(np.array([0, 195, 293.5]), largecompundrot0)) largeboard1.sethomomat( rm.homobuild(np.array([0, -195, 293.5]), largecompundrot1)) largeboard0.setColor(.4, .4, .4, .7) largeboard0.reparentTo(cabinet) largeboard1.setColor(.4, .4, .4, .7) largeboard1.reparentTo(cabinet) # the small board, 2 pieces smallboardpath = os.path.join(__this_dir, "objects", "smallboard.stl") smallboard0 = cm.CollisionModel(smallboardpath, radius=3, betransparency=True) smallboard1 = copy.deepcopy(smallboard0) smallcompundrot = np.dot(rm.rodrigues([0, 1, 0], 90), rm.rodrigues([0, 0, 1], -90)) smallboard0.sethomomat( rm.homobuild(np.array([-142.5, 0, 149.25]), smallcompundrot)) smallboard1.sethomomat( rm.homobuild(np.array([-142.5, 0, 587 - 149.25]), smallcompundrot)) smallboard0.setColor(.4, .4, .4, .7) smallboard0.reparentTo(cabinet) smallboard1.setColor(.4, .4, .4, .7) smallboard1.reparentTo(cabinet) if isrendercoord == True: middleboard0.showLocalFrame() middleboard1.showLocalFrame() middleboard2.showLocalFrame() largeboard0.showLocalFrame() largeboard1.showLocalFrame() smallboard0.showLocalFrame() smallboard1.showLocalFrame() #rotate the cabinet cabinetassemblypos = cabpos # on the edge is 773, a bit outside is 814 cabinetassemblyrot = rm.rodrigues([0, 0, 1], cabrotangle) cabinet.setMat( base.pg.np4ToMat4(rm.homobuild(cabinetassemblypos, cabinetassemblyrot))) return cabinet
return effect_grasps except: print("load failed, create new graqsp file or grasp first.") raise ValueError("File or data not found!") if __name__ == '__main__': import manipulation.grip.freegrip as fg import environment.bulletcdhelper as bcd base = pc.World(camp=[2000, -2000, 1500], lookatpos=[0, 0, 100], up=[0, -1, 1], autocamrotate=False) hndfa = yi.YumiIntegratedFactory() objcm = cm.CollisionModel(objinit="../objects/" + "vacuumhead.stl") gp = fg.Freegrip(objinit="../objects/" + "vacuumhead.stl", hand=hndfa.genHand()) bmc = bcd.MCMchecker(toggledebug=True) gp.segShow(base, togglesamples=False, togglenormals=False, togglesamples_ref=False, togglenormals_ref=False, togglesamples_refcls=False, togglenormals_refcls=False, alpha=1) predefinedgrasps = load_pickle_file_grip(objcm.name) counter = [0]
import environment.collisionmodel as cm import environment.bulletcdhelper as bch base = pc.World(camp = [3000,0,3000], lookatpos= [0, 0, 0]) base.pggen.plotAxis(base.render) # shape = BulletBoxShape(Vec3(50, 200, 450)) # node = BulletRigidBodyNode('Box') # node.setMass(1.0) # node.setAngularVelocity(Vec3(1,1,1)) # node.addShape(shape) # np = base.render.attachNewNode(node) # np.setPos(0, 0, 0) model = cm.CollisionModel("./objects/bunnysim.meshes") # model.reparentTo(base.render) # model.setMat(Mat4.rotateMat(10, Vec3(1,0,0))) # model.setPos(0,0,300) bulletnode = bch.genBulletCDMesh(model) bulletnode.setMass(1) rigidbody = base.render.attachNewNode(bulletnode) model.reparentTo(rigidbody) world = BulletWorld() world.setGravity(Vec3(0, 0, -9.8)) world.attach(bulletnode) def update(task): dt = globalClock.getDt()
def __init__(self, betransparent=False): """ load obstacles model separated by category :param base: author: weiwei date: 20181205 """ self.__this_dir, this_filename = os.path.split(__file__) # table self.__tablepath = os.path.join(self.__this_dir, "obstacles", "nxttable.stl") self.__tablecm = cm.CollisionModel(self.__tablepath, betransparent) self.__tablecm.setColor(.6, .6, .6, 1.0) self.__tablecm.setPos(190.0, 0.0, 0.0) # shelf self.__belowfrontbeampath = os.path.join( self.__this_dir, "obstacles", "nxt_belowfront_beam_800x30x30.stl") self.__belowleftbeampath = os.path.join( self.__this_dir, "obstacles", "nxt_belowleft_beam_600x30x30.stl") self.__frontleftbeampath = os.path.join( self.__this_dir, "obstacles", "nxt_frontleft_beam_850x30x30.stl") self.__topleftbeampath = os.path.join( self.__this_dir, "obstacles", "nxt_topleft_beam_300x30x30.stl") self.__framebelowrgtcm = cm.CollisionModel(self.__belowleftbeampath) self.__framebelowrgtcm.setColor(.6, .6, .6, 1.0) self.__framebelowrgtcm.setPos(300.0 + 190.0, -415.0, 912.0) self.__framebelowlftcm = cm.CollisionModel(self.__belowleftbeampath) self.__framebelowlftcm.setColor(.6, .6, .6, 1.0) self.__framebelowlftcm.setPos(300.0 + 190.0, 415.0, 912.0) self.__framebelowfrontcm = cm.CollisionModel(self.__belowfrontbeampath) self.__framebelowfrontcm.setColor(.6, .6, .6, 1.0) self.__framebelowfrontcm.setPos(600.0 - 15.0 + 190.0, 0.0, 912.0) self.__framebelowbackcm = cm.CollisionModel(self.__belowfrontbeampath) self.__framebelowbackcm.setColor(.6, .6, .6, 1.0) self.__framebelowbackcm.setPos(15.0 + 190.0, 0.0, 912.0) self.__framefrontrgtcm = cm.CollisionModel(self.__frontleftbeampath) self.__framefrontrgtcm.setColor(.6, .6, .6, 1.0) self.__framefrontrgtcm.setPos(600.0 - 15.0 + 190.0, -415.0, 912.0 + 425.0) self.__framefrontlftcm = cm.CollisionModel(self.__frontleftbeampath) self.__framefrontlftcm.setColor(.6, .6, .6, 1.0) self.__framefrontlftcm.setPos(600.0 - 15.0 + 190.0, 415.0, 912.0 + 425.0) self.__frametoprgtcm = cm.CollisionModel(self.__topleftbeampath) self.__frametoprgtcm.setColor(.6, .6, .6, 1.0) self.__frametoprgtcm.setPos(600.0 - 150.0 + 190.0, -415.0, 912.0 + 850.0 + 15.0) self.__frametoplftcm = cm.CollisionModel(self.__topleftbeampath) self.__frametoplftcm.setColor(.6, .6, .6, 1.0) self.__frametoplftcm.setPos(600.0 - 150.0 + 190.0, 415.0, 912.0 + 850.0 + 15.0) self.__frametopfrontcm = cm.CollisionModel(self.__belowfrontbeampath) self.__frametopfrontcm.setColor(.6, .6, .6, .3) self.__frametopfrontcm.setPos(600.0 - 15.0 + 190.0, 0.0, 912.0 + 850.0 + 15.0) self.__frametopbackcm = cm.CollisionModel(self.__belowfrontbeampath) self.__frametopbackcm.setColor(.6, .6, .6, .3) self.__frametopbackcm.setPos(600.0 - 300.0 + 15.0 + 190.0, 0.0, 912.0 + 850.0 + 15.0) # housing ## housing pillar # self.__beam1200path = os.path.join(self.__this_dir, "obstacles", "ur3housing1200.stl") # self.__beam1500path = os.path.join(self.__this_dir, "obstacles", "ur3housing1500.stl") # self.__beam2100path = os.path.join(self.__this_dir, "obstacles", "ur3housing2100.stl") # # self.__pillarfrontrgtcm = cm.CollisionModel(self.__beam2100path) # self.__pillarfrontrgtcm.setColor(.6,.6,.6,1.0) # self.__pillarfrontrgtcm.setPos(860.0+50.0+30.0, -780.0, 0.0) # self.__pillarfrontlftcm = cm.CollisionModel(self.__beam2100path) # self.__pillarfrontlftcm.setColor(.6,.6,.6,1.0) # self.__pillarfrontlftcm.setPos(860.0+50.0+30.0, 780.0, 0.0) # self.__pillarbackrgtcm = cm.CollisionModel(self.__beam2100path) # self.__pillarbackrgtcm.setColor(.6,.6,.6,1.0) # self.__pillarbackrgtcm.setPos(860+50-1200, -780.0, 0.0) # self.__pillarbacklftcm = cm.CollisionModel(self.__beam2100path) # self.__pillarbacklftcm.setColor(.6,.6,.6,1.0) # self.__pillarbacklftcm.setPos(860+50-1200, 780.0, 0.0) # ## housing rgt # self.__rowrgtbottomcm = cm.CollisionModel(self.__beam1200path) # self.__rowrgtbottomcm.setColor(.6,.6,.6,1.0) # self.__rowrgtbottomcm.setPos(860+50-600, -780.0, 75.0) # self.__rowrgtmiddlecm = cm.CollisionModel(self.__beam1200path) # self.__rowrgtmiddlecm.setColor(.6,.6,.6,1.0) # self.__rowrgtmiddlecm.setPos(860+50-600, -780.0, 867.0) # self.__rowrgttopcm = cm.CollisionModel(self.__beam1200path) # self.__rowrgttopcm.setColor(.6,.6,.6,1.0) # self.__rowrgttopcm.setPos(860+50-600, -780.0, 2100.0-60.0) # ## housing lft # self.__rowlftbottomcm = cm.CollisionModel(self.__beam1200path) # self.__rowlftbottomcm.setColor(.6,.6,.6,1.0) # self.__rowlftbottomcm.setPos(860+50-600, 780.0, 75.0) # self.__rowlfttopcm = cm.CollisionModel(self.__beam1200path) # self.__rowlfttopcm.setColor(.6,.6,.6,1.0) # self.__rowlfttopcm.setPos(860+50-600, 780.0, 2100.0-60.0) # ## housing front # self.__rowfrontbottomcm = cm.CollisionModel(self.__beam1500path) # self.__rowfrontbottomcm.setColor(.6,.6,.6,1.0) # self.__rowfrontbottomcm.setPos(860+50+30, 0.0, 75.0) # self.__rowfrontmiddlecm = cm.CollisionModel(self.__beam1500path) # self.__rowfrontmiddlecm.setColor(.6,.6,.6,1.0) # self.__rowfrontmiddlecm.setPos(860+50+30, 0.0, 867.0) # # self.__rowfronttopcm = cm.CollisionModel(self.__beam1500path) # # self.__rowfronttopcm.setColor(.6,.6,.6,1.0) # # self.__rowfronttopcm.setPos(860+50+30, 0.0, 2100.0-60.0) # ## housing back # self.__rowbackbottomcm = cm.CollisionModel(self.__beam1500path) # self.__rowbackbottomcm.setColor(.6,.6,.6,1.0) # self.__rowbackbottomcm.setPos(860+50-1200, 0.0, 2100.0-530.0-60.0) # self.__rowbackmiddlecm = cm.CollisionModel(self.__beam1500path) # self.__rowbackmiddlecm.setColor(.6,.6,.6,1.0) # self.__rowbackmiddlecm.setPos(860+50-1200, 0.0, 2100.0-264.0-60.0) # self.__rowbacktopcm = cm.CollisionModel(self.__beam1500path) # self.__rowbacktopcm.setColor(.6,.6,.6,1.0) # self.__rowbacktopcm.setPos(860+50-1200, 0.0, 2100.0-60.0) self.__battached = False self.__changableobslist = []
if __name__ == '__main__': yhx = robothelper.RobotHelperX(usereal=False) # tubestand # tscm = cm.CollisionModel(os.path.join(yhx.root, "objects", "tubestand.stl")) # tspcd, _ = tscm.samplesurface(radius=2) # tspcd = tspcd[tspcd[:,2]>55] # # pickle.dump(tspcd, open(os.path.join(yhx.root, "dataobjtemplate", "tubestandtemplatepcd.pkl"), "wb")) # base = yhx.startworld() # pcdcm = cm.CollisionModel(tspcd) # pcdcm.reparentTo(base.render) # base.run() # tubestand_light vhcm = cm.CollisionModel( os.path.join(yhx.root, "objects", "vacuumhead.stl")) vhpcd, _ = vhcm.samplesurface(radius=2) vhpcd = vhpcd[vhpcd[:, 1] < -5] # pickle.dump( vhpcd, open( os.path.join(yhx.root, "dataobjtemplate", "vacuumhead_templatepcd.pkl"), "wb")) pcdcm = cm.CollisionModel(vhpcd) pcdcm.reparentTo(yhx.base.render) yhx.base.run() # tubestand_light tscm = cm.CollisionModel( os.path.join(yhx.root, "objects", "tubestand_light.stl"))