import os import database.dbaccess as db from manipulation.grip.robotiq85 import rtq85nm from manipulation.grip import freegrip from pandaplotutils import pandactrl base = pandactrl.World(camp=[700, 300, 700], lookatp=[0, 0, 100]) this_dir, this_filename = os.path.split(__file__) print this_dir objpath = os.path.join(this_dir, "../manipulation/grip/objects", "planerearstay22.stl") handpkg = rtq85nm freegriptst = freegrip.Freegrip(objpath, handpkg, readser=False, torqueresist=50) freegriptst.segShow(base, togglesamples=False, togglenormals=False, togglesamples_ref=False, togglenormals_ref=False, togglesamples_refcls=False, togglenormals_refcls=False) # freegriptst.removeFgrpcc(base) # freegriptst.removeHndcc(base, discretesize=16) # gdb = db.GraspDB() # freegriptst.saveToDB(gdb) #
from panda3d.core import * base = pandactrl.World(camp=[700, 300, 700], lookatp=[0, 0, 100]) this_dir, this_filename = os.path.split(__file__) print(this_dir) # objpath = os.path.join(this_dir, "objects", "housing.stl") # objpath = os.path.join(this_dir, "objects", "box.stl") # objpath = os.path.join(this_dir, "objects", "tool_motordriver.stl") # objpath = os.path.join(this_dir, "objects", "housingshaft.stl") objpath = os.path.join(this_dir, "objects", "bunnysim.stl") handpkg = rtq85nm freegriptst = freegrip.Freegrip(objpath, handpkg, readser=False, faceangle=.8, segangle=.9, hmax=1.0) freegriptst.segShow(base, togglesamples=False, togglenormals=False, togglesamples_ref=False, togglenormals_ref=False, togglesamples_refcls=False, togglenormals_refcls=False) # freegriptst.removeFgrpcc(base) # freegriptst.removeHndcc(base, discretesize = 16) freegriptst.planGrasps(base, discretesize=8)
-newcv[1], -newcv[2], -newav[0], -newav[1], newav[2], jawwidth=17)) # newyihand.setColor(.7,.7,.7,1) newyihand.reparentTo(base.render) # yihnd = hndfa.genHand(usesuction=True) # c0nvec = np.array([0, -1, 0]) # approachvec = np.array([1, 0, 0]) # for z in [60, 90]: # for anglei in [0, 22.5, 45, 67.5, 90, 112.5, 135, 157.5, 180, 202.5, 225, 247.5, 270, 292.5, 315, 337.5]: # newcv = np.dot(rm.rodrigues((0, 0, 1), anglei), c0nvec) # tempav = np.dot(rm.rodrigues((0, 0, 1), anglei), approachvec) # for anglej in [0, 22.5, 45, 67.5, 90, 112.5, 135, 157.5, 180, 202.5, 225, 247.5, 270, 292.5, 315, 337.5]: # newyihand = yihnd.copy() # newav = np.dot(rm.rodrigues(newcv, anglej), tempav) # base.pggen.plotAxis(newyihand.hndnp, length=15, thickness=2) # pregrasps.append( # newyihand.approachAt(0, 0, z, newcv[0], newcv[1], newcv[2], newav[0], newav[1], # newav[2], jaw_width=17)) # # toggle this part for manually defined plans fgplanner = freegrip.Freegrip(objpath, yihnd) gdb = db.GraspDB(database="yumi") print("Saving to database...") fgplanner.saveManuallyDefinedToDB(gdb, pregrasps) base.run()
# 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) # geom = pandageom.packpandageom_fn(freegriptst.objtrimesh.vertices + # np.tile(0 * facetnormal, # [freegriptst.objtrimesh.vertices.shape[0], 1]), # freegriptst.objtrimesh.face_normals[faces], # freegriptst.objtrimesh.faces[faces]) # node = GeomNode('piece')
for objn in objns: objpath = pathname + objname + str(objn) + ".stl" objcm = cm.CollisionModel(objinit=objpath) # objcm.reparentTo(base.render) objstcm = cm.CollisionModel(objinit=objstpath) hndfa = hnd.Robotiq85Factory() difflistall = [] for i in range(10): hand = hndfa.genHand() freegriptst = fg.Freegrip(objpath, hand, faceangle=.9, segangle=.9, refine1min=5, togglebcdcdebug=True, useoverlap=True) tic = time.time() freegriptst.planGrasps() toc = time.time() print("plan grasp cost", toc - tic) ngrasp = 0 for pfacets in freegriptst.griprotmats_planned: for gmats in pfacets: ngrasp += len(gmats) nsamples = 0 for j in range(len(freegriptst.facets)):
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, togglebcdcdebug=True, useoverlap=True) # geom = None facetsizes = [] for i, faces in enumerate(freegriptst.facets): rgba = [ np.random.random(), np.random.random(), np.random.random(), 1 ] tm = trimesh.Trimesh( vertices=freegriptst.objtrm.vertices, faces=freegriptst.objtrm.faces[faces], face_normals=freegriptst.objtrm.face_normals[faces])
from pandaplotutils import pandactrl base = pandactrl.World(camp=[700, 300, 700], lookatp=[0, 0, 100]) this_dir, this_filename = os.path.split(__file__) print this_dir objpath = os.path.join(this_dir, "../manipulation/grip/objects", "newpoisson_body.stl") # objnp = base.pg.loadstlaspandanp_fn(objpath) # objnp.reparentTo(base.render) # base.run() handpkg = rtq85nm freegriptst = freegrip.Freegrip(objpath, handpkg, readser=False, torqueresist=70, dotnormplan=.90, dotnoarmovlp=.95, dotnormpara=-.80) freegriptst.segShow(base, togglesamples=True, togglenormals=True, togglesamples_ref=False, togglenormals_ref=False, togglesamples_refcls=False, togglenormals_refcls=True, alpha=.1) freegriptst.removeFgrpcc(base) freegriptst.removeHndcc(base, discretesize=16)
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] hndnps = [None]
import pandaplotutils.pandactrl as pandactrl import manipulation.grip.yumiintegrated.yumiintegrated as yi import manipulation.grip.freegrip as freegrip import environment.collisionmodel as cm import environment.suitayuminotop as yumisetting if __name__=='__main__': base = pandactrl.World(camp=[2700, -2000, 2000], lookatpos=[0, 0, 500]) _this_dir, _ = os.path.split(__file__) objpath = os.path.join(_this_dir, "objects", "tubelarge.stl") hndfa = yi.YumiIntegratedFactory() hnd = hndfa.genHand() fgplanner = freegrip.Freegrip(objpath, hnd, faceangle = .95, segangle = .95, refine1min=2, refine1max=30, refine2radius=5, fpairparallel=-0.95, hmax=5, objmass = 5.0, bypasssoftfgr = True, togglebcdcdebug = False, useoverlap = True) objcm = cm.CollisionModel(objinit = objpath) objcm.reparentTo(base.render) objcm.setColor(.3,.3,.3,1) # toggle this part for automatic planning print("Planning...") fgplanner.planGrasps() gdb = db.GraspDB(database="yumi") print("Saving to database...") fgplanner.saveToDB(gdb) print("Loading from database...") fgdata = freegrip.FreegripDB(gdb, objcm.name, hnd.name) print("Plotting...") fgdata.plotHands(base.render, hndfa, rgba=(0,1,0,.3))