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 addworkstl(self):
workx = 0
worky = 0
workz = 0
geom1 = pg.packpandageom(self.worktrimesh.vertices,
self.worktrimesh.face_normals,
self.worktrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom1)
self.workcell = NodePath('obj')
self.workcell.attachNewNode(node)
#workcell.setColor(Vec4(.7, 0.3, 0, 1))
#workcell.setTransparency(TransparencyAttrib.MAlpha)
self.workcell.setPos(-workx, worky, workz)
self.workcell.reparentTo(base.render)
self.workcellbody = OdeBody(self.odeworld)
mass = OdeMass()
#mass.setSphereTotal(50000, 1)
mass.setSphereTotal(5000, 1)
self.workcellbody.setMass(mass)
self.workcellbody.setPosition(self.workcell.getPos())
self.workcellbody.setQuaternion(self.workcell.getQuat())
modelTrimesh2 = OdeTriMeshData(self.workcell, True)
modelGeom2 = OdeTriMeshGeom(self.odespace, modelTrimesh2)
modelGeom2.setBody(self.workcellbody)
joint = OdeFixedJoint(self.odeworld)
joint.attachBody(self.workcellbody, 0)
self.workcell.setPythonTag("body", self.workcellbody)
self.partlist.append(self.workcell)
def showOnePlacementAndAssociatedGrips(self, base):
"""
show one placement and its associated grasps
:param base:
:return:
"""
for i in range(len(self.tpsmat4s)):
if i == 0:
objrotmat = self.tpsmat4s[i]
# objrotmat.setRow(0, -objrotmat.getRow3(0))
rotzmat = Mat4.rotateMat(0, Vec3(0,0,1))
objrotmat = objrotmat*rotzmat
# 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(.7,0.3,0,1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
for j in range(len(self.tpsgriprotmats[i])):
# for j in range(13,14):
hndrotmat = self.tpsgriprotmats[i][j]
hndjawwidth = self.tpsgripjawwidth[i][j]
# show grasps
tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .5])
tmphnd.setMat(pandanpmat4 = hndrotmat)
tmphnd.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmphnd.reparentTo(base.render)
def ocfacetshow(self, base):
print self.objcom
npf = base.render.find("**/supportfacet")
if npf:
npf.removeNode()
plotoffsetfp = 10
print self.counter
print len(self.ocfacets)
if self.counter < len(self.ocfacets):
geom = pandageom.packpandageom(self.objtrimeshconv.vertices+
np.tile(plotoffsetfp*self.ocfacetnormals[self.counter],
[self.objtrimeshconv.vertices.shape[0],1]),
self.objtrimeshconv.face_normals[self.ocfacets[self.counter]],
self.objtrimeshconv.faces[self.ocfacets[self.counter]])
# geom = pandageom.packpandageom(self.objtrimeshconv.vertices,
# self.objtrimeshconv.face_normals,
# self.objtrimeshconv.faces)
node = GeomNode('supportfacet')
node.addGeom(geom)
star = NodePath('supportfacet')
star.attachNewNode(node)
star.setColor(Vec4(1,0,1,1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
self.counter+=1
else:
self.counter = 0
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(pandanpmat4 = hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def showOnePlacementAndAssociatedGrips(self, base):
"""
show one placement and its associated grasps
:param base:
:return:
"""
for i in range(len(self.tpsmat4s)):
if i == 2:
objrotmat = self.tpsmat4s[i]
# objrotmat.setRow(0, -objrotmat.getRow3(0))
rotzmat = Mat4.rotateMat(180, Vec3(0, 0, 1))
objrotmat = objrotmat * rotzmat
# 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(.7, 0.3, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
for j in range(len(self.tpsgriprotmats[i])):
# for j in range(13,14):
hndrotmat = self.tpsgriprotmats[i][j]
hndjawwidth = self.tpsgripjawwidth[i][j]
# show grasps
tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .5])
tmphnd.setMat(hndrotmat)
tmphnd.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmphnd.reparentTo(base.render)
def ocfacetshow(self, base):
print self.objcom
npf = base.render.find("**/supportfacet")
if npf:
npf.removeNode()
plotoffsetfp = 10
print self.counter
print len(self.ocfacets)
if self.counter < len(self.ocfacets):
geom = pandageom.packpandageom(
self.objtrimeshconv.vertices +
np.tile(plotoffsetfp * self.ocfacetnormals[self.counter],
[self.objtrimeshconv.vertices.shape[0], 1]),
self.objtrimeshconv.face_normals[self.ocfacets[self.counter]],
self.objtrimeshconv.faces[self.ocfacets[self.counter]])
# geom = pandageom.packpandageom(self.objtrimeshconv.vertices,
# self.objtrimeshconv.face_normals,
# self.objtrimeshconv.faces)
node = GeomNode('supportfacet')
node.addGeom(geom)
star = NodePath('supportfacet')
star.attachNewNode(node)
star.setColor(Vec4(1, 0, 1, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
self.counter += 1
else:
self.counter = 0
def loadObjModel(self, ompath):
self.objtrimesh = trimesh.load_mesh(ompath)
# oversegmentation
self.facets, self.facetnormals = self.objtrimesh.facets_over(
faceangle=.95)
self.facetcolorarray = pandageom.randomColorArray(self.facets.shape[0])
self.sampleObjModel()
# prepare the model for collision detection
self.objgeom = pandageom.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom)
self.bulletworld.attachRigidBody(self.objmeshbullnode)
# object center
self.objcenter = [0, 0, 0]
for pnt in self.objtrimesh.vertices:
self.objcenter[0] += pnt[0]
self.objcenter[1] += pnt[1]
self.objcenter[2] += pnt[2]
self.objcenter[
0] = self.objcenter[0] / self.objtrimesh.vertices.shape[0]
self.objcenter[
1] = self.objcenter[1] / self.objtrimesh.vertices.shape[0]
self.objcenter[
2] = self.objcenter[2] / self.objtrimesh.vertices.shape[0]
def __init__(self, objpath,objpathWorkcell, handpkg, dbidstablepos,readser):
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
self.objtrimesh=None
self.dbidstablepos = dbidstablepos
[objrotmat, objposmat]=self.loadDropStablePos()
self.loadObjModel(objpath,objrotmat,objposmat)
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
# 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=-55)
self.bulletworldhp.attachRigidBody(self.planebullnode)
#workcell to remove hand
self.workcellmesh = trimesh.load_mesh(objpathWorkcell)
self.objgeom = pandageom.packpandageom(self.workcellmesh.vertices, self.workcellmesh.face_normals,
self.workcellmesh.faces)
self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom)
self.bulletworldhp.attachRigidBody(self.objmeshbullnode)
node = GeomNode('obj')
node.addGeom(self.objgeom)
self.worcell = NodePath('obj')
self.worcell.attachNewNode(node)
self.worcell.reparentTo(base.render)
self.handpkg = handpkg
self.handname = handpkg.getHandName()
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
#get dropfreegrip
self.loadDropfreeGrip()
def __init__(self, objpath, handpkg, readser=False, torqueresist=500):
"""
initialization
:param objpath: path of the object
:param ser: True use pre-computed template file for debug (in order to debug large models like tool.stl
:param torqueresist: the maximum allowable distance to com (see FreegripContactpairs.planContactpairs)
author: weiwei
date: 20161201, osaka
"""
super(self.__class__, self).__init__(objpath, readser)
if readser is False:
self.removeBadSamples()
self.clusterFacetSamplesRNN(reduceRadius=3)
#self.planContactpairs(torqueresist,200)
self.planContactpairs(200)
self.saveSerialized("tmpcp.pickle")
else:
self.loadSerialized("tmpcp.pickle", objpath)
self.handpkg = handpkg
self.hand = handpkg.newHandNM(hndcolor=[0, 1, 0, .1])
self.handfgrpcc_uninstanced = handpkg.newHandFgrpcc()
self.handname = handpkg.getHandName()
# gripcontactpairs_precc is the gripcontactpairs ([[p0,p1,p2],[p0',p1',p2']] pairs) after precc (collision free)
# gripcontactpairnormals_precc is the gripcontactpairnormals ([[n0,n1,n2],[n0',n1',n2']] pairs) after precc
# likewise, gripcontactpairfacets_precc is the [faceid0, faceid1] pair corresponding to the upper two
self.gripcontactpairs_precc = None
self.gripcontactpairnormals_precc = None
self.gripcontactpairfacets_precc = None
# the final results: gripcontacts: a list of [cct0, cct1]
# griprotmats: a list of Mat4
# gripcontactnormals: a list of [nrml0, nrml1]
self.gripcontacts = None
self.griprotmats = None
self.gripjawwidth = None
self.gripcontactnormals = None
self.bulletworld = BulletWorld()
# prepare the model for collision detection
self.objgeom = pandageom.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom)
self.bulletworld.attachRigidBody(self.objmeshbullnode)
# for plot
self.rtq85plotlist = []
self.counter2 = 0
# for dbupdate
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
def __init__(self, objpath, handpkg, readser=False, torqueresist = 50):
"""
initialization
:param objpath: path of the object
:param ser: True use pre-computed template file for debug (in order to debug large models like tool.stl
:param torqueresist: the maximum allowable distance to com (see FreegripContactpairs.planContactpairs)
author: weiwei
date: 20161201, osaka
"""
super(self.__class__, self).__init__(objpath, readser)
if readser is False:
self.removeBadSamples()
self.clusterFacetSamplesRNN(reduceRadius=10)
self.planContactpairs(torqueresist)
self.saveSerialized("tmpcp.pickle")
else:
self.loadSerialized("tmpcp.pickle", objpath)
self.handpkg = handpkg
self.hand = handpkg.newHandNM(hndcolor=[0,1,0,.1])
self.handfgrpcc_uninstanced = handpkg.newHandFgrpcc()
self.handname = handpkg.getHandName()
# gripcontactpairs_precc is the gripcontactpairs ([[p0,p1,p2],[p0',p1',p2']] pairs) after precc (collision free)
# gripcontactpairnormals_precc is the gripcontactpairnormals ([[n0,n1,n2],[n0',n1',n2']] pairs) after precc
# likewise, gripcontactpairfacets_precc is the [faceid0, faceid1] pair corresponding to the upper two
self.gripcontactpairs_precc = None
self.gripcontactpairnormals_precc = None
self.gripcontactpairfacets_precc = None
# the final results: gripcontacts: a list of [cct0, cct1]
# griprotmats: a list of Mat4
# gripcontactnormals: a list of [nrml0, nrml1]
self.gripcontacts = None
self.griprotmats = None
self.gripjawwidth = None
self.gripcontactnormals = None
self.bulletworld = BulletWorld()
# prepare the model for collision detection
self.objgeom = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom)
self.bulletworld.attachRigidBody(self.objmeshbullnode)
# for plot
self.rtq85plotlist = []
self.counter2 = 0
# for dbupdate
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
def showOnePlacementAndAssociatedGrips(self, base):
"""
show one placement and its associated grasps
:param base:
:return:
"""
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(.7, 0.3, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
#star.setMat(objrotmat)
star.reparentTo(base.render)
def removebadfacets(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
self.tpsmat4s = []
for i in range(len(self.ocfacets)):
geom = pg.packpandageom(
self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom + self.ocfacetnormals[i] * 99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
facetinterpnt = np.array([hitpos[0], hitpos[1], hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(
self.objtrimeshconv, self.ocfacets[i], facetinterpnt,
facetnormal)
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4,
facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(
np.array([hitpos[0], hitpos[1], hitpos[2]]) -
np.array([pFrom[0], pFrom[1], pFrom[2]]))
if dist2p / dist2c >= doverh:
# hit and stable
self.tpsmat4s.append(pg.cvtMat4np4(facetmat4))
def loadObjModel(self, ompath):
self.objtrimesh=trimesh.load_mesh(ompath)
# oversegmentation
self.facets, self.facetnormals = self.objtrimesh.facets_over(faceangle=.95)
self.facetcolorarray = pandageom.randomColorArray(self.facets.shape[0])
self.sampleObjModel()
# prepare the model for collision detection
self.objgeom = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom)
self.bulletworld.attachRigidBody(self.objmeshbullnode)
# object center
self.objcenter = [0,0,0]
for pnt in self.objtrimesh.vertices:
self.objcenter[0]+=pnt[0]
self.objcenter[1]+=pnt[1]
self.objcenter[2]+=pnt[2]
self.objcenter[0] = self.objcenter[0]/self.objtrimesh.vertices.shape[0]
self.objcenter[1] = self.objcenter[1]/self.objtrimesh.vertices.shape[0]
self.objcenter[2] = self.objcenter[2]/self.objtrimesh.vertices.shape[0]
def grpshow(self, base, gdb):
sql = "SELECT tabletopplacements.idtabletopplacements, tabletopplacements.rotmat \
FROM tabletopplacements,freetabletopplacement,object,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s' AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopplacement.idfreetabletopplacement = %d AND angle.value = %d" % (
self.dbobjname, 1, 45)
# WHY 1 and why 45.0?
result = gdb.execute(sql)
print("SHOW RESULT LEN = " + str(len(result)))
if len(result) != 0:
for resultrow in result:
idtabletopplacements = int(resultrow[0])
objrotmat = dc.strToMat4(resultrow[1])
objpos = objrotmat.getRow3(3)
base.changeLookAt(lookatp=[objpos[0], objpos[1], objpos[2]])
# 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, .67, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
sql = "SELECT tabletopgrips.rotmat, tabletopgrips.jawwidth FROM tabletopgrips WHERE \
tabletopgrips.idtabletopplacements=%d" % idtabletopplacements
result = gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .1])
tmphnd.setMat(pandanpmat4=hndrotmat)
tmphnd.setJawwidth(hndjawwidth)
tmphnd.reparentTo(base.render)
def removebadfacets(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
self.tpsmat4s = []
for i in range(len(self.ocfacets)):
geom = pg.packpandageom(self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom+self.ocfacetnormals[i]*99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
facetinterpnt = np.array([hitpos[0],hitpos[1],hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(self.objtrimeshconv, self.ocfacets[i],
facetinterpnt, facetnormal)
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(np.array([hitpos[0],hitpos[1],hitpos[2]])-np.array([pFrom[0],pFrom[1],pFrom[2]]))
if dist2p/dist2c >= doverh:
# hit and stable
self.tpsmat4s.append(pg.cvtMat4np4(facetmat4))
def grpshow(self, base, gdb):
sql = "SELECT tabletopplacements.idtabletopplacements, tabletopplacements.rotmat \
FROM tabletopplacements,freetabletopplacement,object,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s' AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopplacement.idfreetabletopplacement = %d AND angle.value = %d" % (self.dbobjname, 1, 45)
result = gdb.execute(sql)
if len(result) != 0:
for resultrow in result:
idtabletopplacements = int(resultrow[0])
objrotmat = dc.strToMat4(resultrow[1])
objpos = objrotmat.getRow3(3)
base.changeLookAt(lookatp=[objpos[0],objpos[1],objpos[2]])
# 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,.67,0,1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
sql = "SELECT tabletopgrips.rotmat, tabletopgrips.jawwidth FROM tabletopgrips WHERE \
tabletopgrips.idtabletopplacements=%d" % idtabletopplacements
result = gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .1])
tmphnd.setMat(pandanpmat4 = hndrotmat)
tmphnd.setJawwidth(hndjawwidth)
tmphnd.reparentTo(base.render)
def preObj(self):
"""
:return: prepare add obj into odespace, set node paht ,set geom, set body mass, do only once in a loop
author:jiayao
time:20170811
"""
geom = pg.packpandageom(self.smiley.vertices, self.smiley.face_normals,
self.smiley.faces)
node = GeomNode('obj')
node.addGeom(geom)
self.objnodepath = NodePath('obj')
self.objnodepath.attachNewNode(node)
self.objnodepath.setColor(Vec4(.7, 0.3, 0, 1))
self.body = OdeBody(self.odeworld)
mass = OdeMass()
mass.setSphereTotal(5, 1)
self.body.setMass(mass)
self.modelTrimesh = OdeTriMeshData(self.objnodepath, True)
self.modelGeom = OdeTriMeshGeom(self.odespace, self.modelTrimesh)
def __init__(self, ompath, density = 4.0):
"""
generate point cloud template for cad model
:param ompath:
:param numpointsoververts: the density of sampling, the total count is numpointsoververts*nverts
it could be 'auto' if decide automatically
:return an array of points
author: weiwei
date: 20170713
"""
self.objtrimesh = trimesh.load_mesh(ompath)
# decide the number of samples considering surface area
area = self.objtrimesh.area_faces
area_sum = np.sum(area)
# 1 point every 4by4
nsample = area_sum/(density*density)
samples, faceid = trimesh.sample.sample_surface_even_withfaceid(
mesh = self.objtrimesh, count = nsample)
self.__samplestemp = np.append(samples, self.objtrimesh.vertices, axis = 0)
self.__samplestempnoverts = samples
self.__samplestempnovertsnormals = self.objtrimesh.face_normals[faceid]
# for computing the inner samples
self.bulletworldray = BulletWorld()
self.__objgeom = pg.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
self.__objmeshbullnode = cd.genCollisionMeshGeom(self.__objgeom)
self.bulletworldray.attachRigidBody(self.__objmeshbullnode)
# remove inner
# use the inner samples and normals for ppf match
self.__newsamplesnoverts = []
self.__newsamplesnovertsnormals = []
self.__removeinnersamples()
def planContactpairs(self, torqueresist = 50, fgrtipdist = 82):
"""
find the grasps using parallel pairs
:param: torqueresist the maximum allowable distance to com
:param: fgrtipdist the maximum dist between finger tips
:return:
author: weiwei
date: 20161130, harada office @ osaka university
"""
# note that pairnormals and pairfacets are duplicated for each contactpair
# the duplication is performed on purpose for convenient access
# also, each contactpair"s" corresponds to a facetpair
# it is empty when no contactpair is available
self.gripcontactpairs = []
# gripcontactpairnormals and gripcontactpairfacets are not helpful
# they are kept for convenience (they could be accessed using facetnormals and facetpairs)
self.gripcontactpairnormals = []
self.gripcontactpairfacets = []
# for precollision detection
# bulletworldprecc = BulletWorld()
# # build the collision shape of objtrimesh
# geomobj = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals,
# self.objtrimesh.faces)
# objmesh = BulletTriangleMesh()
# objmesh.addGeom(geomobj)
# objmeshnode = BulletRigidBodyNode('objmesh')
# objmeshnode.addShape(BulletTriangleMeshShape(objmesh, dynamic=True))
# bulletworldprecc.attachRigidBody(objmeshnode)
# for raytracing
bulletworldray = BulletWorld()
nfacets = self.facets.shape[0]
self.facetpairs = list(itertools.combinations(range(nfacets), 2))
for facetpair in self.facetpairs:
# print "facetpair"
# print facetpair, len(self.facetpairs)
self.gripcontactpairs.append([])
self.gripcontactpairnormals.append([])
self.gripcontactpairfacets.append([])
# if one of the facet doesnt have samples, jump to next
if self.objsamplepnts_refcls[facetpair[0]].shape[0] is 0 or \
self.objsamplepnts_refcls[facetpair[1]].shape[0] is 0:
# print "no sampled points"
continue
# check if the faces are opposite and parallel
dotnorm = np.dot(self.facetnormals[facetpair[0]], self.facetnormals[facetpair[1]])
if dotnorm < -0.95:
# check if any samplepnts's projection from facetpairs[i][0] falls in the polygon of facetpairs[i][1]
facet0pnts = self.objsamplepnts_refcls[facetpair[0]]
facet0normal = self.facetnormals[facetpair[0]]
facet1normal = self.facetnormals[facetpair[1]]
# generate collision mesh
facetmesh = BulletTriangleMesh()
faceidsonfacet = self.facets[facetpair[1]]
geom = pandageom.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals[faceidsonfacet],
self.objtrimesh.faces[faceidsonfacet])
facetmesh.addGeom(geom)
facetmeshbullnode = BulletRigidBodyNode('facet')
facetmeshbullnode.addShape(BulletTriangleMeshShape(facetmesh, dynamic=True))
bulletworldray.attachRigidBody(facetmeshbullnode)
# check the projection of a ray
for facet0pnt in facet0pnts:
pFrom = Point3(facet0pnt[0], facet0pnt[1], facet0pnt[2])
pTo = pFrom + Vec3(facet1normal[0], facet1normal[1], facet1normal[2])*9999
result = bulletworldray.rayTestClosest(pFrom, pTo)
if result.hasHit():
hitpos = result.getHitPos()
if np.linalg.norm(np.array(facet0pnt.tolist())-np.array([hitpos[0], hitpos[1], hitpos[2]])) < fgrtipdist:
fgrcenter = (np.array(facet0pnt.tolist())+np.array([hitpos[0], hitpos[1], hitpos[2]]))/2.0
# avoid large torque
if np.linalg.norm(self.objtrimesh.center_mass - fgrcenter) < torqueresist:
self.gripcontactpairs[-1].append([facet0pnt.tolist(), [hitpos[0], hitpos[1], hitpos[2]]])
self.gripcontactpairnormals[-1].append([[facet0normal[0], facet0normal[1], facet0normal[2]],
[facet1normal[0], facet1normal[1], facet1normal[2]]])
self.gripcontactpairfacets[-1].append(facetpair)
# pre collision checking: it takes one finger as a cylinder and
# computes its collision with the object
## first finger
# cylindernode0 = BulletRigidBodyNode('cylindernode0')
# cylinder0height = 50
# cylinder0normal = Vec3(facet0normal[0], facet0normal[1], facet0normal[2])
# cylindernode0.addShape(BulletCylinderShape(radius=self.preccradius,
# height=cylinder0height,
# up=cylinder0normal),
# TransformState.makePos(pFrom+cylinder0normal*cylinder0height*.6))
# bulletworldprecc.attachRigidBody(cylindernode0)
# ## second finger
# cylindernode1 = BulletRigidBodyNode('cylindernode1')
# cylinder1height = cylinder1height
# # use the inverse of facet0normal instead of facet1normal to follow hand orientation
# cylinder1normal = Vec3(-facet0normal[0], -facet0normal[1], -facet0normal[2])
# cylindernode1.addShape(BulletCylinderShape(radius=self.preccradius,
# height=cylinder1height,
# up=cylinder1normal),
# TransformState.makePos(pFrom+cylinder1normal*cylinder1height*.6))
# bulletworldprecc.attachRigidBody(cylindernode1)
# if bulletworldprecc.contactTestPair(cylindernode0, objmeshnode) and \
# bulletworldprecc.contactTestPair(cylindernode1, objmeshnode):
bulletworldray.removeRigidBody(facetmeshbullnode)
# update the pairs
availablepairids = np.where(self.gripcontactpairs)[0]
self.facetpairs = np.array(self.facetpairs)[availablepairids]
self.gripcontactpairs = np.array(self.gripcontactpairs)[availablepairids]
self.gripcontactpairnormals = np.array(self.gripcontactpairnormals)[availablepairids]
self.gripcontactpairfacets = np.array(self.gripcontactpairfacets)[availablepairids]
def segShow2(self, base, togglesamples=False, togglenormals=False,
togglesamples_ref=False, togglenormals_ref=False,
togglesamples_refcls=False, togglenormals_refcls=False, specificface = True):
"""
:param base:
:param togglesamples:
:param togglenormals:
:param togglesamples_ref: toggles the sampled points that fulfills the dist requirements
:param togglenormals_ref:
:return:
"""
nfacets = self.facets.shape[0]
facetcolorarray = self.facetcolorarray
rgbapnts0 = [1, 1, 1, 1]
rgbapnts1 = [0.2, 0.7, 1, 1]
rgbapnts2 = [1, 0.7, 0.2, 1]
# offsetf = facet
plotoffsetf = .0
faceplotted = False
# plot the segments
for i, facet in enumerate(self.facets):
if not specificface:
geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(Vec4(.77, .17, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j],
rgba=rgbapnts0, length=10)
if togglesamples_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.9, rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1, length=10)
if togglesamples_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2, length=10)
if specificface:
plotoffsetf = .1
if faceplotted:
continue
else:
if len(self.objsamplepnts[i])>25:
faceplotted = True
geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[facet], self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(Vec4(facetcolorarray[i][0], facetcolorarray[i][1], facetcolorarray[i][2], 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=2.8, rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls[i][j],
rgba=rgbapnts0, length=10)
if togglesamples_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1, length=10)
if togglesamples_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3.5, rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotArrow(star, spos=apnt+plotoffsetf*i*self.facetnormals[i],
epos=apnt + plotoffsetf*i*self.facetnormals[i] + self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2, length=10)
def removebadfacetsshow(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
plotoffsetfp = 10
# print self.counter
if self.counter < len(self.ocfacets):
i = self.counter
# for i in range(len(self.ocfacets)):
geom = pg.packpandageom(self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom+self.ocfacetnormals[i]*99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
pg.plotArrow(base.render, spos=self.objcom,
epos = self.objcom+self.ocfacetnormals[i], length=100)
facetinterpnt = np.array([hitpos[0],hitpos[1],hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(self.objtrimeshconv, self.ocfacets[i],
facetinterpnt, facetnormal)
for j in range(len(bdverts3d)-1):
spos = bdverts3d[j]
epos = bdverts3d[j+1]
pg.plotStick(base.render, spos, epos, thickness = 1, rgba=[.5,.5,.5,1])
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(np.array([hitpos[0],hitpos[1],hitpos[2]])-np.array([pFrom[0],pFrom[1],pFrom[2]]))
if dist2p/dist2c < doverh:
print "not stable"
# return
else:
print dist2p/dist2c
pol_ext = LinearRing(bdverts2d)
d = pol_ext.project(apntpnt)
p = pol_ext.interpolate(d)
closest_point_coords = list(p.coords)[0]
closep = np.array([closest_point_coords[0], closest_point_coords[1], 0])
closep3d = rm.transformmat4(rm.homoinverse(facetmat4), closep)[:3]
pg.plotDumbbell(base.render, spos=facetinterpnt, epos=closep3d, thickness=1.5, rgba=[0,0,1,1])
for j in range(len(bdverts3d)-1):
spos = bdverts3d[j]
epos = bdverts3d[j+1]
pg.plotStick(base.render, spos, epos, thickness = 1.5, rgba=[0,1,0,1])
# geomoff = pg.packpandageom(self.objtrimeshconv.vertices +
# np.tile(plotoffsetfp * self.ocfacetnormals[i],
# [self.objtrimeshconv.vertices.shape[0], 1]),
# self.objtrimeshconv.face_normals[self.ocfacets[i]],
# self.objtrimeshconv.faces[self.ocfacets[i]])
#
# nodeoff = GeomNode('supportfacet')
# nodeoff.addGeom(geomoff)
# staroff = NodePath('supportfacet')
# staroff.attachNewNode(nodeoff)
# staroff.setColor(Vec4(1,0,1,1))
# staroff.setTransparency(TransparencyAttrib.MAlpha)
# staroff.setTwoSided(True)
# staroff.reparentTo(base.render)
self.counter+=1
else:
self.counter=0
def removebadfacetsshow(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
plotoffsetfp = 10
# print self.counter
if self.counter < len(self.ocfacets):
i = self.counter
# for i in range(len(self.ocfacets)):
geom = pg.packpandageom(
self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom + self.ocfacetnormals[i] * 99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
pg.plotArrow(base.render,
spos=self.objcom,
epos=self.objcom + self.ocfacetnormals[i],
length=100)
facetinterpnt = np.array([hitpos[0], hitpos[1], hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(
self.objtrimeshconv, self.ocfacets[i], facetinterpnt,
facetnormal)
for j in range(len(bdverts3d) - 1):
spos = bdverts3d[j]
epos = bdverts3d[j + 1]
pg.plotStick(base.render,
spos,
epos,
thickness=1,
rgba=[.5, .5, .5, 1])
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4,
facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(
np.array([hitpos[0], hitpos[1], hitpos[2]]) -
np.array([pFrom[0], pFrom[1], pFrom[2]]))
if dist2p / dist2c < doverh:
print "not stable"
# return
else:
pol_ext = LinearRing(bdverts2d)
d = pol_ext.project(apntpnt)
p = pol_ext.interpolate(d)
closest_point_coords = list(p.coords)[0]
closep = np.array(
[closest_point_coords[0], closest_point_coords[1], 0])
closep3d = rm.transformmat4(rm.homoinverse(facetmat4),
closep)[:3]
pg.plotDumbbell(base.render,
spos=facetinterpnt,
epos=closep3d,
thickness=1.5,
rgba=[0, 0, 1, 1])
for j in range(len(bdverts3d) - 1):
spos = bdverts3d[j]
epos = bdverts3d[j + 1]
pg.plotStick(base.render,
spos,
epos,
thickness=1.5,
rgba=[0, 1, 0, 1])
# geomoff = pg.packpandageom(self.objtrimeshconv.vertices +
# np.tile(plotoffsetfp * self.ocfacetnormals[i],
# [self.objtrimeshconv.vertices.shape[0], 1]),
# self.objtrimeshconv.face_normals[self.ocfacets[i]],
# self.objtrimeshconv.faces[self.ocfacets[i]])
#
# nodeoff = GeomNode('supportfacet')
# nodeoff.addGeom(geomoff)
# staroff = NodePath('supportfacet')
# staroff.attachNewNode(nodeoff)
# staroff.setColor(Vec4(1,0,1,1))
# staroff.setTransparency(TransparencyAttrib.MAlpha)
# staroff.setTwoSided(True)
# staroff.reparentTo(base.render)
self.counter += 1
else:
self.counter = 0
togglenormals_ref=False,
togglesamples_refcls=False,
togglenormals_refcls=False)
freegriptst.plotObj()
freegriptst.removeFgrpcc(base)
freegriptst.removeHndcc(base)
gdb = db.GraspDB()
freegriptst.saveToDB(gdb)
freegriptst.showAllGrips()
# work cell
this_dir = "E:/project/manipulation/binpicking"
objpathWorkcell = os.path.join(this_dir, "objects", "workcell22.stl")
workcellmesh = trimesh.load_mesh(objpathWorkcell)
objgeom = pandageom.packpandageom(workcellmesh.vertices,
workcellmesh.face_normals,
workcellmesh.faces)
workx = 0
worky = 0
workz = 0
# the first obejct sm1;
node = GeomNode('obj')
node.addGeom(objgeom)
worcell = NodePath('obj')
worcell.attachNewNode(node)
worcell.setPos(-workx, worky, workz)
worcell.reparentTo(base.render)
base.run()
def removeFgrpccShow(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
:return:
author: weiwei
date: 20161201, osaka
"""
# 6 is used because I am supposing 4+2 where 4 is the default
# margin of bullet in panda3d. (NOTE: This is a guess)
plotoffsetfp = 6
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
#
# np0collection = base.render.findAllMatches("**/rtq85fgrpcc0")
# for np0 in np0collection:
# np0.removeNode()
# np1collection = base.render.findAllMatches("**/rtq85fgrpcc1")
# for np1 in np1collection:
# np1.removeNode()
#
# npscollection = base.render.findAllMatches("**/sphere")
# for nps in npscollection:
# nps.removeNode()
npbrchild = base.render.find("**/tempplot")
if npbrchild:
npbrchild.removeNode()
# for fast delete
brchild = NodePath('tempplot')
brchild.reparentTo(base.render)
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
facetcolorarray = self.facetcolorarray
star0.setColor(
Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(brchild)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
star1.setColor(
Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
star1.setTwoSided(True)
star1.reparentTo(brchild)
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
# the following two choices decide the way to detect contacts
cctnormal00 = np.array(
self.gripcontactpairnormals[self.counter][j][0])
cctnormal01 = -np.array(
self.gripcontactpairnormals[self.counter][j][1])
cctnormal0raw = (cctnormal00 + cctnormal01)
cctnormal0 = (cctnormal0raw /
np.linalg.norm(cctnormal0raw)).tolist()
# the following two choices decide the way to detect contacts
cctnormal10 = -cctnormal00
cctnormal11 = -cctnormal01
cctnormal1raw = (cctnormal10 + cctnormal11)
cctnormal1 = (cctnormal1raw /
np.linalg.norm(cctnormal1raw)).tolist()
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc0)
handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2])
handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0],
cctpnt0[1] + cctnormal0[1],
cctpnt0[2] + cctnormal0[2])
handfgrpcc1 = NodePath("handfgrpcc1")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc1)
handfgrpcc1.setPos(cctpnt1[0], cctpnt1[1], cctpnt1[2])
handfgrpcc1.lookAt(cctpnt1[0] + cctnormal1[0],
cctpnt1[1] + cctnormal1[1],
cctpnt1[2] + cctnormal1[2])
handfgrpcc = NodePath("handfgrpcc")
handfgrpcc0.reparentTo(handfgrpcc)
handfgrpcc1.reparentTo(handfgrpcc)
# prepare the model for collision detection
facetmeshbullnode = cd.genCollisionMeshMultiNp(handfgrpcc, brchild)
result = self.bulletworld.contactTest(facetmeshbullnode)
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))
if result.getNumContacts():
handfgrpcc0.setColor(1, 0, 0, .3)
handfgrpcc1.setColor(1, 0, 0, .3)
else:
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(brchild)
handfgrpcc1.reparentTo(brchild)
pandageom.plotArrow(star0,
spos=cctpnt0,
epos=cctpnt0 +
plotoffsetfp * self.facetnormals[facetidx0] +
cctnormal0,
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
],
length=10)
pandageom.plotArrow(star1,
spos=cctpnt1,
epos=cctpnt1 +
plotoffsetfp * self.facetnormals[facetidx1] +
cctnormal1,
rgba=[
facetcolorarray[facetidx1][0],
facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2],
facetcolorarray[facetidx1][3]
],
length=10)
base = pandactrl.World(camp=[1000, 400, 1000], lookatp=[400, 0, 0])
#this_dir, this_filename = os.path.split(__file__)
#objpath = os.path.join(os.path.split(this_dir)[0] + os.sep, "grip", "objects", "t2tube.stl")
this_dir = "E:/project/manipulation/regrasp_onworkcell/dropsimulation"
#objpath = os.path.join(this_dir, "objects", "t2tube.stl")
objpath = os.path.join(this_dir, "objects", "CameraFrontCase.stl")
workcellpath = os.path.join(this_dir, "objects", "ipadbox.stl")
#from manipulation.grip.hrp5three import hrp5threenm
#handpkg = hrp5threenm
handpkg = rtq85nm
print objpath
tps = TablePlacements(objpath, handpkg)
#plot obj and its convexhull
geom = pg.packpandageom(tps.objtrimesh.vertices,
tps.objtrimesh.face_normals, tps.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(1, 0, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.reparentTo(base.render)
# # build grid space on table range.
grids = []
for x in range(300, 500, 100):
for y in range(-400, -200, 100):
grids.append([x, y, -55])
print len(grids)
print grids
def removeFgrpccShow(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
:return:
author: weiwei
date: 20161201, osaka
"""
# 6 is used because I am supposing 4+2 where 4 is the default
# margin of bullet in panda3d. (NOTE: This is a guess)
plotoffsetfp = 6
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
#
# np0collection = base.render.findAllMatches("**/rtq85fgrpcc0")
# for np0 in np0collection:
# np0.removeNode()
# np1collection = base.render.findAllMatches("**/rtq85fgrpcc1")
# for np1 in np1collection:
# np1.removeNode()
#
# npscollection = base.render.findAllMatches("**/sphere")
# for nps in npscollection:
# nps.removeNode()
npbrchild = base.render.find("**/tempplot")
if npbrchild:
npbrchild.removeNode()
# for fast delete
brchild = NodePath('tempplot')
brchild.reparentTo(base.render)
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
facetcolorarray = self.facetcolorarray
star0.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(brchild)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
star1.setColor(Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
star1.setTwoSided(True)
star1.reparentTo(brchild)
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
# the following two choices decide the way to detect contacts
cctnormal00 = np.array(self.gripcontactpairnormals[self.counter][j][0])
cctnormal01 = -np.array(self.gripcontactpairnormals[self.counter][j][1])
cctnormal0raw = (cctnormal00 + cctnormal01)
cctnormal0 = (cctnormal0raw/np.linalg.norm(cctnormal0raw)).tolist()
# the following two choices decide the way to detect contacts
cctnormal10 = -cctnormal00
cctnormal11 = -cctnormal01
cctnormal1raw = (cctnormal10 + cctnormal11)
cctnormal1 = (cctnormal1raw/np.linalg.norm(cctnormal1raw)).tolist()
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc0)
handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2])
handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0], cctpnt0[1] + cctnormal0[1], cctpnt0[2] + cctnormal0[2])
handfgrpcc1 = NodePath("handfgrpcc1")
self.handfgrpcc_uninstanced.instanceTo(rtq85pcc1)
handfgrpcc1.setPos(cctpnt1[0], cctpnt1[1], cctpnt1[2])
handfgrpcc1.lookAt(cctpnt1[0] + cctnormal1[0], cctpnt1[1] + cctnormal1[1], cctpnt1[2] + cctnormal1[2])
handfgrpcc = NodePath("handfgrpcc")
handfgrpcc0.reparentTo(handfgrpcc)
handfgrpcc1.reparentTo(handfgrpcc)
# prepare the model for collision detection
facetmeshbullnode = cd.genCollisionMeshMultiNp(handfgrpcc, brchild)
result = self.bulletworld.contactTest(facetmeshbullnode)
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))
if result.getNumContacts():
handfgrpcc0.setColor(1, 0, 0, .3)
handfgrpcc1.setColor(1, 0, 0, .3)
else:
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(brchild)
handfgrpcc1.reparentTo(brchild)
pandageom.plotArrow(star0, spos=cctpnt0,
epos=cctpnt0 + plotoffsetfp*self.facetnormals[facetidx0] + cctnormal0,
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10)
pandageom.plotArrow(star1, spos=cctpnt1,
epos=cctpnt1 + plotoffsetfp*self.facetnormals[facetidx1] + cctnormal1,
rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]], length=10)
def segShow2(self,
base,
togglesamples=False,
togglenormals=False,
togglesamples_ref=False,
togglenormals_ref=False,
togglesamples_refcls=False,
togglenormals_refcls=False,
specificface=True):
"""
:param base:
:param togglesamples:
:param togglenormals:
:param togglesamples_ref: toggles the sampled points that fulfills the dist requirements
:param togglenormals_ref:
:return:
"""
nfacets = self.facets.shape[0]
facetcolorarray = self.facetcolorarray
rgbapnts0 = [1, 1, 1, 1]
rgbapnts1 = [0, 0, 1, 1]
rgbapnts2 = [1, 0, 0, 1]
# offsetf = facet
plotoffsetf = .0
faceplotted = False
# plot the segments
for i, facet in enumerate(self.facets):
if not specificface:
geom = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetf * i * self.facetnormals[i],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[facet],
self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(Vec4(.77, .17, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(
star,
pos=apnt + plotoffsetf * i * self.facetnormals[i],
radius=2.8,
rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(
star,
spos=apnt + plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls[i][j],
rgba=rgbapnts0,
length=10)
if togglesamples_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotSphere(
star,
pos=apnt + plotoffsetf * i * self.facetnormals[i],
radius=2.9,
rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(self.objsamplepnts_ref[i]):
pandageom.plotArrow(
star,
spos=apnt + plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1,
length=10)
if togglesamples_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotSphere(
star,
pos=apnt + plotoffsetf * i * self.facetnormals[i],
radius=3,
rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(self.objsamplepnts_refcls[i]):
pandageom.plotArrow(
star,
spos=apnt + plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2,
length=10)
if specificface:
plotoffsetf = .3
if faceplotted:
continue
else:
if len(self.objsamplepnts[i]) > 85:
faceplotted = True
geom = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetf * i * self.facetnormals[i],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[facet],
self.objtrimesh.faces[facet])
node = GeomNode('piece')
node.addGeom(geom)
star = NodePath('piece')
star.attachNewNode(node)
star.setColor(
Vec4(facetcolorarray[i][0], facetcolorarray[i][1],
facetcolorarray[i][2], 1))
star.setColor(Vec4(.7, .3, .3, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
# sampledpnts = samples[sample_idxes[i]]
# for apnt in sampledpnts:
# pandageom.plotSphere(base, star, pos=apnt, radius=1, rgba=rgba)
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(
star,
pos=apnt +
plotoffsetf * i * self.facetnormals[i],
radius=2.8,
rgba=rgbapnts0)
if togglenormals:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotArrow(
star,
spos=apnt +
plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls[i][j],
rgba=rgbapnts0,
length=10)
if togglesamples_ref:
for j, apnt in enumerate(
self.objsamplepnts_ref[i]):
pandageom.plotSphere(
star,
pos=apnt +
plotoffsetf * i * self.facetnormals[i],
radius=2.9,
rgba=rgbapnts1)
if togglenormals_ref:
for j, apnt in enumerate(
self.objsamplepnts_ref[i]):
pandageom.plotArrow(
star,
spos=apnt +
plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_ref[i][j],
rgba=rgbapnts1,
length=10)
if togglesamples_refcls:
for j, apnt in enumerate(
self.objsamplepnts_refcls[i]):
pandageom.plotSphere(
star,
pos=apnt +
plotoffsetf * i * self.facetnormals[i],
radius=3,
rgba=rgbapnts2)
if togglenormals_refcls:
for j, apnt in enumerate(
self.objsamplepnts_refcls[i]):
pandageom.plotArrow(
star,
spos=apnt +
plotoffsetf * i * self.facetnormals[i],
epos=apnt +
plotoffsetf * i * self.facetnormals[i] +
self.objsamplenrmls_refcls[i][j],
rgba=rgbapnts2,
length=10)
def planContactpairs(self, torqueresist=50, fgrtipdist=82):
"""
find the grasps using parallel pairs
:param: torqueresist the maximum allowable distance to com
:param: fgrtipdist the maximum dist between finger tips
:return:
author: weiwei
date: 20161130, harada office @ osaka university
"""
# note that pairnormals and pairfacets are duplicated for each contactpair
# the duplication is performed on purpose for convenient access
# also, each contactpair"s" corresponds to a facetpair
# it is empty when no contactpair is available
self.gripcontactpairs = []
# gripcontactpairnormals and gripcontactpairfacets are not helpful
# they are kept for convenience (they could be accessed using facetnormals and facetpairs)
self.gripcontactpairnormals = []
self.gripcontactpairfacets = []
# for precollision detection
# bulletworldprecc = BulletWorld()
# # build the collision shape of objtrimesh
# geomobj = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals,
# self.objtrimesh.faces)
# objmesh = BulletTriangleMesh()
# objmesh.addGeom(geomobj)
# objmeshnode = BulletRigidBodyNode('objmesh')
# objmeshnode.addShape(BulletTriangleMeshShape(objmesh, dynamic=True))
# bulletworldprecc.attachRigidBody(objmeshnode)
# for raytracing
bulletworldray = BulletWorld()
nfacets = self.facets.shape[0]
self.facetpairs = list(itertools.combinations(range(nfacets), 2))
for facetpair in self.facetpairs:
# print "facetpair"
# print facetpair, len(self.facetpairs)
self.gripcontactpairs.append([])
self.gripcontactpairnormals.append([])
self.gripcontactpairfacets.append([])
# if one of the facet doesnt have samples, jump to next
if self.objsamplepnts_refcls[facetpair[0]].shape[0] is 0 or \
self.objsamplepnts_refcls[facetpair[1]].shape[0] is 0:
# print "no sampled points"
continue
# check if the faces are opposite and parallel
dotnorm = np.dot(self.facetnormals[facetpair[0]],
self.facetnormals[facetpair[1]])
if dotnorm < -0.95:
# check if any samplepnts's projection from facetpairs[i][0] falls in the polygon of facetpairs[i][1]
facet0pnts = self.objsamplepnts_refcls[facetpair[0]]
facet0normal = self.facetnormals[facetpair[0]]
facet1normal = self.facetnormals[facetpair[1]]
# generate collision mesh
facetmesh = BulletTriangleMesh()
faceidsonfacet = self.facets[facetpair[1]]
geom = pandageom.packpandageom(
self.objtrimesh.vertices,
self.objtrimesh.face_normals[faceidsonfacet],
self.objtrimesh.faces[faceidsonfacet])
facetmesh.addGeom(geom)
facetmeshbullnode = BulletRigidBodyNode('facet')
facetmeshbullnode.addShape(
BulletTriangleMeshShape(facetmesh, dynamic=True))
bulletworldray.attachRigidBody(facetmeshbullnode)
# check the projection of a ray
for facet0pnt in facet0pnts:
pFrom = Point3(facet0pnt[0], facet0pnt[1], facet0pnt[2])
pTo = pFrom + Vec3(facet1normal[0], facet1normal[1],
facet1normal[2]) * 9999
result = bulletworldray.rayTestClosest(pFrom, pTo)
if result.hasHit():
hitpos = result.getHitPos()
if np.linalg.norm(
np.array(facet0pnt.tolist()) -
np.array([hitpos[0], hitpos[1], hitpos[2]])
) < fgrtipdist:
fgrcenter = (
np.array(facet0pnt.tolist()) + np.array(
[hitpos[0], hitpos[1], hitpos[2]])) / 2.0
# avoid large torque
if np.linalg.norm(self.objtrimesh.center_mass -
fgrcenter) < torqueresist:
self.gripcontactpairs[-1].append([
facet0pnt.tolist(),
[hitpos[0], hitpos[1], hitpos[2]]
])
self.gripcontactpairnormals[-1].append(
[[
facet0normal[0], facet0normal[1],
facet0normal[2]
],
[
facet1normal[0], facet1normal[1],
facet1normal[2]
]])
self.gripcontactpairfacets[-1].append(
facetpair)
# pre collision checking: it takes one finger as a cylinder and
# computes its collision with the object
## first finger
# cylindernode0 = BulletRigidBodyNode('cylindernode0')
# cylinder0height = 50
# cylinder0normal = Vec3(facet0normal[0], facet0normal[1], facet0normal[2])
# cylindernode0.addShape(BulletCylinderShape(radius=self.preccradius,
# height=cylinder0height,
# up=cylinder0normal),
# TransformState.makePos(pFrom+cylinder0normal*cylinder0height*.6))
# bulletworldprecc.attachRigidBody(cylindernode0)
# ## second finger
# cylindernode1 = BulletRigidBodyNode('cylindernode1')
# cylinder1height = cylinder1height
# # use the inverse of facet0normal instead of facet1normal to follow hand orientation
# cylinder1normal = Vec3(-facet0normal[0], -facet0normal[1], -facet0normal[2])
# cylindernode1.addShape(BulletCylinderShape(radius=self.preccradius,
# height=cylinder1height,
# up=cylinder1normal),
# TransformState.makePos(pFrom+cylinder1normal*cylinder1height*.6))
# bulletworldprecc.attachRigidBody(cylindernode1)
# if bulletworldprecc.contactTestPair(cylindernode0, objmeshnode) and \
# bulletworldprecc.contactTestPair(cylindernode1, objmeshnode):
bulletworldray.removeRigidBody(facetmeshbullnode)
# update the pairs
availablepairids = np.where(self.gripcontactpairs)[0]
self.facetpairs = np.array(self.facetpairs)[availablepairids]
self.gripcontactpairs = np.array(
self.gripcontactpairs)[availablepairids]
self.gripcontactpairnormals = np.array(
self.gripcontactpairnormals)[availablepairids]
self.gripcontactpairfacets = np.array(
self.gripcontactpairfacets)[availablepairids]
def pairShow(self, base, togglecontacts = False, togglecontactnormals = False):
# the following sentence requires segshow to be executed first
facetcolorarray = self.facetcolorarray
# offsetfp = facetpair
plotoffsetfp = np.random.random()*50
# plot the pairs and their contacts
# for i in range(self.counter+1, len(self.facetpairs)):
# if self.gripcontactpairs[i]:
# self.counter = i
# break
# if i is len(self.facetpairs):
# return
# delete the facetpair after show
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
# self.counter = 0
return
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(self.objtrimesh.vertices+
np.tile(plotoffsetfp*self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0],1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
star0.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(base.render)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
# star1.setColor(Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
# set to the same color
star1.setColor(Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star1.setTwoSided(True)
star1.reparentTo(base.render)
if togglecontacts:
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotSphere(star0, pos=cttpnt0+plotoffsetfp*self.facetnormals[facetidx0], radius=4,
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]])
# pandageom.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4,
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]])
# use the same color
pandageom.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4,
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]])
if togglecontactnormals:
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotArrow(star0, spos=cttpnt0+plotoffsetfp*self.facetnormals[facetidx0],
epos=cttpnt0 + plotoffsetfp*self.facetnormals[facetidx0] +
self.gripcontactpairnormals[self.counter][j][0],
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10)
# pandageom.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1],
# epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] +
# self.gripcontactpairnormals[self.counter][j][1],
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]], length=10)
# use the same color
pandageom.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1],
epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] +
self.gripcontactpairnormals[self.counter][j][1],
rgba=[facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]], length=10)
def pairShow(self, base, togglecontacts=False, togglecontactnormals=False):
# the following sentence requires segshow to be executed first
facetcolorarray = self.facetcolorarray
# offsetfp = facetpair
plotoffsetfp = np.random.random() * 50
# plot the pairs and their contacts
# for i in range(self.counter+1, len(self.facetpairs)):
# if self.gripcontactpairs[i]:
# self.counter = i
# break
# if i is len(self.facetpairs):
# return
# delete the facetpair after show
# np0 = base.render.find("**/pair0")
# if np0:
# np0.removeNode()
# np1 = base.render.find("**/pair1")
# if np1:
# np1.removeNode()
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
# self.counter = 0
return
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx0],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx0]],
self.objtrimesh.faces[self.facets[facetidx0]])
geomfacet1 = pandageom.packpandageom(
self.objtrimesh.vertices +
np.tile(plotoffsetfp * self.facetnormals[facetidx1],
[self.objtrimesh.vertices.shape[0], 1]),
self.objtrimesh.face_normals[self.facets[facetidx1]],
self.objtrimesh.faces[self.facets[facetidx1]])
# show the facetpair
node0 = GeomNode('pair0')
node0.addGeom(geomfacet0)
star0 = NodePath('pair0')
star0.attachNewNode(node0)
star0.setColor(
Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star0.setTwoSided(True)
star0.reparentTo(base.render)
node1 = GeomNode('pair1')
node1.addGeom(geomfacet1)
star1 = NodePath('pair1')
star1.attachNewNode(node1)
# star1.setColor(Vec4(facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]))
# set to the same color
star1.setColor(
Vec4(facetcolorarray[facetidx0][0], facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2], facetcolorarray[facetidx0][3]))
star1.setTwoSided(True)
star1.reparentTo(base.render)
if togglecontacts:
for j, contactpair in enumerate(
self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotSphere(
star0,
pos=cttpnt0 + plotoffsetfp * self.facetnormals[facetidx0],
radius=4,
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
])
# pandageom.plotSphere(star1, pos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1], radius=4,
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]])
# use the same color
pandageom.plotSphere(
star1,
pos=cttpnt1 + plotoffsetfp * self.facetnormals[facetidx1],
radius=4,
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
])
if togglecontactnormals:
for j, contactpair in enumerate(
self.gripcontactpairs[self.counter]):
cttpnt0 = contactpair[0]
cttpnt1 = contactpair[1]
pandageom.plotArrow(
star0,
spos=cttpnt0 + plotoffsetfp * self.facetnormals[facetidx0],
epos=cttpnt0 +
plotoffsetfp * self.facetnormals[facetidx0] +
self.gripcontactpairnormals[self.counter][j][0],
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
],
length=10)
# pandageom.plotArrow(star1, spos=cttpnt1+plotoffsetfp*self.facetnormals[facetidx1],
# epos=cttpnt1 + plotoffsetfp*self.facetnormals[facetidx1] +
# self.gripcontactpairnormals[self.counter][j][1],
# rgba=[facetcolorarray[facetidx1][0], facetcolorarray[facetidx1][1],
# facetcolorarray[facetidx1][2], facetcolorarray[facetidx1][3]], length=10)
# use the same color
pandageom.plotArrow(
star1,
spos=cttpnt1 + plotoffsetfp * self.facetnormals[facetidx1],
epos=cttpnt1 +
plotoffsetfp * self.facetnormals[facetidx1] +
self.gripcontactpairnormals[self.counter][j][1],
rgba=[
facetcolorarray[facetidx0][0],
facetcolorarray[facetidx0][1],
facetcolorarray[facetidx0][2],
facetcolorarray[facetidx0][3]
],
length=10)
startrotmat4 = rplacement1
goalrotmat4 = lplacement5
# one time show for start and end
objstart = pg.genObjmnp(objpath, color=Vec4(.7, .7, 0, 1))
objstart.setMat(startrotmat4)
objend = pg.genObjmnp(objpath, color=Vec4(.3, .3, 0, .5))
objend.setMat(goalrotmat4)
objstart.reparentTo(base.render)
objend.reparentTo(base.render)
pg.plotAxisSelf(base.render)
# show workcell
worktrimesh = trimesh.load_mesh(workcellpath)
geom1 = pg.packpandageom(worktrimesh.vertices, worktrimesh.face_normals,
worktrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom1)
workcell = NodePath('obj')
workcell.attachNewNode(node)
workcell.setPos(0, 0, 0)
workcell.reparentTo(base.render)
#show table
this_dir, this_filename = os.path.split(__file__)
ttpath = Filename.fromOsSpecific(
os.path.join(
os.path.split(this_dir)[0] + os.sep, "grip", "supports",
"tabletop.egg"))
ttnodepath = NodePath("tabletop")
ttnodepath.setPos(0, 0, -55)
