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