# 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)
# geom = pandageom.packpandageom_fn(freegriptst.objtrimesh.vertices +
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()