def grpshow(self, base):
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
idfreetabletopplacement = int(result[3][0])
objrotmat = dc.strToMat4(result[3][1])
# show object
geom = pg.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(.77, 0.67, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
sql = "SELECT freetabletopgrip.rotmat, freetabletopgrip.jawwidth FROM freetabletopgrip WHERE \
freetabletopgrip.idfreetabletopplacement=%d" % idfreetabletopplacement
result = self.gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1])
tmprtq85.setMat(hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def updateshow(task):
global countdlw, freegriprotmats
if countdlw < (len(freegriprotmats) - 1):
countdlw = countdlw + 1
freegriprotmat = freegriprotmats[countdlw]
rtqhnd = rtq85nm.Rtq85NM(hndcolor=[1, 1, 1, .2])
rtqhnd.setMat(pandanpmat4=freegriprotmat)
rtqhnd.setJawwidth(freegripjawwidth[countdlw])
rtqhnd.reparentTo(base.render)
return task.again
def loadRtq85Models(self):
"""
load the rtq85 model and its fgrprecc model
:return:
"""
self.rtq85hnd = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
handfgrpccpath = Filename.fromOsSpecific(
os.path.join(this_dir, "robotiq85/rtq85egg",
"robotiq_85_tip_precc.egg"))
self.handfgrpcc_uninstanced = loader.loadModel(handfgrpccpath)
def grpsshow(self, base):
sql = "SELECT dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth FROM dropworkcellgrip WHERE \
dropworkcellgrip.iddropstablepos=%d" % self.dbidstablepos
result = self.gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
#tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1])
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, 0.7])
tmprtq85.setMat(hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def showAllGrips(self):
"""
showAllGrips
:return:
author: weiwei
date: 20170206
"""
for i in range(len(self.gripcontacts)):
# for i in range(2,3):
hndrotmat = self.griprotmats[i]
hndjawwidth = self.gripjawwidth[i]
# show grasps
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[.7, .7, 0.7, .7])
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .5])
tmprtq85.setMat(hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def __init__(self, objpath, handpkg, gdb):
self.objtrimesh = trimesh.load_mesh(objpath)
self.objcom = self.objtrimesh.center_mass
self.objtrimeshconv = self.objtrimesh.convex_hull
# oc means object convex
self.ocfacets, self.ocfacetnormals = self.objtrimeshconv.facets_over(
.9999)
# for dbaccess
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
# use two bulletworld, one for the ray, the other for the tabletop
self.bulletworldray = BulletWorld()
self.bulletworldhp = BulletWorld()
# plane to remove hand
self.planebullnode = cd.genCollisionPlane(offset=0)
self.bulletworldhp.attachRigidBody(self.planebullnode)
self.handpkg = handpkg
self.handname = handpkg.getHandName()
print("HandName: " + str(self.handname))
# self.hand = handpkg.newHandNM(hndcolor=[0,1,0,.1])
self.rtq85hnd = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# for dbsave
# each tpsmat4 corresponds to a set of tpsgripcontacts/tpsgripnormals/tpsgripjawwidth list
self.tpsmat4s = None
self.tpsgripcontacts = None
self.tpsgripnormals = None
self.tpsgripjawwidth = None
# for ocFacetShow
self.counter = 0
self.gdb = gdb
self.loadFreeAirGrip()
def removeHndccShow(self, base, discretesize=8):
"""
Handcc means hand collision detection
This one is developed for demonstration
This function should be called after executing removeHndcc
:param discretesize: the number of hand orientations
:return: delayTime
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
if self.rtq85plotlist:
for rtq85plotnode in self.rtq85plotlist:
rtq85plotnode.removeNode()
self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
if self.counter2 == 0:
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
self.counter2 += 1
if self.counter2 >= discretesize:
self.counter2 = 0
print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1)
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(
self.gripcontactpairs_precc[self.counter]):
if j == 0:
print j, contactpair
# for angleid in range(discretesize):
angleid = self.counter2
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[
self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
# TODO setting jawwidth to 80 is enough
# since fgrpcc already detects inner collisions
tmprtq85.setJawwidth(fgrdist)
tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat())
axx = tmprtq85.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array(
[axx[0], axx[1], axx[2]])
tmprtq85.setPos(
Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
self.hndbullnode = cd.genCollisionMeshMultiNp(
tmprtq85.rtq85np, base.render)
result = self.bulletworld.contactTest(self.hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmprtq85.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(
self.gripcontactpairnormals_precc[self.counter][j])
# pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
tmprtq85.setColor([1, 1, 1, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
else:
for contact in result.getContacts():
# cp = contact.getManifoldPoint()
# pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1))
# pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1))
tmprtq85.setColor([.5, 0, 0, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
# debugNode.showBoundingBoxes(False)
# debugNode.showNormals(False)
# bullcldrnp = base.render.attachNewNode("bulletcollider")
# debugNP = bullcldrnp.attachNewNode(debugNode)
# debugNP.show()
# freegriptst.bulletworld.setDebugNode(debugNP.node())
# taskMgr.add(updateworld, "updateworld", extraArgs=[freegriptst.bulletworld], appendTask=True)
# freegriptst.showAllGrips()
data = gdb.loadFreeAirGrip('bunnysim', 'rtq85')
if data:
freegripid, freegripcontacts, freegripnormals, freegriprotmats, freegripjawwidth = data
print(len(freegripid))
for i, freegriprotmat in enumerate(freegriprotmats):
rtqhnd = rtq85nm.Rtq85NM(hndcolor=[1, 1, 1, .3])
newpos = freegriprotmat.getRow3(
3) - freegriprotmat.getRow3(2) * 0.0
freegriprotmat.setRow(3, newpos)
rtqhnd.setMat(pandanpmat4=freegriprotmat)
rtqhnd.setJawwidth(freegripjawwidth[i])
pandamat4 = rtqhnd.handnp.getMat()
# x = pandamat4.getRow3(0)
# if (Vec3(1,0,0).angleDeg(x)-45) < 15:
rtqhnd.reparentTo(base.render)
# base.pggen.plotAxis(base.render, spos=rtqhnd.handnp.getPos(),pandamat4=rtqhnd.handnp.getMat())
# base.pggen.plotSphere(base.render, freegripcontacts[i][0], radius = 10, rgba = [1.0,0,0,1])
# base.pggen.plotArrow(base.render, spos = freegripcontacts[i][0], epos = freegripcontacts[i][0]+freegripnormals[i][0], thickness = 5, rgba = [1.0,0,0,1])
# base.pggen.plotSphere(base.render, freegripcontacts[i][1], radius = 10, rgba = [0,1,0,1])
# base.run()
base.run()
# armjntsgoal = hrp5robot.numikr(objpos, objrot, armid)
# if armjntsgoal is not None:
# hrp5robot.movearmfkr(armjntsgoal, armid)
# hrp5plot.plotstick(base.render, hrp5robot)
# hrp5mnp = hrp5plot.genHrp5mnp(hrp5robot)
# hrp5mnp.reparentTo(base.render)
armjntsgoal = hrp5robot.numik(objpos, objrot, armid)
if armjntsgoal is not None:
hrp5robot.movearmfk6(armjntsgoal, armid)
hrp5plot.plotstick(base.render, hrp5robot)
hrp5mnp = hrp5plot.genHrp5mnp(hrp5robot)
# hrp5mnp.reparentTo(base.render)
# goal hand
from manipulation.grip.robotiq85 import rtq85nm
hrp5robotrgthnd = rtq85nm.Rtq85NM()
hrp5robotrgthnd.setColor([1, 0, 0, .3])
hrp5robotrgtarmlj9_rotmat = pandageom.cvtMat4(objrot,
objpos + objrot[:, 0] * 130)
pg.plotAxisSelf(base.render, objpos, hrp5robotrgtarmlj9_rotmat)
hrp5robotrgthnd.setMat(hrp5robotrgtarmlj9_rotmat)
hrp5robotrgthnd.reparentTo(base.render)
#
# angle = nxtik.eurgtbik(objpos)
# nxtrobot.movewaist(angle)
# armjntsgoal=nxtik.numik(nxtrobot, objpos, objrot)
#
# # nxtplot.plotstick(base.render, nxtrobot)
# pandamat4=Mat4()
# pandamat4.setRow(3,Vec3(0,0,250))
# # pg.plotAxis(base.render, pandamat4)
