def genAvailableFAGsSgl(base, basepath, baseMat4, objpath, objMat4, gdb, handpkg):
"""
find the collision freeairgrips of objpath without considering rotation
:param base: panda base
:param basepath: the path of base object
:param objpath: the path of obj object, the object to be assembled
:param baseMat4, objMat4: all in world coordinates, not relative
:param gdb: grasp db
:param handpkg: hand package
:return: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
author: weiwei
date: 20170307
"""
bulletworld = BulletWorld()
robothand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
basetrimesh = trimesh.load_mesh(basepath)
basenp = pg.packpandanp(basetrimesh.vertices, basetrimesh.face_normals, basetrimesh.faces)
basenp.setMat(baseMat4)
basebullnode = cd.genCollisionMeshNp(basenp, base.render)
bulletworld.attachRigidBody(basebullnode)
dbobjname = os.path.splitext(os.path.basename(objpath))[0]
objfag = F*g(gdb, dbobjname, handpkg)
assgripcontacts = []
assgripnormals = []
assgriprotmat4s = []
assgripjawwidth = []
assgripidfreeair = []
for i, rotmat in enumerate(objfag.freegriprotmats):
assgrotmat = rotmat*objMat4
robothand.setMat(assgrotmat)
# detect the collisions when hand is open!
robothand.setJawwidth(robothand.jawwidthopen)
hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp)
result0 = bulletworld.contactTest(hndbullnode)
robothand.setJawwidth(objfag.freegripjawwidth[i])
hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp)
result1 = bulletworld.contactTest(hndbullnode)
if (not result0.getNumContacts()) and (not result1.getNumContacts()):
cct0 = objMat4.xformPoint(objfag.freegripcontacts[i][0])
cct1 = objMat4.xformPoint(objfag.freegripcontacts[i][1])
cctn0 = objMat4.xformPoint(objfag.freegripnormals[i][0])
cctn1 = objMat4.xformPoint(objfag.freegripnormals[i][1])
assgripcontacts.append([cct0, cct1])
assgripnormals.append([cctn0, cctn1])
assgriprotmat4s.append(assgrotmat)
assgripjawwidth.append(objfag.freegripjawwidth[i])
assgripidfreeair.append(objfag.freegripids[i])
return [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
def genAvailableFAGsSgl(base, basepath, baseMat4, objpath, objMat4, gdb, handpkg):
"""
find the collision freeairgrips of objpath without considering rotation
:param base: panda base
:param basepath: the path of base object
:param objpath: the path of obj object, the object to be assembled
:param baseMat4, objMat4: all in world coordinates, not relative
:param gdb: grasp db
:param handpkg: hand package
:return: [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
author: weiwei
date: 20170307
"""
bulletworld = BulletWorld()
robothand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
basetrimesh = trimesh.load_mesh(basepath)
basenp = pg.packpandanp(basetrimesh.vertices, basetrimesh.face_normals, basetrimesh.faces)
basenp.setMat(baseMat4)
basebullnode = cd.genCollisionMeshNp(basenp, base.render)
bulletworld.attachRigidBody(basebullnode)
dbobjname = os.path.splitext(os.path.basename(objpath))[0]
objfag = F*g(gdb, dbobjname, handpkg)
assgripcontacts = []
assgripnormals = []
assgriprotmat4s = []
assgripjawwidth = []
assgripidfreeair = []
for i, rotmat in enumerate(objfag.freegriprotmats):
assgrotmat = rotmat*objMat4
robothand.setMat(assgrotmat)
# detect the collisions when hand is open!
robothand.setJawwidth(robothand.jawwidthopen)
hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp)
result0 = bulletworld.contactTest(hndbullnode)
robothand.setJawwidth(objfag.freegripjawwidth[i])
hndbullnode = cd.genCollisionMeshMultiNp(robothand.handnp)
result1 = bulletworld.contactTest(hndbullnode)
if (not result0.getNumContacts()) and (not result1.getNumContacts()):
cct0 = objMat4.xformPoint(objfag.freegripcontacts[i][0])
cct1 = objMat4.xformPoint(objfag.freegripcontacts[i][1])
cctn0 = objMat4.xformPoint(objfag.freegripnormals[i][0])
cctn1 = objMat4.xformPoint(objfag.freegripnormals[i][1])
assgripcontacts.append([cct0, cct1])
assgripnormals.append([cctn0, cctn1])
assgriprotmat4s.append(assgrotmat)
assgripjawwidth.append(objfag.freegripjawwidth[i])
assgripidfreeair.append(objfag.freegripids[i])
return [assgripcontacts, assgripnormals, assgriprotmat4s, assgripjawwidth, assgripidfreeair]
class FreeTabletopPlacement(object):
"""
manipulation.freetabletopplacement doesn't take into account
the position and orientation of the object
it is "free" in position and rotation around z axis
in contrast, each item in regrasp.tabletopplacements
has different position and orientation
it is at a specific pose in the workspace
To clearly indicate the difference, "free" is attached
to the front of "freetabletopplacement"
"s" is attached to the end of "tabletopplacements"
"""
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()
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 loadFreeAirGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: weiwei
date: 20170110
"""
freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname,
handname=self.handname)
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripid = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def loadFreeTabletopPlacement(self):
"""
load free tabletopplacements
:return:
"""
tpsmat4s = self.gdb.loadFreeTabletopPlacement(self.dbobjname)
if tpsmat4s is not None:
self.tpsmat4s = tpsmat4s
return True
else:
self.tpsmat4s = []
return False
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 gentpsgrip(self, base):
"""
Originally the code of this function is embedded in the removebadfacet function
It is separated on 20170608 to enable common usage of placements for different hands
:return:
author: weiwei
date: 20170608
"""
self.tpsgripcontacts = []
self.tpsgripnormals = []
self.tpsgriprotmats = []
self.tpsgripjawwidth = []
# the id of the grip in freeair
self.tpsgripidfreeair = []
for i in range(len(self.tpsmat4s)):
self.tpsgripcontacts.append([])
self.tpsgripnormals.append([])
self.tpsgriprotmats.append([])
self.tpsgripjawwidth.append([])
self.tpsgripidfreeair.append([])
for j, rotmat in enumerate(self.freegriprotmats):
tpsgriprotmat = rotmat * self.tpsmat4s[i]
# check if the hand collide with tabletop
# tmprtq85 = self.rtq85hnd
tmphnd = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, 1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# open the hand to ensure it doesnt collide with surrounding obstacles
# tmprtq85.setJawwidth(self.freegripjawwidth[j])
tmphnd.setJawwidth(80)
tmphnd.setMat(tpsgriprotmat)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworldhp.contactTest(hndbullnode)
# print result.getNumContacts()
if not result.getNumContacts():
self.tpsgriprotmats[-1].append(tpsgriprotmat)
cct0 = self.tpsmat4s[i].xformPoint(
self.freegripcontacts[j][0])
cct1 = self.tpsmat4s[i].xformPoint(
self.freegripcontacts[j][1])
self.tpsgripcontacts[-1].append([cct0, cct1])
cctn0 = self.tpsmat4s[i].xformVec(
self.freegripnormals[j][0])
cctn1 = self.tpsmat4s[i].xformVec(
self.freegripnormals[j][1])
self.tpsgripnormals[-1].append([cctn0, cctn1])
self.tpsgripjawwidth[-1].append(self.freegripjawwidth[j])
self.tpsgripidfreeair[-1].append(self.freegripid[j])
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
def saveToDB(self):
"""
save freetabletopplacement
manipulation.freetabletopplacement doesn't take into account the position and orientation of the object
it is "free" in position and rotation around z axis
in contrast, each item in regrasp.tabletopplacements has different position and orientation
it is at a specific pose in the workspace
To clearly indicate the difference, "free" is attached to the front of "freetabletopplacement"
"s" is attached to the end of "tabletopplacements"
:param discretesize:
:param gdb:
:return:
author: weiwei
date: 20170111
"""
# save freetabletopplacement
sql = "SELECT * FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
# the fretabletopplacements for the self.dbobjname is not saved
sql = "INSERT INTO freetabletopplacement(rotmat, idobject) VALUES "
for i in range(len(self.tpsmat4s)):
sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \
(dc.mat4ToStr(self.tpsmat4s[i]), self.dbobjname)
sql = sql[:-2] + ";"
self.gdb.execute(sql)
else:
print "Freetabletopplacement already exist!"
# save freetabletopgrip
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT * FROM freetabletopgrip,freetabletopplacement,freeairgrip,object WHERE \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand = %d" % (
self.dbobjname, idhand)
result = self.gdb.execute(sql)
if len(result) == 0:
for i in range(len(self.tpsmat4s)):
sql = "SELECT freetabletopplacement.idfreetabletopplacement FROM freetabletopplacement,object WHERE \
freetabletopplacement.rotmat LIKE '%s' AND \
object.name LIKE '%s'" % (dc.mat4ToStr(
self.tpsmat4s[i]), self.dbobjname)
result = self.gdb.execute(sql)[0]
print result
if len(result) != 0:
idfreetabletopplacement = result[0]
# note self.tpsgriprotmats[i] might be empty (no cd-free grasps)
if len(self.tpsgriprotmats[i]) != 0:
sql = "INSERT INTO freetabletopgrip(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreetabletopplacement, idfreeairgrip) VALUES "
for j in range(len(self.tpsgriprotmats[i])):
cct0 = self.tpsgripcontacts[i][j][0]
cct1 = self.tpsgripcontacts[i][j][1]
cctn0 = self.tpsgripnormals[i][j][0]
cctn1 = self.tpsgripnormals[i][j][1]
sql += "('%s', '%s', '%s', '%s', '%s', '%s', %d, %d), " % \
(dc.v3ToStr(cct0), dc.v3ToStr(cct1), dc.v3ToStr(cctn0), dc.v3ToStr(cctn1), \
dc.mat4ToStr(self.tpsgriprotmats[i][j]), str(self.tpsgripjawwidth[i][j]), \
idfreetabletopplacement, self.tpsgripidfreeair[i][j])
sql = sql[:-2] + ";"
self.gdb.execute(sql)
else:
print "Freetabletopgrip already exist!"
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
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 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
class World():
CULLDISTANCE = 10
def __init__(self, size):
self.bw = BulletWorld()
self.bw.setGravity(0,0,0)
self.size = size
self.perlin = PerlinNoise2()
#utilities.app.accept('bullet-contact-added', self.onContactAdded)
#utilities.app.accept('bullet-contact-destroyed', self.onContactDestroyed)
self.player = Player(self)
self.player.initialise()
self.entities = list()
self.bgs = list()
self.makeChunk(Point2(0,0), Point2(3.0, 3.0))
self.cmap = buildMap(self.entities, self.player.location)
self.mps = list()
self.entities.append(Catcher(Point2(10, 10), self.player, self.cmap, self))
def update(self, timer):
dt = globalClock.getDt()
self.bw.doPhysics(dt, 5, 1.0/180.0)
self.doHits(Flame)
self.doHits(Catcher)
for entity in self.entities:
if entity.remove == True:
entity.destroy()
self.entities.remove(entity)
self.player.update(dt)
self.cmap = buildMap(self.entities, self.player.location)
for entity in self.entities:
entity.update(dt)
def doHits(self, hit_type):
for entity in self.entities:
if isinstance(entity, hit_type):
ctest = self.bw.contactTest(entity.bnode)
if ctest.getNumContacts() > 0:
entity.removeOnHit()
mp = ctest.getContacts()[0].getManifoldPoint()
if isinstance(ctest.getContacts()[0].getNode0().getPythonTag("entity"), hit_type):
ctest.getContacts()[0].getNode1().getPythonTag("entity").hitby(hit_type, mp.getIndex0())
else:
ctest.getContacts()[0].getNode0().getPythonTag("entity").hitby(hit_type, mp.getIndex0())
def makeChunk(self, pos, size):
self.bgs.append(utilities.loadObject("stars", depth=100, scaleX=200, scaleY=200.0, pos=Point2(pos.x*worldsize.x,pos.y*worldsize.y)))
genFillBox(self, Point2(5,5), 3, 6, 'metalwalls')
genBox(self, Point2(10,5), 2, 2, 'metalwalls')
#self.entities[0].bnode.applyTorque(Vec3(0,50,10))
def addEntity(self, entity):
self.entities.append(entity)
def onContactAdded(self, node1, node2):
e1 = node1.getPythonTag("entity")
e2 = node2.getPythonTag("entity")
if isinstance(e1, Flame):
e1.remove = True
if isinstance(e2, Flame):
e2.remove = True
print "contact"
def onContactDestroyed(self, node1, node2):
return
class Application(ShowBase):
def __init__(self):
ShowBase.__init__(self)
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.height = 85.0
self.img = PNMImage(Filename('height1.png'))
self.shape = BulletHeightfieldShape(self.img, self.height, ZUp)
self.offset = self.img.getXSize() / 2.0 - 0.5
self.terrain = GeoMipTerrain('terrain')
self.terrain.setHeightfield(self.img)
self.terrain.setColorMap("grass.png")
self.terrainNP = self.terrain.getRoot()
self.terrainNP.setSz(self.height)
self.terrainNP.setPos(-self.offset, -self.offset, -self.height / 2.0)
self.terrain.getRoot().reparentTo(render)
self.terrain.generate()
self.node = BulletRigidBodyNode('Ground')
self.node.addShape(self.shape)
self.np = render.attachNewNode(self.node)
self.np.setPos(0, 0, 0)
self.world.attachRigidBody(self.node)
self.info = self.world.getWorldInfo()
self.info.setAirDensity(1.2)
self.info.setWaterDensity(0)
self.info.setWaterOffset(0)
self.info.setWaterNormal(Vec3(0, 0, 0))
self.cam.setPos(20, 20, 20)
self.cam.setHpr(0, 0, 0)
self.model = loader.loadModel('out6.egg')
#self.model.setPos(0.0, 0.0, 0.0)
self.model.flattenLight()
min, max = self.model.getTightBounds()
size = max
mmax = size[0]
if size[1] > mmax:
mmax = size[1]
if size[2] > mmax:
mmax = size[2]
self.rocketScale = 20.0/mmax / 2
shape = BulletBoxShape(Vec3(size[0]*self.rocketScale, size[1]*self.rocketScale, size[2]*self.rocketScale))
self.ghostshape = BulletBoxShape(Vec3(size[0]*self.rocketScale, size[1]*self.rocketScale, size[2]*self.rocketScale))
self.ghost = BulletRigidBodyNode('Ghost')
self.ghost.addShape(self.ghostshape)
self.ghostNP = render.attachNewNode(self.ghost)
self.ghostNP.setPos(19.2220401046, 17.5158313723, 35.2665607047 )
#self.ghostNP.setCollideMask(BitMask32(0x0f))
self.model.setScale(self.rocketScale)
self.world.attachRigidBody(self.ghost)
self.model.copyTo(self.ghostNP)
self.rocketnode = BulletRigidBodyNode('rocketBox')
self.rocketnode.setMass(1.0)
self.rocketnode.addShape(shape)
self.rocketnp = render.attachNewNode(self.rocketnode)
self.rocketnp.setPos(0.0, 0.0, 40.0)
tex = loader.loadTexture('crate.jpg')
self.model.find('**/Line001').setTexture(tex, 1)
#self.rocketnp.setTexture(tex)
self.model.setScale(self.rocketScale)
self.world.attachRigidBody(self.rocketnode)
self.model.copyTo(self.rocketnp)
self.hh = 35.0
print self.ghostNP.getPos()
self.accept('w', self.eventKeyW)
self.accept('r', self.eventKeyR)
self.taskMgr.add(self.update, 'update')
self.massive = self.getdata('data.txt')
self.massiveResampled = self.resample(self.massive)
self.posInArray = 0
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.timerNow = 0.0
self.taskMgr.add(self.timerTask, 'timerTask')
def checkGhost(self):
'''
ghost = self.ghostNP.node()
if ghost.getNumOverlappingNodes() > 1:
print ghost.getNumOverlappingNodes()
for node in ghost.getOverlappingNodes():
print node
'''
result = self.world.contactTest(self.ghost)
for contact in result.getContacts():
print contact.getNode0()
print contact.getNode1()
def eventKeyW(self):
self.hh -= 1.0
def eventKeyR(self):
self.cam.setPos(self.x, self.y - 15.0, self.z + 2.50)
def resample(self, array):
len1 = len(array[0]) - 1
res = []
counter = 0
resamplerArray = []
bufArray = []
for i in xrange(len1):
resamplerArray.append(resampler(0.02, 0.125))
bufArray.append([])
for i in xrange(len(array)):
buf = []
for j in xrange(len1):
bufArray[j] = resamplerArray[j].feed(array[i][j+1])
for j in xrange(len(bufArray[0])):
buf.append(0.02*(counter))
for k in xrange(len1):
buf.append(bufArray[k][j])
res.append(buf)
counter += 1
buf = []
return res
def getdata(self, name):
array = []
with open(name) as f:
for line in f:
numbers_float = map(float, line.split())
array.append([numbers_float[0], numbers_float[1], numbers_float[2], numbers_float[3]])
return array
def update(self, task):
dt = globalClock.getDt()
#self.world.doPhysics(dt)
self.getMyPos(self.massiveResampled)
self.rocketnp.setPos(self.x, self.y, self.z)
self.cam.setPos(self.x, self.y - 15.0, self.z + 2.50)
return task.cont
def getMyPos(self, array):
flag = False
len1 = len(array)
if self.posInArray >= len1:
flag = True
while not flag:
if self.timerNow < array[self.posInArray][0]:
flag = True
self.x = array[self.posInArray][1]
self.y = array[self.posInArray][2]
self.z = array[self.posInArray][3]
break
else:
if self.posInArray >= len1:
flag = True
break
else:
self.posInArray += 1
self.checkGhost()
result = self.world.contactTest(self.rocketnode)
for contact in result.getContacts():
print contact.getNode0()
print contact.getNode1()
def timerTask(self, task):
self.timerNow = task.time
return Task.cont
class Taskik():
def __init__(self, objpath, robot, handpkg, gdb, offset=-55):
self.objtrimesh = trimesh.load_mesh(objpath)
self.handpkg = handpkg
self.handname = handpkg.getHandName()
# for dbaccess
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
# regg = regrip graph
self.regg = nx.Graph()
self.ndiscreterot = 0
self.nplacements = 0
self.globalgripids = []
# for removing the grasps at start and goal
self.robothand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
# plane to remove hand
self.bulletworld = BulletWorld()
self.planebullnode = cd.genCollisionPlane(offset=offset)
self.bulletworld.attachRigidBody(self.planebullnode)
# add tabletop plane model to bulletworld
# dont forget offset
# 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"))
# self.ttnodepath = NodePath("tabletop")
# ttl = loader.loadModel(ttpath)
# ttl.instanceTo(self.ttnodepath)
self.startnodeids = None
self.goalnodeids = None
self.shortestpaths = None
self.gdb = gdb
self.robot = robot
# load retraction distances
self.rethandx, self.retworldz, self.retworlda, self.worldz = self.gdb.loadIKRet(
)
# worlda is default for the general grasps on table top
# for assembly at start and goal, worlda is computed by assembly planner
self.worlda = Vec3(0, 0, 1)
# loadfreeairgrip
self.__loadFreeAirGrip()
def __loadFreeAirGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: weiwei
date: 20170110
"""
freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname,
self.handname)
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripid = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def savestartik(self, startrotmat4, startid, base):
nodeidofglobalidinstart = {}
# the startnodeids is also for quick access
self.startnodeids = []
for j, rotmat in enumerate(self.freegriprotmats):
# print j, len(self.freegriprotmats)
# # print rotmat
ttgsrotmat = rotmat * startrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(startrotmat4)
ttgsrotmatx0y0.setCell(3, 0, 0)
ttgsrotmatx0y0.setCell(3, 1, 0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
# tmphnd = self.robothand
tmphnd = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# set jawwidth to 80 to avoid collision with surrounding obstacles
# set to gripping with is unnecessary
tmphnd.setJawwidth(80)
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
ttgscct0 = startrotmat4.xformPoint(self.freegripcontacts[j][0])
ttgscct1 = startrotmat4.xformPoint(self.freegripcontacts[j][1])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx * self.rethandx
# handxworldz is not necessary for start
# ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda * self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + self.worldz * self.retworldz
ttgsjawwidth = self.freegripjawwidth[j]
ttgsidfreeair = self.freegripid[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
# handxworldz is not necessary for start
# ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(
ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
# print "solving starting iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz,
ttgsrotmat3np)
feasibility = {}
feasibility_handx = {}
#feasibility_handxworldz = {}
feasibility_worlda = {}
feasibility_worldaworldz = {}
if ikc is not None:
feasibility[j] = 'True'
else:
feasibility[j] = 'False'
if ikcx is not None:
feasibility_handx[j] = 'True'
else:
feasibility_handx[j] = 'False'
if ikca is not None:
feasibility_worlda[j] = 'True'
else:
feasibility_worlda[j] = 'False'
if ikcaz is not None:
feasibility_worldaworldz[j] = 'True'
else:
feasibility_worldaworldz[j] = 'False'
sql = "INSERT IGNORE INTO ik_start(idstart,idfreeairgrip, feasibility, feasibility_handx, \
feasibility_worlda, feasibility_worldaworldz) VALUES (%d,%d, '%s', '%s', '%s', '%s')" % \
(startid, j, feasibility[j], feasibility_handx[j],\
feasibility_worlda[j], feasibility_worldaworldz[j])
self.gdb.execute(sql)
def selectstartik(self, startrotmat4, startid, j, base):
sql = "SELECT ik_start.idstart,ik_start.idfreeairgrip,ik_start.feasibility, ik_start.feasibility_handx, \
ik_start.feasibility_worlda, ik_start.feasibility_worldaworldz FROM ik_start\
WHERE ik_start.idstart=%d AND ik_start.idfreeairgrip=%d \
AND ik_start.feasibility='True' AND ik_start.feasibility_handx='True' \
AND ik_start.feasibility_worlda='True' AND ik_start.feasibility_worldaworldz='True' " % (
startid, j)
result = self.gdb.execute(sql)
if len(result) != 0:
return True
else:
#print "no ik this start this grip"
return False
def savegoalik(self, goalrotmat4, goalid, base):
### goal
# the node id of a globalgripid in goalnode, for quick setting up edges
nodeidofglobalidingoal = {}
# the goalnodeids is also for quick access
self.goalnodeids = []
for j, rotmat in enumerate(self.freegriprotmats):
# print j, len(self.freegriprotmats)
ttgsrotmat = rotmat * goalrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(goalrotmat4)
ttgsrotmatx0y0.setCell(3, 0, 0)
ttgsrotmatx0y0.setCell(3, 1, 0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
tmphnd = self.robothand
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
tmphnd.setJawwidth(self.freegripjawwidth[j])
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
ttgscct0 = goalrotmat4.xformPoint(self.freegripcontacts[j][0])
ttgscct1 = goalrotmat4.xformPoint(self.freegripcontacts[j][1])
ttgsfgrcenter = (ttgscct0 + ttgscct1) / 2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx * self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz * self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda * self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda + self.worldz * self.retworldz
ttgsjawwidth = self.freegripjawwidth[j]
ttgsidfreeair = self.freegripid[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(
ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(
ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
# print "solving goal iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikcxz = self.robot.numikr(ttgsfgrcenternp_handxworldz,
ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz,
ttgsrotmat3np)
feasibility = {}
feasibility_handx = {}
feasibility_handxworldz = {}
feasibility_worlda = {}
feasibility_worldaworldz = {}
if ikc is not None:
feasibility[j] = 'True'
else:
feasibility[j] = 'False'
if ikcx is not None:
feasibility_handx[j] = 'True'
else:
feasibility_handx[j] = 'False'
if ikcxz is not None:
feasibility_handxworldz[j] = 'True'
else:
feasibility_handxworldz[j] = 'False'
if ikca is not None:
feasibility_worlda[j] = 'True'
else:
feasibility_worlda[j] = 'False'
if ikcaz is not None:
feasibility_worldaworldz[j] = 'True'
else:
feasibility_worldaworldz[j] = 'False'
sql = "INSERT IGNORE INTO ik_goal(idgoal,idfreeairgrip, feasibility, feasibility_handx, feasibility_handxworldz, \
feasibility_worlda, feasibility_worldaworldz) VALUES (%d,%d, '%s', '%s', '%s', '%s', '%s')" % \
(goalid, j, feasibility[j], feasibility_handx[j], \
feasibility_handxworldz[j], \
feasibility_worlda[j], feasibility_worldaworldz[j])
self.gdb.execute(sql)
def selectgoalik(self, goalrotmat4, goalid, j, base):
sql = "SELECT ik_goal.idgoal,ik_goal.idfreeairgrip,ik_goal.feasibility, ik_goal.feasibility_handx,ik_goal.feasibility_handxworldz, \
ik_goal.feasibility_worlda, ik_goal.feasibility_worldaworldz FROM ik_goal\
WHERE ik_goal.idgoal=%d AND ik_goal.idfreeairgrip=%d AND\
ik_goal.feasibility='True' AND ik_goal.feasibility_handx='True' AND ik_goal.feasibility_handxworldz='True' \
AND ik_goal.feasibility_worlda='True' AND ik_goal.feasibility_worldaworldz='True' \
" % (goalid, j)
result = self.gdb.execute(sql)
if len(result) != 0:
return True
else:
#print "no ik this start this grip"
return False
class RegripTppAss(object):
def __init__(self, objpath, nxtrobot, handpkg, gdb):
self.graphtpp0 = GraphTpp(objpath, nxtrobot, handpkg, gdb, 'rgt')
self.armname = armname
self.gdb = gdb
self.robot = nxtrobot
self.hand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
# plane to remove hand
self.bulletworld = BulletWorld()
self.planebullnode = cd.genCollisionPlane()
self.bulletworld.attachRigidBody(self.planebullnode)
# add tabletop plane model to bulletworld
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"))
self.ttnodepath = NodePath("tabletop")
ttl = loader.loadModel(ttpath)
ttl.instanceTo(self.ttnodepath)
# self.endnodeids is a dictionary where
# self.endnodeids['rgt'] equals self.startnodeids
# self.endnodeids['lft'] equals self.endnodeids # in right->left order
self.endnodeids = {}
# load retraction distances
self.rethandx, self.retworldz, self.retworlda, self.worldz = self.gdb.loadIKRet(
)
# worlda is default for the general grasps on table top
# for assembly at start and goal, worlda is computed by assembly planner
self.worlda = Vec3(0, 0, 1)
self.gnodesplotpos = {}
self.freegripid = []
self.freegripcontacts = []
self.freegripnormals = []
self.freegriprotmats = []
self.freegripjawwidth = []
# for start and goal grasps poses:
radiusgrip = 1
self.__xyzglobalgrippos_startgoal = {}
for k, globalgripid in enumerate(self.graphtpp.globalgripids):
xypos = [
radiusgrip *
math.cos(2 * math.pi / len(self.graphtpp.globalgripids) * k),
radiusgrip *
math.sin(2 * math.pi / len(self.graphtpp.globalgripids) * k)
]
self.__xyzglobalgrippos_startgoal[globalgripid] = [
xypos[0], xypos[1], 0
]
self.__loadFreeAirGrip()
@property
def dbobjname(self):
# read-only property
return self.graphtpp.dbobjname
def __loadFreeAirGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: weiwei
date: 20170110
"""
freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname)
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripid = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def addEnds(self, rotmat4, armname):
# the node id of a globalgripid in end
nodeidofglobalidinend = {}
# the endnodeids is also for quick access
self.endnodeids[armname] = []
for j, rotmat in enumerate(self.freegriprotmats):
assgsrotmat = rotmat * rotmat4
# for collision detection, we move the object back to x=0,y=0
assgsrotmatx0y0 = Mat4(rotmat4)
assgsrotmatx0y0.setCell(3, 0, 0)
assgsrotmatx0y0.setCell(3, 1, 0)
assgsrotmatx0y0 = rotmat * assgsrotmatx0y0
# check if the hand collide with tabletop
tmphand = self.hand
initmat = tmphand.getMat()
initjawwidth = tmphand.jawwidth
# set jawwidth to 80 to avoid collision with surrounding obstacles
# set to gripping with is unnecessary
# tmphand.setJawwidth(self.freegripjawwidth[j])
tmphand.setJawwidth(tmphand.jawwidthopen)
tmphand.setMat(assgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
assgscct0 = rotmat4.xformPoint(self.freegripcontacts[j][0])
assgscct1 = rotmat4.xformPoint(self.freegripcontacts[j][1])
assgsfgrcenter = (assgscct0 + assgscct1) / 2
handx = assgsrotmat.getRow3(0)
assgsfgrcenterhandx = assgsfgrcenter + handx * self.rethandx
# handxworldz is not necessary for start
# assgsfgrcenterhandxworldz = assgsfgrcenterhandx + self.worldz*self.retworldz
assgsfgrcenterworlda = assgsfgrcenter + self.worlda * self.retworlda
assgsfgrcenterworldaworldz = assgsfgrcenterworlda + self.worldz * self.retworldz
assgsjawwidth = self.freegripjawwidth[j]
assgsidfreeair = self.freegripid[j]
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsfgrcenternp_handx = pg.v3ToNp(assgsfgrcenterhandx)
# handxworldz is not necessary for start
# assgsfgrcenternp_handxworldz = pg.v3ToNp(assgsfgrcenterhandxworldz)
assgsfgrcenternp_worlda = pg.v3ToNp(assgsfgrcenterworlda)
assgsfgrcenternp_worldaworldz = pg.v3ToNp(
assgsfgrcenterworldaworldz)
assgsrotmat3np = pg.mat3ToNp(assgsrotmat.getUpper3())
ikc = self.robot.numikr(assgsfgrcenternp, assgsrotmat3np)
ikcx = self.robot.numikr(assgsfgrcenternp_handx,
assgsrotmat3np)
ikca = self.robot.numikr(assgsfgrcenternp_worlda,
assgsrotmat3np)
ikcaz = self.robot.numikr(assgsfgrcenternp_worldaworldz,
assgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (
ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = rotmat4.getRow3(3)
startrotmat4worlda = Mat4(rotmat4)
startrotmat4worlda.setRow(
3,
rotmat4.getRow3(3) + self.worlda * self.retworlda)
startrotmat4worldaworldz = Mat4(startrotmat4worlda)
startrotmat4worldaworldz.setRow(
3,
startrotmat4worlda.getRow3(3) +
self.worldz * self.retworldz)
self.graphtpp.regg.add_node(
'end' + armname + str(j),
fgrcenter=assgsfgrcenternp,
fgrcenterhandx=assgsfgrcenternp_handx,
fgrcenterhandxworldz='na',
fgrcenterworlda=assgsfgrcenternp_worlda,
fgrcenterworldaworldz=assgsfgrcenternp_worldaworldz,
jawwidth=assgsjawwidth,
hndrotmat3np=assgsrotmat3np,
globalgripid=assgsidfreeair,
freetabletopplacementid='na',
tabletopplacementrotmat=rotmat4,
tabletopplacementrotmathandx=rotmat4,
tabletopplacementrotmathandxworldz='na',
tabletopplacementrotmatworlda=startrotmat4worlda,
tabletopplacementrotmatworldaworldz=
startrotmat4worldaworldz,
angle='na',
tabletopposition=tabletopposition)
nodeidofglobalidinend[assgsidfreeair] = 'start' + str(j)
self.endnodeids[armname].append('start' + str(j))
tmphand.setMat(initmat)
tmphand.setJawwidth(initjawwidth)
if len(self.endnodeids[armname]) == 0:
raise ValueError("No available end grip at " + armname)
# add start transit edge
for edge in list(itertools.combinations(self.endnodeids[armname], 2)):
self.graphtpp.regg.add_edge(*edge, weight=1, edgetype='endtransit')
# add start transfer edge
for reggnode, reggnodedata in self.graphtpp.regg.nodes(data=True):
if reggnode.startswith(self.armname):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidinend.keys():
endnodeid = nodeidofglobalidinend[globalgripid]
self.graphtpp.regg.add_edge(endnodeid,
reggnode,
weight=1,
edgetype='endtransfer')
for nid in self.graphtpp.regg.nodes():
if nid.startswith('end'):
ggid = self.graphtpp.regg.node[nid]['globalgripid']
tabletopposition = self.graphtpp.regg.node[nid][
'tabletopposition']
xyzpos = map(add, self.__xyzglobalgrippos_startgoal[ggid], [
tabletopposition[0], tabletopposition[1],
tabletopposition[2]
])
self.gnodesplotpos[nid] = xyzpos[:2]
def plotGraph(self, pltax, endname='start', gtppoffset=[0, 0]):
"""
:param pltax:
:param endname:
:param gtppoffset: where to plot graphtpp, see the plotGraph function of GraphTpp class
:return:
"""
self.graphtpp.plotGraph(pltax, offset=gtppoffset)
self.__plotEnds(pltax, endname)
def __plotEnds(self, pltax, endname='end'):
transitedges = []
transferedges = []
for nid0, nid1, reggedgedata in self.graphtpp.regg.edges(data=True):
if nid0.startswith('end'):
pos0 = self.gnodesplotpos[nid0][:2]
else:
pos0 = self.graphtpp.gnodesplotpos[nid0][:2]
if nid1.startswith('end'):
pos1 = self.gnodesplotpos[nid1][:2]
else:
pos1 = self.graphtpp.gnodesplotpos[nid1][:2]
if (reggedgedata['edgetype'] == 'endtransit'):
transitedges.append([pos0, pos1])
elif (reggedgedata['edgetype'] is 'endtransfer'):
transferedges.append([pos0, pos1])
transitec = mc.LineCollection(transitedges,
colors=[.5, 0, 0, .3],
linewidths=1)
transferec = mc.LineCollection(transferedges,
colors=[1, 0, 0, .3],
linewidths=1)
pltax.add_collection(transferec)
pltax.add_collection(transitec)
class Dropworkcellgrip(object):
"""
manipulation.Dropworkcellgrip take into account
the position and orientation of the object
in position and rotation around z axis
"""
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 loadIDObj(self):
sql = "SELECT idobject FROM object WHERE name LIKE '%s'" % self.dbobjname
returnlist = gdb.execute(sql)
if len(returnlist) != 0:
idobject = returnlist[0][0]
else:
sql = "INSERT INTO object(name) VALUES('%s')" % self.dbobjname
idobject = gdb.execute(sql)
return idobject
def loadObjModel(self, ompath, objrotmat, objposmat):
#print "loadObjModel(self, ompath,objrotmat,objposmat):", objrotmat, objposmat
self.objtrimesh = trimesh.load_mesh(ompath)
# add pos and rot to origin trimensh
self.objtrimesh.vertices = np.transpose(np.dot(objrotmat, np.transpose(self.objtrimesh.vertices)))
self.objtrimesh.vertices = self.objtrimesh.vertices + objposmat
def loadDropStablePos(self):
"""
load self.dropid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: jiayao
date: 20170810
"""
self.gdb=db.GraspDB()
dropstableposdata = self.gdb.loadDropStablePos(self.dbobjname)
if dropstableposdata is None:
raise ValueError("Plan the drop stable pos first!")
# "success load drop stable pos"
objrotmat = dropstableposdata[1][self.dbidstablepos-1]
objposmat = dropstableposdata[2][self.dbidstablepos-1]
return [objrotmat,objposmat]
def loadDropfreeGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: jiayao
date: 20170811
"""
freeairgripdata = self.gdb.loadDropFreeGrip(self.dbobjname, handname = self.handname,idstablepos=self.dbidstablepos)
if freeairgripdata is None:
print("no freeairgrip")
return None
#raise ValueError("Plan the freeairgrip first!")
else:
self.freegripid = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
self.freeairgripid = freeairgripdata[5]
return 1
def gentpsgrip(self, base):
"""
Originally the code of this function is embedded in the removebadfacet function
It is separated on 20170608 to enable common usage of placements for different hands
:return:
author: weiwei
date: 20170608
"""
self.tpsgripcontacts = []
self.tpsgripnormals = []
self.tpsgriprotmats = []
self.tpsgripjawwidth = []
# the id of the grip in freeair
self.tpsgripidfreeair = []
self.tpsgripiddropfree = []
for j, rotmat in enumerate(self.freegriprotmats):
tpsgriprotmat = rotmat
# check if the hand collide with tabletop
# tmprtq85 = self.rtq85hnd
tmphnd = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, 1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# open the hand to ensure it doesnt collide with surrounding obstacles
# tmprtq85.setJawwidth(self.freegripjawwidth[j])
tmphnd.setJawwidth(80)
tmphnd.setMat(tpsgriprotmat)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworldhp.contactTest(hndbullnode)
# print result.getNumContacts()
if not result.getNumContacts():
self.tpsgriprotmats.append(tpsgriprotmat)
cct0 = self.freegripcontacts[j][0]
cct1 = self.freegripcontacts[j][1]
self.tpsgripcontacts.append([cct0, cct1])
cctn0 = self.freegripnormals[j][0]
cctn1 = self.freegripnormals[j][1]
self.tpsgripnormals.append([cctn0, cctn1])
self.tpsgripjawwidth.append(self.freegripjawwidth[j])
self.tpsgripidfreeair.append(self.freeairgripid[j])
self.tpsgripiddropfree.append(self.freegripid[j])
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
def saveToDB(self):
"""
save dropworkcellgrip
dropworkcellgrip take the position and orientation of stable object on th eworkcell into account ,
:param discretesize:
:param gdb:
:return:
author: jiayao
date: 20170816
"""
# save freetabletopgrip
idhand = gdb.loadIdHand(self.handname)
idobject = gdb.loadIdObject(self.dbobjname)
for i in range(len(self.tpsgripcontacts)):
#print self.freeairgripid[i]
sql = "INSERT INTO freegrip.dropworkcellgrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand,iddropstablepos,iddropfreegrip,idfreeairgrip) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d, %d,%d,%d)" % \
(idobject, dc.v3ToStr(self.tpsgripcontacts[i][0]), dc.v3ToStr(self.tpsgripcontacts[i][1]),
dc.v3ToStr(self.tpsgripnormals[i][0]), dc.v3ToStr(self.tpsgripnormals[i][1]),
dc.mat4ToStr(self.tpsgriprotmats[i]), str(self.tpsgripjawwidth[i]), idhand, self.dbidstablepos,self.tpsgripiddropfree[i], \
self.tpsgripidfreeair[i])
gdb.execute(sql)
print "save ok"
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"
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
def grpshow(self, base,grip):
sql = "SELECT dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth FROM dropworkcellgrip WHERE \
dropworkcellgrip.iddropstablepos=%d\
AND dropworkcellgrip.iddropworkcellgrip=%d" % (self.dbidstablepos,grip)
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 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 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)
class CarEnv(DirectObject):
def __init__(self, params={}):
self._params = params
if 'random_seed' in self._params:
np.random.seed(self._params['random_seed'])
self._use_vel = self._params.get('use_vel', True)
self._run_as_task = self._params.get('run_as_task', False)
self._do_back_up = self._params.get('do_back_up', False)
self._use_depth = self._params.get('use_depth', False)
self._use_back_cam = self._params.get('use_back_cam', False)
self._collision_reward = self._params.get('collision_reward', 0.)
if not self._params.get('visualize', False):
loadPrcFileData('', 'window-type offscreen')
# Defines base, render, loader
try:
ShowBase()
except:
pass
base.setBackgroundColor(0.0, 0.0, 0.0, 1)
# World
self._worldNP = render.attachNewNode('World')
self._world = BulletWorld()
self._world.setGravity(Vec3(0, 0, -9.81))
self._dt = params.get('dt', 0.25)
self._step = 0.05
# Vehicle
shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
ts = TransformState.makePos(Point3(0., 0., 0.25))
self._vehicle_node = BulletRigidBodyNode('Vehicle')
self._vehicle_node.addShape(shape, ts)
self._mass = self._params.get('mass', 10.)
self._vehicle_node.setMass(self._mass)
self._vehicle_node.setDeactivationEnabled(False)
self._vehicle_node.setCcdSweptSphereRadius(1.0)
self._vehicle_node.setCcdMotionThreshold(1e-7)
self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)
self._world.attachRigidBody(self._vehicle_node)
self._vehicle = BulletVehicle(self._world, self._vehicle_node)
self._vehicle.setCoordinateSystem(ZUp)
self._world.attachVehicle(self._vehicle)
self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)
# Camera
size = self._params.get('size', [160, 90])
hfov = self._params.get('hfov', 60)
near_far = self._params.get('near_far', [0.1, 100.])
self._camera_sensor = Panda3dCameraSensor(base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='front cam')
self._camera_node = self._camera_sensor.cam
self._camera_node.setPos(0.0, 0.5, 0.375)
self._camera_node.lookAt(0.0, 6.0, 0.0)
self._camera_node.reparentTo(self._vehicle_pointer)
if self._use_back_cam:
self._back_camera_sensor = Panda3dCameraSensor(
base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='back cam')
self._back_camera_node = self._back_camera_sensor.cam
self._back_camera_node.setPos(0.0, -0.5, 0.375)
self._back_camera_node.lookAt(0.0, -6.0, 0.0)
self._back_camera_node.reparentTo(self._vehicle_pointer)
# Car Simulator
self._des_vel = None
self._setup()
# Input
self.accept('escape', self._doExit)
self.accept('r', self.reset)
self.accept('f1', self._toggleWireframe)
self.accept('f2', self._toggleTexture)
self.accept('f3', self._view_image)
self.accept('f5', self._doScreenshot)
self.accept('q', self._forward_0)
self.accept('w', self._forward_1)
self.accept('e', self._forward_2)
self.accept('a', self._left)
self.accept('s', self._stop)
self.accept('x', self._backward)
self.accept('d', self._right)
self.accept('m', self._mark)
self._steering = 0.0 # degree
self._engineForce = 0.0
self._brakeForce = 0.0
self._p = self._params.get('p', 1.25)
self._d = self._params.get('d', 0.0)
self._last_err = 0.0
self._curr_time = 0.0
self._accelClamp = self._params.get('accelClamp', 2.0)
self._engineClamp = self._accelClamp * self._mass
self._collision = False
if self._run_as_task:
self._mark_d = 0.0
taskMgr.add(self._update_task, 'updateWorld')
base.run()
# _____HANDLER_____
def _doExit(self):
sys.exit(1)
def _toggleWireframe(self):
base.toggleWireframe()
def _toggleTexture(self):
base.toggleTexture()
def _doScreenshot(self):
base.screenshot('Bullet')
def _forward_0(self):
self._des_vel = 1
self._brakeForce = 0.0
def _forward_1(self):
self._des_vel = 2
self._brakeForce = 0.0
def _forward_2(self):
self._des_vel = 4
self._brakeForce = 0.0
def _stop(self):
self._des_vel = 0.0
self._brakeForce = 0.0
def _backward(self):
self._des_vel = -4
self._brakeForce = 0.0
def _right(self):
self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])
def _left(self):
self._steering = np.max([np.min([30, self._steering + 5]), 0.0])
def _view_image(self):
from matplotlib import pyplot as plt
image = self._camera_sensor.observe()[0]
if self._use_depth:
plt.imshow(image[:, :, 0], cmap='gray')
else:
import cv2
def rgb2gray(rgb):
return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])
image = rgb2gray(image)
im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
) # TODO how does this deal with aspect ratio
plt.imshow(im.astype(np.uint8), cmap='Greys_r')
plt.show()
def _mark(self):
self._mark_d = 0.0
# Setup
def _setup(self):
if hasattr(self, '_model_path'):
# Collidable objects
visNP = loader.loadModel(self._model_path)
visNP.clearModelNodes()
visNP.reparentTo(render)
pos = (0., 0., 0.)
visNP.setPos(pos[0], pos[1], pos[2])
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
bodyNP.setPos(pos[0], pos[1], pos[2])
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
bodyNP.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(bodyNP.node())
else:
ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
ground.node().addShape(shape)
ground.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(ground.node())
self._place_vehicle()
self._setup_light()
self._setup_restart_pos()
def _setup_restart_pos(self):
self._restart_pos = []
self._restart_index = 0
if self._params.get('position_ranges', None) is not None:
ranges = self._params['position_ranges']
num_pos = self._params['num_pos']
if self._params.get('range_type', 'random') == 'random':
for _ in range(num_pos):
ran = ranges[np.random.randint(len(ranges))]
self._restart_pos.append(np.random.uniform(ran[0], ran[1]))
elif self._params['range_type'] == 'fix_spacing':
num_ran = len(ranges)
num_per_ran = num_pos // num_ran
for i in range(num_ran):
ran = ranges[i]
low = np.array(ran[0])
diff = np.array(ran[1]) - np.array(ran[0])
for j in range(num_per_ran):
val = diff * ((j + 0.0) / num_per_ran) + low
self._restart_pos.append(val)
elif self._params.get('positions', None) is not None:
self._restart_pos = self._params['positions']
else:
self._restart_pos = self._default_restart_pos()
def _next_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
pos_hpr = self._restart_pos[self._restart_index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _next_random_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
index = np.random.randint(num)
pos_hpr = self._restart_pos[index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _setup_light(self):
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
render.clearLight()
render.setLight(alightNP)
# Vehicle
def _default_pos(self):
return (0.0, 0.0, 0.3)
def _default_hpr(self):
return (0.0, 0.0, 3.14)
def _default_restart_pos():
return [self._default_pos() + self._default_hpr()]
def _get_speed(self):
vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
return vel
def _update(self, dt=1.0, coll_check=True):
self._vehicle.setSteeringValue(self._steering, 0)
self._vehicle.setSteeringValue(self._steering, 1)
self._vehicle.setBrake(self._brakeForce, 0)
self._vehicle.setBrake(self._brakeForce, 1)
self._vehicle.setBrake(self._brakeForce, 2)
self._vehicle.setBrake(self._brakeForce, 3)
if dt >= self._step:
# TODO maybe change number of timesteps
for i in range(int(dt / self._step)):
if self._des_vel is not None:
vel = self._get_speed()
err = self._des_vel - vel
d_err = (err - self._last_err) / self._step
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(self._step, 1, self._step)
self._collision = self._is_contact()
elif self._run_as_task:
self._curr_time += dt
if self._curr_time > 0.05:
if self._des_vel is not None:
vel = self._get_speed()
self._mark_d += vel * self._curr_time
print(vel, self._mark_d, self._is_contact())
err = self._des_vel - vel
d_err = (err - self._last_err) / 0.05
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._curr_time = 0.0
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(dt, 1, dt)
self._collision = self._is_contact()
else:
raise ValueError(
"dt {0} s is too small for velocity control".format(dt))
def _stop_car(self):
self._steering = 0.0
self._engineForce = 0.0
self._vehicle.setSteeringValue(0.0, 0)
self._vehicle.setSteeringValue(0.0, 1)
self._vehicle.applyEngineForce(0.0, 0)
self._vehicle.applyEngineForce(0.0, 1)
self._vehicle.applyEngineForce(0.0, 2)
self._vehicle.applyEngineForce(0.0, 3)
if self._des_vel is not None:
self._des_vel = 0
self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
for i in range(self._vehicle.getNumWheels()):
wheel = self._vehicle.getWheel(i)
wheel.setRotation(0.0)
self._vehicle_node.clearForces()
def _place_vehicle(self, pos=None, hpr=None):
if pos is None:
pos = self._default_pos()
if hpr is None:
hpr = self._default_hpr()
self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
self._stop_car()
def _addWheel(self, pos, front, radius=0.25):
wheel = self._vehicle.createWheel()
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e2)
wheel.setRollInfluence(0.1)
# Task
def _update_task(self, task):
dt = globalClock.getDt()
self._update(dt=dt)
self._get_observation()
return task.cont
# Helper functions
def _get_observation(self):
self._obs = self._camera_sensor.observe()
observation = []
observation.append(self._obs[0])
if self._use_back_cam:
self._back_obs = self._back_camera_sensor.observe()
observation.append(self._back_obs[0])
observation = np.concatenate(observation, axis=2)
return observation
def _get_reward(self):
reward = self._collision_reward if self._collision else self._get_speed(
)
return reward
def _get_done(self):
return self._collision
def _get_info(self):
info = {}
info['pos'] = np.array(self._vehicle_pointer.getPos())
info['hpr'] = np.array(self._vehicle_pointer.getHpr())
info['vel'] = self._get_speed()
info['coll'] = self._collision
return info
def _back_up(self):
assert (self._use_vel)
back_up_vel = self._params['back_up'].get('vel', -2.0)
self._des_vel = back_up_vel
back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
# TODO
self._steering = np.random.uniform(*back_up_steer)
self._brakeForce = 0.
duration = self._params['back_up'].get('duration', 1.0)
self._update(dt=duration)
self._des_vel = 0.0
self._steering = 0.0
self._update(dt=duration)
self._brakeForce = 0.
def _is_contact(self):
result = self._world.contactTest(self._vehicle_node)
num_contacts = result.getNumContacts()
return result.getNumContacts() > 0
# Environment functions
def reset(self, pos=None, hpr=None, hard_reset=False, random_reset=False):
if self._do_back_up and not hard_reset and \
pos is None and hpr is None:
if self._collision:
self._back_up()
else:
if pos is None and hpr is None:
if random_reset:
pos, hpr = self._next_random_restart_pos_hpr()
else:
pos, hpr = self._next_restart_pos_hpr()
self._place_vehicle(pos=pos, hpr=hpr)
self._collision = False
return self._get_observation()
def step(self, action):
self._steering = action[0]
if action[1] == 0.0:
self._brakeForce = 1000.
else:
self._brakeForce = 0.
if self._use_vel:
# Convert from m/s to km/h
self._des_vel = action[1]
else:
self._engineForce = self._engineClamp * \
((action[1] - 49.5) / 49.5)
self._update(dt=self._dt)
observation = self._get_observation()
reward = self._get_reward()
done = self._get_done()
info = self._get_info()
return observation, reward, done, info
class CarEnv(DirectObject):
def __init__(self, params={}):
self._params = params
if 'random_seed' in self._params:
np.random.seed(self._params['random_seed'])
self._use_vel = self._params.get('use_vel', True)
self._run_as_task = self._params.get('run_as_task', False)
self._do_back_up = self._params.get('do_back_up', False)
self._use_depth = self._params.get('use_depth', False)
self._use_back_cam = self._params.get('use_back_cam', False)
self._collision_reward_only = self._params.get('collision_reward_only',
False)
self._collision_reward = self._params.get('collision_reward', -10.0)
self._obs_shape = self._params.get('obs_shape', (64, 36))
self._steer_limits = params.get('steer_limits', (-30., 30.))
self._speed_limits = params.get('speed_limits', (-4.0, 4.0))
self._motor_limits = params.get('motor_limits', (-0.5, 0.5))
self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1]
and self._use_vel)
if not self._params.get('visualize', False):
loadPrcFileData('', 'window-type offscreen')
# Defines base, render, loader
try:
ShowBase()
except:
pass
base.setBackgroundColor(0.0, 0.0, 0.0, 1)
# World
self._worldNP = render.attachNewNode('World')
self._world = BulletWorld()
self._world.setGravity(Vec3(0, 0, -9.81))
self._dt = params.get('dt', 0.25)
self._step = 0.05
# Vehicle
shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
ts = TransformState.makePos(Point3(0., 0., 0.25))
self._vehicle_node = BulletRigidBodyNode('Vehicle')
self._vehicle_node.addShape(shape, ts)
self._mass = self._params.get('mass', 10.)
self._vehicle_node.setMass(self._mass)
self._vehicle_node.setDeactivationEnabled(False)
self._vehicle_node.setCcdSweptSphereRadius(1.0)
self._vehicle_node.setCcdMotionThreshold(1e-7)
self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)
self._world.attachRigidBody(self._vehicle_node)
self._vehicle = BulletVehicle(self._world, self._vehicle_node)
self._vehicle.setCoordinateSystem(ZUp)
self._world.attachVehicle(self._vehicle)
self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)
# Camera
size = self._params.get('size', [160, 90])
hfov = self._params.get('hfov', 120)
near_far = self._params.get('near_far', [0.1, 100.])
self._camera_sensor = Panda3dCameraSensor(base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='front cam')
self._camera_node = self._camera_sensor.cam
self._camera_node.setPos(0.0, 0.5, 0.375)
self._camera_node.lookAt(0.0, 6.0, 0.0)
self._camera_node.reparentTo(self._vehicle_pointer)
if self._use_back_cam:
self._back_camera_sensor = Panda3dCameraSensor(
base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='back cam')
self._back_camera_node = self._back_camera_sensor.cam
self._back_camera_node.setPos(0.0, -0.5, 0.375)
self._back_camera_node.lookAt(0.0, -6.0, 0.0)
self._back_camera_node.reparentTo(self._vehicle_pointer)
# Car Simulator
self._des_vel = None
self._setup()
# Input
self.accept('escape', self._doExit)
self.accept('r', self.reset)
self.accept('f1', self._toggleWireframe)
self.accept('f2', self._toggleTexture)
self.accept('f3', self._view_image)
self.accept('f5', self._doScreenshot)
self.accept('q', self._forward_0)
self.accept('w', self._forward_1)
self.accept('e', self._forward_2)
self.accept('a', self._left)
self.accept('s', self._stop)
self.accept('x', self._backward)
self.accept('d', self._right)
self.accept('m', self._mark)
self._steering = 0.0 # degree
self._engineForce = 0.0
self._brakeForce = 0.0
self._env_time_step = 0
self._p = self._params.get('p', 1.25)
self._d = self._params.get('d', 0.0)
self._last_err = 0.0
self._curr_time = 0.0
self._accelClamp = self._params.get('accelClamp', 0.5)
self._engineClamp = self._accelClamp * self._mass
self._collision = False
self._setup_spec()
self.spec = EnvSpec(observation_im_space=self.observation_im_space,
action_space=self.action_space,
action_selection_space=self.action_selection_space,
observation_vec_spec=self.observation_vec_spec,
action_spec=self.action_spec,
action_selection_spec=self.action_selection_spec,
goal_spec=self.goal_spec)
if self._run_as_task:
self._mark_d = 0.0
taskMgr.add(self._update_task, 'updateWorld')
base.run()
def _setup_spec(self):
self.action_spec = OrderedDict()
self.action_selection_spec = OrderedDict()
self.observation_vec_spec = OrderedDict()
self.goal_spec = OrderedDict()
self.action_spec['steer'] = Box(low=-45., high=45.)
self.action_selection_spec['steer'] = Box(low=self._steer_limits[0],
high=self._steer_limits[1])
if self._use_vel:
self.action_spec['speed'] = Box(low=-4., high=4.)
self.action_space = Box(low=np.array([
self.action_spec['steer'].low[0],
self.action_spec['speed'].low[0]
]),
high=np.array([
self.action_spec['steer'].high[0],
self.action_spec['speed'].high[0]
]))
self.action_selection_spec['speed'] = Box(
low=self._speed_limits[0], high=self._speed_limits[1])
self.action_selection_space = Box(
low=np.array([
self.action_selection_spec['steer'].low[0],
self.action_selection_spec['speed'].low[0]
]),
high=np.array([
self.action_selection_spec['steer'].high[0],
self.action_selection_spec['speed'].high[0]
]))
else:
self.action_spec['motor'] = Box(low=-self._accelClamp,
high=self._accelClamp)
self.action_space = Box(low=np.array([
self.action_spec['steer'].low[0],
self.action_spec['motor'].low[0]
]),
high=np.array([
self.action_spec['steer'].high[0],
self.action_spec['motor'].high[0]
]))
self.action_selection_spec['motor'] = Box(
low=self._motor_limits[0], high=self._motor_limits[1])
self.action_selection_space = Box(
low=np.array([
self.action_selection_spec['steer'].low[0],
self.action_selection_spec['motor'].low[0]
]),
high=np.array([
self.action_selection_spec['steer'].high[0],
self.action_selection_spec['motor'].high[0]
]))
assert (np.logical_and(
self.action_selection_space.low >= self.action_space.low - 1e-4,
self.action_selection_space.high <=
self.action_space.high + 1e-4).all())
self.observation_im_space = Box(low=0,
high=255,
shape=tuple(
self._get_observation()[0].shape))
self.observation_vec_spec['coll'] = Discrete(1)
self.observation_vec_spec['heading'] = Box(low=0, high=2 * 3.14)
self.observation_vec_spec['speed'] = Box(low=-4.0, high=4.0)
@property
def _base_dir(self):
return os.path.join(os.path.dirname(os.path.abspath(__file__)),
'models')
@property
def horizon(self):
return np.inf
@property
def max_reward(self):
return np.inf
# _____HANDLER_____
def _doExit(self):
sys.exit(1)
def _toggleWireframe(self):
base.toggleWireframe()
def _toggleTexture(self):
base.toggleTexture()
def _doScreenshot(self):
base.screenshot('Bullet')
def _forward_0(self):
self._des_vel = 1
self._brakeForce = 0.0
def _forward_1(self):
self._des_vel = 2
self._brakeForce = 0.0
def _forward_2(self):
self._des_vel = 4
self._brakeForce = 0.0
def _stop(self):
self._des_vel = 0.0
self._brakeForce = 0.0
def _backward(self):
self._des_vel = -4
self._brakeForce = 0.0
def _right(self):
self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])
def _left(self):
self._steering = np.max([np.min([30, self._steering + 5]), 0.0])
def _view_image(self):
from matplotlib import pyplot as plt
image = self._camera_sensor.observe()[0]
if self._use_depth:
plt.imshow(image[:, :, 0], cmap='gray')
else:
def rgb2gray(rgb):
return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])
image = rgb2gray(image)
im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
) # TODO how does this deal with aspect ratio
plt.imshow(im.astype(np.uint8), cmap='Greys_r')
plt.show()
def _mark(self):
self._mark_d = 0.0
# Setup
def _setup(self):
self._setup_map()
self._place_vehicle()
self._setup_light()
self._setup_restart_pos()
def _setup_map(self):
if hasattr(self, '_model_path'):
# Collidable objects
self._setup_collision_object(self._model_path)
else:
ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
ground.node().addShape(shape)
ground.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(ground.node())
def _setup_collision_object(self,
path,
pos=(0.0, 0.0, 0.0),
hpr=(0.0, 0.0, 0.0),
scale=1):
visNP = loader.loadModel(path)
visNP.clearModelNodes()
visNP.reparentTo(render)
visNP.setPos(pos[0], pos[1], pos[2])
visNP.setHpr(hpr[0], hpr[1], hpr[2])
visNP.set_scale(scale, scale, scale)
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
bodyNP.setPos(pos[0], pos[1], pos[2])
bodyNP.setHpr(hpr[0], hpr[1], hpr[2])
bodyNP.set_scale(scale, scale, scale)
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
bodyNP.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(bodyNP.node())
else:
print("Issue")
def _setup_restart_pos(self):
self._restart_index = 0
self._restart_pos = self._default_restart_pos()
def _next_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
pos_hpr = self._restart_pos[self._restart_index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _setup_light(self):
# alight = AmbientLight('ambientLight')
# alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
# alightNP = render.attachNewNode(alight)
# render.clearLight()
# render.setLight(alightNP)
pass
# Vehicle
def _default_pos(self):
return (0.0, 0.0, 0.3)
def _default_hpr(self):
return (0.0, 0.0, 0.0)
def _default_restart_pos(self):
return [self._default_pos() + self._default_hpr()]
def _get_speed(self):
vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
return vel
def _get_heading(self):
h = np.array(self._vehicle_pointer.getHpr())[0]
ori = h * (pi / 180.)
while (ori > 2 * pi):
ori -= 2 * pi
while (ori < 0):
ori += 2 * pi
return ori
def _update(self, dt=1.0, coll_check=True):
self._vehicle.setSteeringValue(self._steering, 0)
self._vehicle.setSteeringValue(self._steering, 1)
self._vehicle.setBrake(self._brakeForce, 0)
self._vehicle.setBrake(self._brakeForce, 1)
self._vehicle.setBrake(self._brakeForce, 2)
self._vehicle.setBrake(self._brakeForce, 3)
if dt >= self._step:
# TODO maybe change number of timesteps
# for i in range(int(dt/self._step)):
if self._des_vel is not None:
vel = self._get_speed()
err = self._des_vel - vel
d_err = (err - self._last_err) / self._step
self._last_err = err
self._engineForce = np.clip(self._p * err + self._d * d_err,
-self._accelClamp,
self._accelClamp) * self._mass
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
for _ in range(int(dt / self._step)):
self._world.doPhysics(self._step, 1, self._step)
self._collision = self._is_contact()
elif self._run_as_task:
self._curr_time += dt
if self._curr_time > 0.05:
if self._des_vel is not None:
vel = self._get_speed()
self._mark_d += vel * self._curr_time
print(vel, self._mark_d, self._is_contact())
err = self._des_vel - vel
d_err = (err - self._last_err) / 0.05
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._curr_time = 0.0
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(dt, 1, dt)
self._collision = self._is_contact()
else:
raise ValueError(
"dt {0} s is too small for velocity control".format(dt))
def _stop_car(self):
self._steering = 0.0
self._engineForce = 0.0
self._vehicle.setSteeringValue(0.0, 0)
self._vehicle.setSteeringValue(0.0, 1)
self._vehicle.applyEngineForce(0.0, 0)
self._vehicle.applyEngineForce(0.0, 1)
self._vehicle.applyEngineForce(0.0, 2)
self._vehicle.applyEngineForce(0.0, 3)
if self._des_vel is not None:
self._des_vel = 0
self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
for i in range(self._vehicle.getNumWheels()):
wheel = self._vehicle.getWheel(i)
wheel.setRotation(0.0)
self._vehicle_node.clearForces()
def _place_vehicle(self, pos=None, hpr=None):
if pos is None:
pos = self._default_pos()
if hpr is None:
hpr = self._default_hpr()
self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
self._stop_car()
def _addWheel(self, pos, front, radius=0.25):
wheel = self._vehicle.createWheel()
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e2)
wheel.setRollInfluence(0.1)
# Task
def _update_task(self, task):
dt = globalClock.getDt()
self._update(dt=dt)
self._get_observation()
return task.cont
# Helper functions
def _get_observation(self):
self._obs = self._camera_sensor.observe()
observation = []
observation.append(self.process(self._obs[0], self._obs_shape))
if self._use_back_cam:
self._back_obs = self._back_camera_sensor.observe()
observation.append(self.process(self._back_obs[0],
self._obs_shape))
observation_im = np.concatenate(observation, axis=2)
coll = self._collision
heading = self._get_heading()
speed = self._get_speed()
observation_vec = np.array([coll, heading, speed])
return observation_im, observation_vec
def _get_goal(self):
return np.array([])
def process(self, image, obs_shape):
if self._use_depth:
im = np.reshape(image, (image.shape[0], image.shape[1]))
if im.shape != obs_shape:
im = cv2.resize(im, obs_shape, interpolation=cv2.INTER_AREA)
return im.astype(np.uint8)
else:
image = np.dot(image[..., :3], [0.299, 0.587, 0.114])
im = cv2.resize(image, obs_shape, interpolation=cv2.INTER_AREA
) #TODO how does this deal with aspect ratio
# TODO might not be necessary
im = np.expand_dims(im, 2)
return im.astype(np.uint8)
def _get_reward(self):
if self._collision_reward_only:
if self._collision:
reward = self._collision_reward
else:
reward = 0.0
else:
if self._collision:
reward = self._collision_reward
else:
reward = self._get_speed()
assert (reward <= self.max_reward)
return reward
def _get_done(self):
return self._collision
def _get_info(self):
info = {}
info['pos'] = np.array(self._vehicle_pointer.getPos())
info['hpr'] = np.array(self._vehicle_pointer.getHpr())
info['vel'] = self._get_speed()
info['coll'] = self._collision
info['env_time_step'] = self._env_time_step
return info
def _back_up(self):
assert (self._use_vel)
# logger.debug('Backing up!')
self._params['back_up'] = self._params.get('back_up', {})
back_up_vel = self._params['back_up'].get('vel', -1.0)
self._des_vel = back_up_vel
back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
self._steering = np.random.uniform(*back_up_steer)
self._brakeForce = 0.
duration = self._params['back_up'].get('duration', 3.0)
self._update(dt=duration)
self._des_vel = 0.
self._steering = 0.
self._update(dt=duration)
self._brakeForce = 0.
def _is_contact(self):
result = self._world.contactTest(self._vehicle_node)
return result.getNumContacts() > 0
# Environment functions
def reset(self, pos=None, hpr=None, hard_reset=False):
if self._do_back_up and not hard_reset and \
pos is None and hpr is None:
if self._collision:
self._back_up()
else:
if hard_reset:
logger.debug('Hard resetting!')
if pos is None and hpr is None:
pos, hpr = self._next_restart_pos_hpr()
self._place_vehicle(pos=pos, hpr=hpr)
self._collision = False
self._env_time_step = 0
return self._get_observation(), self._get_goal()
def step(self, action):
self._steering = action[0]
if action[1] == 0.0:
self._brakeForce = 1000.
else:
self._brakeForce = 0.
if self._use_vel:
# Convert from m/s to km/h
self._des_vel = action[1]
else:
self._engineForce = self._mass * action[1]
self._update(dt=self._dt)
observation = self._get_observation()
goal = self._get_goal()
reward = self._get_reward()
done = self._get_done()
info = self._get_info()
self._env_time_step += 1
return observation, goal, reward, done, info
class Freegrip(fgcp.FreegripContactpairs):
def __init__(self, objpath, handpkg, readser=False, faceangle = .9, segangle = .9, refine1min=1, refine1max=50,
refine2radius=10, fpairparallel=-0.7, hmax=5, objmass = 20.0, bypasssoftfgr = True):
"""
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, faceangle=faceangle, segangle=segangle)
if readser is False:
tic = time.time()
self.removeBadSamples(mindist=refine1min, maxdist=refine1max)
toc = time.time()
print("remove bad sample cost", toc-tic)
tic = time.time()
self.clusterFacetSamplesRNN(reduceRadius=refine2radius)
toc = time.time()
print("cluster samples cost", toc-tic)
tic = time.time()
self.planContactpairs(hmax, fpairparallel, objmass, bypasssoftfgr = bypasssoftfgr)
toc = time.time()
print("plan contact pairs cost", toc-tic)
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
# fliphand
self.flipgripcontactpairs_precc = None
self.flipgripcontactpairnormals_precc = None
self.flipgripcontactpairfacets_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_fn(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
print("number of vertices", len(self.objtrimesh.vertices))
print("number of faces", len(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 planGrasps(self, base, discretesize=8):
"""
plan the grasps without saving
this function calls remove Fgrpcc and remove Hndcc
:param discretesize: number of discretized rotation around a contact pair
:return:
"""
self.removeFgrpcc(base)
self.removeHndcc(base, discretesize)
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
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 = 1
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
# print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
# cc0 first
for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
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]]
# tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = self.hand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
if fgrdist > self.hand.jawwidthopen:
continue
# tmphand.setJawwidth(fgrdist)
# tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
# rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
# rotmat = rm.rodrigues(rotax, rotangle)
# tmphand.setMat(pandageom.cvtMat4(rotmat) * tmphand.getMat())
# axx = tmphand.getMat().getRow3(0)
# # 130 is the distance from hndbase to fingertip
# cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
# tmphand.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
fc = (cctpnt0 + cctpnt1)/2.0
self.hand.gripAt(fc[0], fc[1], fc[2], cctnormal0[0], cctnormal0[1], cctnormal0[2], rotangle, jawwidth = fgrdist)
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(self.hand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(self.hand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j])
# reset initial hand pose
self.hand.setMat(initmat)
# cc1 first
for j, contactpair in enumerate(self.flipgripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
cctpnt1 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt0 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal1 = self.flipgripcontactpairnormals_precc[self.counter][j][0]
cctnormal0 = [-cctnormal1[0], -cctnormal1[1], -cctnormal1[2]]
# tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = self.hand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
if fgrdist > self.hand.jawwidthopen:
continue
# tmphand.setJawwidth(fgrdist)
# tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
# rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
# rotmat = rm.rodrigues(rotax, rotangle)
# tmphand.setMat(pandageom.cvtMat4(rotmat) * tmphand.getMat())
# axx = tmphand.getMat().getRow3(0)
# # 130 is the distance from hndbase to fingertip
# cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
# tmphand.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
fc = (cctpnt0 + cctpnt1)/2.0
self.hand.gripAt(fc[0], fc[1], fc[2], cctnormal0[0], cctnormal0[1], cctnormal0[2], rotangle, jawwidth = fgrdist)
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(self.hand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(self.hand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.flipgripcontactpairnormals_precc[self.counter][j])
# reset initial hand pose
self.hand.setMat(initmat)
self.counter+=1
self.counter = 0
def removeFgrpcc(self, base):
"""
Fgrpcc means finger pre collision detection
:return:
author: weiwei
date: 20161212, tsukuba
"""
# cct0 first
self.gripcontactpairs_precc = []
self.gripcontactpairnormals_precc = []
self.gripcontactpairfacets_precc = []
# cct1 first
self.flipgripcontactpairs_precc = []
self.flipgripcontactpairnormals_precc = []
self.flipgripcontactpairfacets_precc = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
# print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs
# cct0 first
self.gripcontactpairs_precc.append([])
self.gripcontactpairnormals_precc.append([])
self.gripcontactpairfacets_precc.append([])
# cct1 first
self.flipgripcontactpairs_precc.append([])
self.flipgripcontactpairnormals_precc.append([])
self.flipgripcontactpairfacets_precc.append([])
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
# cctpnt0 first
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
handfgrpcc0 = NodePath("handfgrpcc0")
# self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0)
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(handfgrpcc1)
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
self.gripcontactpairs_precc[-1].append(contactpair)
self.gripcontactpairnormals_precc[-1].append(self.gripcontactpairnormals[self.counter][j])
self.gripcontactpairfacets_precc[-1].append(self.gripcontactpairfacets[self.counter])
# cctpnt1 first
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt1 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt0 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal1 = self.facetnormals[facetidx0]
cctnormal0 = [-cctnormal1[0], -cctnormal1[1], -cctnormal1[2]]
handfgrpcc0 = NodePath("handfgrpcc0")
# self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0)
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(handfgrpcc1)
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
self.flipgripcontactpairs_precc[-1].append(contactpair)
self.flipgripcontactpairnormals_precc[-1].append(self.gripcontactpairnormals[self.counter][j])
self.flipgripcontactpairfacets_precc[-1].append(self.gripcontactpairfacets[self.counter])
self.counter += 1
self.counter=0
def saveToDB(self, gdb):
"""
save the result to mysqldatabase
:param gdb: is an object of the GraspDB class in the database package
:return:
author: weiwei
date: 20170110
"""
idhand = gdb.loadIdHand(self.handname)
idobject = gdb.loadIdObject(self.dbobjname)
sql = "SELECT * FROM freeairgrip, object WHERE freeairgrip.idobject LIKE '%s' AND \
freeairgrip.idhand LIKE '%s'" % (idobject, idhand)
result = gdb.execute(sql)
if len(result) > 0:
print("Grasps already saved or duplicated filename!")
isredo = input("Do you want to overwrite the database? (Y/N)")
if isredo != "Y" and isredo != "y":
print("Grasp planning aborted.")
else:
sql = "DELETE FROM freeairgrip WHERE freeairgrip.idobject LIKE '%s' AND \
freeairgrip.idhand LIKE '%s'" % (idobject, idhand)
gdb.execute(sql)
print(self.gripcontacts)
for i in range(len(self.gripcontacts)):
sql = "INSERT INTO freeairgrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d)" % \
(idobject, dc.v3ToStr(self.gripcontacts[i][0]), dc.v3ToStr(self.gripcontacts[i][1]),
dc.v3ToStr(self.gripcontactnormals[i][0]), dc.v3ToStr(self.gripcontactnormals[i][1]),
dc.mat4ToStr(self.griprotmats[i]), str(self.gripjawwidth[i]), idhand)
gdb.execute(sql)
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 = 3
npbrchild = base.render.find("**/tempplot")
if npbrchild:
npbrchild.removeNode()
# for fast delete
brchild = NodePath('tempplot')
brchild.reparentTo(base.render)
self.counter += 1
print(self.facetpairs.shape)
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
geomfacet0 = pandageom.packpandageom_fn(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_fn(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(handfgrpcc0)
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(handfgrpcc1)
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()
base.pggen.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1))
base.pggen.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1))
if result.getNumContacts():
handfgrpcc0.setColor(.5, 0, 0, 1)
handfgrpcc1.setColor(.5, 0, 0, 1)
else:
handfgrpcc0.setColor(1, 1, 1, 1)
handfgrpcc1.setColor(1, 1, 1, 1)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(brchild)
handfgrpcc1.reparentTo(brchild)
base.pggen.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)
base.pggen.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 removeFgrpccShowLeft(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
Plot the available grips
:return:
author: weiwei
date: 20161212, tsukuba
"""
plotoffsetfp = 6
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
return
else:
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[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0)
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(handfgrpcc1)
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(base.render)
handfgrpcc1.reparentTo(base.render)
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))
if fgrdist > self.hand.jawwidthopen:
continue
# tmprtq85.setJawwidth(fgrdist)
# since fgrpcc already detects inner collisions
rotangle = 360.0 / discretesize * angleid
fc = (cctpnt0 + cctpnt1) / 2.0
tmprtq85.gripAt(fc[0], fc[1], fc[2], cctnormal0[0], cctnormal0[1], cctnormal0[2], rotangle, 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([.3, .3, .3, 1])
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, 1])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
def plotObj(self):
geomnodeobj = GeomNode('obj')
geomnodeobj.addGeom(self.objgeom)
npnodeobj = NodePath('obj')
npnodeobj.attachNewNode(geomnodeobj)
npnodeobj.reparentTo(base.render)
def showAllGrips(self):
"""
showAllGrips
:return:
author: weiwei
date: 20170206
"""
print("num of grasps", len(self.gripcontacts))
class RegripTpp():
def __init__(self, objpath, robot, handpkg, gdb, offset = -55):
self.objtrimesh=trimesh.load_mesh(objpath)
self.handpkg = handpkg
self.handname = handpkg.getHandName()
# for dbaccess
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
# regg = regrip graph
self.regg = nx.Graph()
self.ndiscreterot = 0
self.nplacements = 0
self.globalgripids = []
# for removing the grasps at start and goal
self.robothand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
# plane to remove hand
self.bulletworld = BulletWorld()
self.planebullnode = cd.genCollisionPlane(offset = offset)
self.bulletworld.attachRigidBody(self.planebullnode)
# add tabletop plane model to bulletworld
# dont forget offset
# 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"))
# self.ttnodepath = NodePath("tabletop")
# ttl = loader.loadModel(ttpath)
# ttl.instanceTo(self.ttnodepath)
self.startnodeids = None
self.goalnodeids = None
self.shortestpaths = None
self.gdb = gdb
self.robot = robot
# load retraction distances
self.rethandx, self.retworldz, self.retworlda, self.worldz = self.gdb.loadIKRet()
# worlda is default for the general grasps on table top
# for assembly at start and goal, worlda is computed by assembly planner
self.worlda = Vec3(0,0,1)
# loadfreeairgrip
self.__loadFreeAirGrip()
self.__loadGripsToBuildGraph()
def __loadFreeAirGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: weiwei
date: 20170110
"""
freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname, self.handname)
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripid = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def __loadGripsToBuildGraph(self, armname = "rgt"):
"""
load tabletopgrips
retraction distance are also loaded from database
:param robot: an robot defined in robotsim.hrp5 or robotsim.nextage
:param gdb: an object of the database.GraspDB class
:param idarm: value = 1 "lft" or 2 "rgt", which arm to use
:return:
author: weiwei
date: 20170112
"""
# load idarm
idarm = self.gdb.loadIdArm(armname)
idhand = self.gdb.loadIdHand(self.handname)
# get the global grip ids
# and prepare the global edges
# for each globalgripid, find all its tabletopids (pertaining to placements)
globalidsedges = {}
sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = self.gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan freeairgrip first!")
for ggid in result:
globalidsedges[str(ggid[0])] = []
self.globalgripids.append(ggid[0])
sql = "SELECT tabletopplacements.idtabletopplacements, angle.value, \
tabletopplacements.idfreetabletopplacement, tabletopplacements.tabletopposition, \
tabletopplacements.rotmat FROM \
tabletopplacements,freetabletopplacement,angle,object WHERE \
tabletopplacements.idangle=angle.idangle AND \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND \
object.name LIKE '%s' AND angle.value IN (0.0, 45.0, 90.0, 135.0, 180.0, 225.0, 270.0, 315.0)" \
% self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
tpsrows = np.array(result)
# nubmer of discreted rotation
self.angles = list(set(map(float, tpsrows[:,1])))
# for plotting
self.fttpsids = list(set(map(int, tpsrows[:,2])))
self.nfttps = len(self.fttpsids)
idrobot = self.gdb.loadIdRobot(self.robot)
for i, idtps in enumerate(tpsrows[:,0]):
sql = "SELECT tabletopgrips.idtabletopgrips, tabletopgrips.contactpnt0, tabletopgrips.contactpnt1, \
tabletopgrips.rotmat, tabletopgrips.jawwidth, tabletopgrips.idfreeairgrip \
FROM tabletopgrips,freeairgrip,ik WHERE tabletopgrips.idtabletopgrips=ik.idtabletopgrips AND \
tabletopgrips.idtabletopplacements = %d AND ik.idrobot=%d AND \
ik.feasibility='True' AND ik.feasibility_handx='True' AND ik.feasibility_handxworldz='True' \
AND ik.feasibility_worlda='True' AND ik.feasibility_worldaworldz='True' AND ik.idarm = %d \
AND tabletopgrips.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = %d" % (int(idtps), idrobot, idarm, idhand)
resultttgs = self.gdb.execute(sql)
if len(resultttgs)==0:
continue
localidedges = []
for ttgsrow in resultttgs:
ttgsid = int(ttgsrow[0])
ttgscct0 = dc.strToV3(ttgsrow[1])
ttgscct1 = dc.strToV3(ttgsrow[2])
ttgsrotmat = dc.strToMat4(ttgsrow[3])
ttgsjawwidth = float(ttgsrow[4])
ttgsidfreeair = int(ttgsrow[5])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
objrotmat4 = dc.strToMat4(tpsrows[:,4][i])
objrotmat4worlda = Mat4(objrotmat4)
objrotmat4worlda.setRow(3, objrotmat4.getRow3(3)+self.worlda*self.retworlda)
objrotmat4worldaworldz = Mat4(objrotmat4worlda)
objrotmat4worldaworldz.setRow(3, objrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('mid' + str(ttgsid), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz,
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = int(tpsrows[:,2][i]),
tabletopplacementrotmat = objrotmat4,
tabletopplacementrotmathandx = objrotmat4,
tabletopplacementrotmathandxworldz = objrotmat4,
tabletopplacementrotmatworlda = objrotmat4worlda,
tabletopplacementrotmatworldaworldz = objrotmat4worldaworldz,
angle = float(tpsrows[:,1][i]), tabletopposition = dc.strToV3(tpsrows[:,3][i]))
globalidsedges[str(ttgsidfreeair)].append('mid' + str(ttgsid))
localidedges.append('mid' + str(ttgsid))
# print list(itertools.combinations(ttgrows[:,0], 2))
for edge in list(itertools.combinations(localidedges, 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transit')
if len(globalidsedges) == 0:
raise ValueError("Plan tabletopgrips first!")
for globalidedgesid in globalidsedges:
for edge in list(itertools.combinations(globalidsedges[globalidedgesid], 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transfer')
else:
assert('No placements planned!')
def __addstartgoal(self, startrotmat4, goalrotmat4, base):
"""
add start and goal for the regg
:param startrotmat4 and goalrotmat4: both are 4by4 panda3d matrix
:return:
author: weiwei
date: 20161216, sapporo
"""
### start
# the node id of a globalgripid in startnode
nodeidofglobalidinstart= {}
# the startnodeids is also for quick access
self.startnodeids = []
for j, rotmat in enumerate(self.freegriprotmats):
print j, len(self.freegriprotmats)
# print rotmat
ttgsrotmat = rotmat * startrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(startrotmat4)
ttgsrotmatx0y0.setCell(3,0,0)
ttgsrotmatx0y0.setCell(3,1,0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
# tmphnd = self.robothand
tmphnd = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# set jawwidth to 80 to avoid collision with surrounding obstacles
# set to gripping with is unnecessary
tmphnd.setJawwidth(80)
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
# tmphnd.setMat(ttgsrotmat)
# tmphnd.reparentTo(base.render)
# if j > 3:
# base.run()
if not result.getNumContacts():
ttgscct0=startrotmat4.xformPoint(self.freegripcontacts[j][0])
ttgscct1=startrotmat4.xformPoint(self.freegripcontacts[j][1])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
# handxworldz is not necessary for start
# ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsjawwidth = self.freegripjawwidth[j]
ttgsidfreeair = self.freegripid[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
# handxworldz is not necessary for start
# ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
print "solving starting iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = startrotmat4.getRow3(3)
startrotmat4worlda = Mat4(startrotmat4)
startrotmat4worlda.setRow(3, startrotmat4.getRow3(3)+self.worlda*self.retworlda)
startrotmat4worldaworldz = Mat4(startrotmat4worlda)
startrotmat4worldaworldz.setRow(3, startrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('start'+str(j), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = 'na',
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = 'na',
tabletopplacementrotmat = startrotmat4,
tabletopplacementrotmathandx = startrotmat4,
tabletopplacementrotmathandxworldz = 'na',
tabletopplacementrotmatworlda = startrotmat4worlda,
tabletopplacementrotmatworldaworldz = startrotmat4worldaworldz,
angle = 'na', tabletopposition = tabletopposition)
nodeidofglobalidinstart[ttgsidfreeair]='start'+str(j)
self.startnodeids.append('start'+str(j))
# tmprtq85.reparentTo(base.render)
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
if len(self.startnodeids) == 0:
raise ValueError("No available starting grip!")
# add start transit edge
for edge in list(itertools.combinations(self.startnodeids, 2)):
self.regg.add_edge(*edge, weight = 1, edgetype = 'starttransit')
# add start transfer edge
for reggnode, reggnodedata in self.regg.nodes(data=True):
if reggnode.startswith('mid'):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidinstart.keys():
startnodeid = nodeidofglobalidinstart[globalgripid]
self.regg.add_edge(startnodeid, reggnode, weight=1, edgetype = 'starttransfer')
### goal
# the node id of a globalgripid in goalnode, for quick setting up edges
nodeidofglobalidingoal= {}
# the goalnodeids is also for quick access
self.goalnodeids = []
for j, rotmat in enumerate(self.freegriprotmats):
print j, len(self.freegriprotmats)
ttgsrotmat = rotmat * goalrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(goalrotmat4)
ttgsrotmatx0y0.setCell(3,0,0)
ttgsrotmatx0y0.setCell(3,1,0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
tmphnd = self.robothand
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
tmphnd.setJawwidth(self.freegripjawwidth[j])
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
ttgscct0=goalrotmat4.xformPoint(self.freegripcontacts[j][0])
ttgscct1=goalrotmat4.xformPoint(self.freegripcontacts[j][1])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsjawwidth = self.freegripjawwidth[j]
ttgsidfreeair = self.freegripid[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
print "solving goal iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikcxz = self.robot.numikr(ttgsfgrcenternp_handxworldz, ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (ikcxz is not None) \
and (ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = goalrotmat4.getRow3(3)
goalrotmat4worlda = Mat4(goalrotmat4)
goalrotmat4worlda.setRow(3, goalrotmat4.getRow3(3)+self.worlda*self.retworlda)
goalrotmat4worldaworldz = Mat4(goalrotmat4worlda)
goalrotmat4worldaworldz.setRow(3, goalrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('goal'+str(j), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz,
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = 'na',
tabletopplacementrotmat = goalrotmat4,
tabletopplacementrotmathandx = goalrotmat4,
tabletopplacementrotmathandxworldz = goalrotmat4,
tabletopplacementrotmatworlda = goalrotmat4worlda,
tabletopplacementrotmatworldaworldz = goalrotmat4worldaworldz,
angleid = 'na', tabletopposition = tabletopposition)
nodeidofglobalidingoal[ttgsidfreeair]='goal'+str(j)
self.goalnodeids.append('goal'+str(j))
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
if len(self.goalnodeids) == 0:
raise ValueError("No available goal grip!")
# add goal transit edges
for edge in list(itertools.combinations(self.goalnodeids, 2)):
self.regg.add_edge(*edge, weight = 1, edgetype = 'goaltransit')
# add goal transfer edge
for reggnode, reggnodedata in self.regg.nodes(data=True):
if reggnode.startswith('mid'):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidingoal.keys():
goalnodeid = nodeidofglobalidingoal[globalgripid]
self.regg.add_edge(goalnodeid, reggnode, weight=1, edgetype = 'goaltransfer')
# add start to goal direct edges
for startnodeid in self.startnodeids:
for goalnodeid in self.goalnodeids:
# startnodeggid = start node global grip id
startnodeggid = self.regg.node[startnodeid]['globalgripid']
goalnodeggid = self.regg.node[goalnodeid]['globalgripid']
if startnodeggid == goalnodeggid:
self.regg.add_edge(startnodeid, goalnodeid, weight=1, edgetype = 'startgoaltransfer')
def findshortestpath(self, startrotmat4, goalrotmat4, base):
self.__addstartgoal(startrotmat4, goalrotmat4, base)
# startgrip = random.select(self.startnodeids)
# goalgrip = random.select(self.goalnodeids)
startgrip = self.startnodeids[0]
goalgrip = self.goalnodeids[0]
self.shortestpaths = nx.all_shortest_paths(self.regg, source = startgrip, target = goalgrip)
self.directshortestpaths = []
# directshortestpaths removed the repeated start and goal transit
try:
for path in self.shortestpaths:
print path
for i, pathnode in enumerate(path):
if pathnode.startswith('start') and i < len(path)-1:
continue
else:
self.directshortestpaths.append(path[i-1:])
break
for i, pathnode in enumerate(self.directshortestpaths[-1]):
if i > 0 and pathnode.startswith('goal'):
self.directshortestpaths[-1]=self.directshortestpaths[-1][:i+1]
break
except:
print "No path found!"
pass
def plotgraph(self, pltfig):
"""
plot the graph without start and goal
:param pltfig: the matplotlib object
:return:
author: weiwei
date: 20161217, sapporos
"""
# biggest circle: grips; big circle: rotation; small circle: placements
radiusplacement = 30
radiusrot = 6
radiusgrip = 1
xyplacementspos = {}
xydiscreterotspos = {}
self.xyzglobalgrippos = {}
for i, ttpsid in enumerate(self.fttpsids):
xydiscreterotspos[ttpsid]={}
self.xyzglobalgrippos[ttpsid]={}
xypos = [radiusplacement*math.cos(2*math.pi/self.nfttps*i),
radiusplacement*math.sin(2*math.pi/self.nfttps*i)]
xyplacementspos[ttpsid] = xypos
for j, anglevalue in enumerate(self.angles):
self.xyzglobalgrippos[ttpsid][anglevalue]={}
xypos = [radiusrot*math.cos(math.radians(anglevalue)), radiusrot*math.sin(math.radians(anglevalue))]
xydiscreterotspos[ttpsid][anglevalue] = \
[xyplacementspos[ttpsid][0]+xypos[0], xyplacementspos[ttpsid][1]+xypos[1]]
for k, globalgripid in enumerate(self.globalgripids):
xypos = [radiusgrip*math.cos(2*math.pi/len(self.globalgripids)* k),
radiusgrip*math.sin(2*math.pi/len(self.globalgripids)*k)]
self.xyzglobalgrippos[ttpsid][anglevalue][globalgripid]=\
[xydiscreterotspos[ttpsid][anglevalue][0]+xypos[0],
xydiscreterotspos[ttpsid][anglevalue][1]+xypos[1], 0]
# for start and goal grasps poses:
self.xyzlobalgrippos={}
for k, globalgripid in enumerate(self.globalgripids):
xypos = [radiusgrip * math.cos(2 * math.pi / len(self.globalgripids) * k),
radiusgrip * math.sin(2 * math.pi / len(self.globalgripids) * k)]
self.xyzlobalgrippos[globalgripid] = [xypos[0],xypos[1],0]
transitedges = []
transferedges = []
starttransferedges = []
goaltransferedges = []
starttransitedges = []
goaltransitedges = []
for nid0, nid1, reggedgedata in self.regg.edges(data=True):
if (reggedgedata['edgetype'] is 'transit') or (reggedgedata['edgetype'] is 'transfer'):
fttpid0 = self.regg.node[nid0]['freetabletopplacementid']
anglevalue0 = self.regg.node[nid0]['angle']
ggid0 = self.regg.node[nid0]['globalgripid']
fttpid1 = self.regg.node[nid1]['freetabletopplacementid']
anglevalue1 = self.regg.node[nid1]['angle']
ggid1 = self.regg.node[nid1]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][ggid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][ggid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
# 3d
# if reggedgedata['edgetype'] is 'transit':
# transitedges.append([xyzpos0, xyzpos1])
# if reggedgedata['edgetype'] is 'transfer':
# transferedges.append([xyzpos0, xyzpos1])
#2d
if reggedgedata['edgetype'] is 'transit':
transitedges.append([xyzpos0[:2], xyzpos1[:2]])
if reggedgedata['edgetype'] is 'transfer':
transferedges.append([xyzpos0[:2], xyzpos1[:2]])
else:
if (reggedgedata['edgetype'] is 'starttransit') or (reggedgedata['edgetype'] is 'goaltransit'):
gid0 = self.regg.node[nid0]['globalgripid']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzlobalgrippos[gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzlobalgrippos[gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
if reggedgedata['edgetype'] is 'starttransit':
starttransitedges.append([xyzpos0[:2], xyzpos1[:2]])
if reggedgedata['edgetype'] is 'goaltransit':
goaltransitedges.append([xyzpos0[:2], xyzpos1[:2]])
else:
# start or goal transfer
if nid0.startswith('mid'):
fttpid0 = self.regg.node[nid0]['freetabletopplacementid']
anglevalue0 = self.regg.node[nid0]['angle']
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzlobalgrippos[gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
if nid1[:4] == 'goal':
goaltransferedges.append([xyzpos0[:2], xyzpos1[:2]])
else:
starttransferedges.append([xyzpos0[:2], xyzpos1[:2]])
if nid1.startswith('mid'):
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
fttpid1 = self.regg.node[nid1]['freetabletopplacementid']
anglevalue1 = self.regg.node[nid1]['angle']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzlobalgrippos[gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
if nid0[:4] == 'goal':
goaltransferedges.append([xyzpos0[:2], xyzpos1[:2]])
else:
starttransferedges.append([xyzpos0[:2], xyzpos1[:2]])
#3d
# transitec = mc3d.Line3DCollection(transitedges, colors=[0,1,1,1], linewidths=1)
# transferec = mc3d.Line3DCollection(transferedges, colors=[0,0,0,.1], linewidths=1)
#2d
transitec = mc.LineCollection(transitedges, colors=[0,1,1,1], linewidths=1)
transferec = mc.LineCollection(transferedges, colors=[0,0,0,.1], linewidths=1)
starttransferec = mc.LineCollection(starttransferedges, colors=[1,0,0,.3], linewidths=1)
goaltransferec = mc.LineCollection(goaltransferedges, colors=[0,0,1,.3], linewidths=1)
starttransitec = mc.LineCollection(starttransitedges, colors=[.5,0,0,.3], linewidths=1)
goaltransitec = mc.LineCollection(goaltransitedges, colors=[0,0,.5,.3], linewidths=1)
ax = pltfig.add_subplot(111)
ax.add_collection(transferec)
ax.add_collection(transitec)
ax.add_collection(starttransferec)
ax.add_collection(goaltransferec)
ax.add_collection(starttransitec)
ax.add_collection(goaltransitec)
# for reggnode, reggnodedata in self.regg.nodes(data=True):
# placementid = reggnodedata['placementid']
# angleid = reggnodedata['angleid']
# globalgripid = reggnodedata['globalgripid']
# tabletopposition = reggnodedata['tabletopposition']
# xyzpos = map(add, xyzglobalgrippos[placementid][angleid][str(globalgripid)],[tabletopposition[0], tabletopposition[1], tabletopposition[2]])
# plt.plot(xyzpos[0], xyzpos[1], 'ro')
def plotshortestpath(self, pltfig, id = 0):
"""
plot the shortest path
about transit and transfer:
The tabletoppositions of start and goal are the local zero of the mesh model
in contrast, the tabletoppositions of the other nodes in the graph are the local zero of the supporting facet
if tabletopposition start == tabletop position goal
there are two possibilities:
1) start and goal are the same, then it is transit
2) start and goal are different, then it is tranfer
Note that start and the second will never be the same since they are in different coordinate systems.
It is reasonable since the shortest path will never let the start go to the same position again.
if the second item is not the goal, the path between the first and second items is
sure to be a transfer path
:param id:
:return:
"""
for i,path in enumerate(self.directshortestpaths):
if i is id:
pathedgestransit = []
pathedgestransfer = []
pathlength = len(path)
for pnidx in range(pathlength-1):
nid0 = path[pnidx]
nid1 = path[pnidx+1]
if pnidx == 0 and pnidx+1 == pathlength-1:
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzlobalgrippos[gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzlobalgrippos[gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
if tabletopposition0 == tabletopposition1:
pathedgestransit.append([xyzpos0[:2], xyzpos1[:2]])
else:
pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]])
else:
if pnidx == 0:
# this is sure to be transfer
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
fttpid1 = self.regg.node[nid1]['freetabletopplacementid']
anglevalue1 = self.regg.node[nid1]['angle']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzlobalgrippos[gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][gid1], [
tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]])
if pnidx+1 == pathlength-1:
# also definitely transfer
fttpid0 = self.regg.node[nid0]['freetabletopplacementid']
anglevalue0 = self.regg.node[nid0]['angle']
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzlobalgrippos[gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]])
if pnidx > 0 and pnidx+1 < pathlength-1:
fttpid0 = self.regg.node[nid0]['freetabletopplacementid']
anglevalue0 = self.regg.node[nid0]['angle']
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
fttpid1 = self.regg.node[nid1]['freetabletopplacementid']
anglevalue1 = self.regg.node[nid1]['angle']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
if tabletopposition0 == tabletopposition1:
pathedgestransit.append([xyzpos0[:2], xyzpos1[:2]])
else:
pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]])
pathtransitec = mc.LineCollection(pathedgestransit, colors=[.5, 1, 0, 1], linewidths=5)
pathtransferec = mc.LineCollection(pathedgestransfer, colors=[0, 1, 0, 1], linewidths=5)
ax = pltfig.gca()
ax.add_collection(pathtransitec)
ax.add_collection(pathtransferec)
def plotgraphp3d(self, base):
"""
draw the graph in panda3d
:param base:
:return:
author: weiwei
date: 20161216, osaka itami airport
"""
# big circle: rotation; small circle: placements
radiusplacement = 30
radiusrot = 6
radiusgrip = 1
xyplacementspos = []
xydiscreterotspos = []
xyzglobalgrippos = []
for i in range(self.nplacements):
xydiscreterotspos.append([])
xyzglobalgrippos.append([])
xypos = [radiusplacement*math.cos(2*math.pi/self.nplacements*i), radiusplacement*math.sin(2*math.pi/self.nplacements*i)]
xyplacementspos.append(xypos)
for j in range(self.ndiscreterot):
xyzglobalgrippos[-1].append({})
xypos = [radiusrot*math.cos(2*math.pi/self.ndiscreterot* j), radiusrot*math.sin(2*math.pi/self.ndiscreterot * j)]
xydiscreterotspos[-1].append([xyplacementspos[-1][0]+xypos[0],xyplacementspos[-1][1]+xypos[1]])
for k, globalgripid in enumerate(self.globalgripids):
xypos = [radiusgrip*math.cos(2*math.pi/len(self.globalgripids)* k), radiusgrip*math.sin(2*math.pi/len(self.globalgripids)*k)]
xyzglobalgrippos[-1][-1][globalgripid]=[xydiscreterotspos[-1][-1][0]+xypos[0],xydiscreterotspos[-1][-1][1]+xypos[1], 0]
transitedges = []
transferedges = []
for nid0, nid1, reggedgedata in self.regg.edges(data=True):
fttpid0 = self.regg.node[nid0]['freetabletopplacementid']
anglevalue0 = self.regg.node[nid0]['angle']
gid0 = self.regg.node[nid0]['globalgripid']
fttpid1 = self.regg.node[nid1]['freetabletopplacementid']
angelvalue1 = self.regg.node[nid1]['angle']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, xyzglobalgrippos[fttpid0][anglevalue0][str(gid0)],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, xyzglobalgrippos[fttpid1][angelvalue1][str(gid1)],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
# 3d
if reggedgedata['edgetype'] is 'transit':
transitedges.append([xyzpos0, xyzpos1])
if reggedgedata['edgetype'] is 'transfer':
transferedges.append([xyzpos0, xyzpos1])
#3d
transitecnp = pg.makelsnodepath(transitedges, rgbacolor=[0,1,1,1])
transferecnp = pg.makelsnodepath(transferedges, rgbacolor=[0,0,0,.1])
transitecnp.reparentTo(base.render)
transferecnp.reparentTo(base.render)
class FreeTabletopPlacement(object):
"""
manipulation.freetabletopplacement doesn't take into account
the position and orientation of the object
it is "free" in position and rotation around z axis
in contrast, each item in regrasp.tabletopplacements
has different position and orientation
it is at a specific pose in the workspace
To clearly indicate the difference, "free" is attached
to the front of "freetabletopplacement"
"s" is attached to the end of "tabletopplacements"
"""
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()
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 loadFreeAirGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: weiwei
date: 20170110
"""
freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname, handname = self.handname)
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripid = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def loadFreeTabletopPlacement(self):
"""
load free tabletopplacements
:return:
"""
tpsmat4s = self.gdb.loadFreeTabletopPlacement(self.dbobjname)
if tpsmat4s is not None:
self.tpsmat4s = tpsmat4s
return True
else:
self.tpsmat4s = []
return False
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 gentpsgrip(self, base):
"""
Originally the code of this function is embedded in the removebadfacet function
It is separated on 20170608 to enable common usage of placements for different hands
:return:
author: weiwei
date: 20170608
"""
self.tpsgripcontacts = []
self.tpsgripnormals = []
self.tpsgriprotmats = []
self.tpsgripjawwidth = []
# the id of the grip in freeair
self.tpsgripidfreeair = []
for i in range(len(self.tpsmat4s)):
self.tpsgripcontacts.append([])
self.tpsgripnormals.append([])
self.tpsgriprotmats.append([])
self.tpsgripjawwidth.append([])
self.tpsgripidfreeair.append([])
for j, rotmat in enumerate(self.freegriprotmats):
tpsgriprotmat = rotmat * self.tpsmat4s[i]
# check if the hand collide with tabletop
# tmprtq85 = self.rtq85hnd
tmphnd = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, 1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# open the hand to ensure it doesnt collide with surrounding obstacles
# tmprtq85.setJawwidth(self.freegripjawwidth[j])
tmphnd.setJawwidth(80)
tmphnd.setMat(pandanpmat4 = tpsgriprotmat)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworldhp.contactTest(hndbullnode)
# print result.getNumContacts()
if not result.getNumContacts():
self.tpsgriprotmats[-1].append(tpsgriprotmat)
cct0 = self.tpsmat4s[i].xformPoint(self.freegripcontacts[j][0])
cct1 = self.tpsmat4s[i].xformPoint(self.freegripcontacts[j][1])
self.tpsgripcontacts[-1].append([cct0, cct1])
cctn0 = self.tpsmat4s[i].xformVec(self.freegripnormals[j][0])
cctn1 = self.tpsmat4s[i].xformVec(self.freegripnormals[j][1])
self.tpsgripnormals[-1].append([cctn0, cctn1])
self.tpsgripjawwidth[-1].append(self.freegripjawwidth[j])
self.tpsgripidfreeair[-1].append(self.freegripid[j])
tmphnd.setMat(pandanpmat4 = initmat)
tmphnd.setJawwidth(initjawwidth)
def saveToDB(self):
"""
save freetabletopplacement
manipulation.freetabletopplacement doesn't take into account the position and orientation of the object
it is "free" in position and rotation around z axis
in contrast, each item in regrasp.tabletopplacements has different position and orientation
it is at a specific pose in the workspace
To clearly indicate the difference, "free" is attached to the front of "freetabletopplacement"
"s" is attached to the end of "tabletopplacements"
:param discretesize:
:param gdb:
:return:
author: weiwei
date: 20170111
"""
# save freetabletopplacement
sql = "SELECT * FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
# the fretabletopplacements for the self.dbobjname is not saved
sql = "INSERT INTO freetabletopplacement(rotmat, idobject) VALUES "
for i in range(len(self.tpsmat4s)):
sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \
(dc.mat4ToStr(self.tpsmat4s[i]), self.dbobjname)
sql = sql[:-2] + ";"
self.gdb.execute(sql)
else:
print "Freetabletopplacement already exist!"
# save freetabletopgrip
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT * FROM freetabletopgrip,freetabletopplacement,freeairgrip,object WHERE \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = self.gdb.execute(sql)
if len(result) == 0:
for i in range(len(self.tpsmat4s)):
sql = "SELECT freetabletopplacement.idfreetabletopplacement FROM freetabletopplacement,object WHERE \
freetabletopplacement.rotmat LIKE '%s' AND \
object.name LIKE '%s'" % (dc.mat4ToStr(self.tpsmat4s[i]), self.dbobjname)
result = self.gdb.execute(sql)[0]
print result
if len(result) != 0:
idfreetabletopplacement = result[0]
# note self.tpsgriprotmats[i] might be empty (no cd-free grasps)
if len(self.tpsgriprotmats[i]) != 0:
sql = "INSERT INTO freetabletopgrip(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreetabletopplacement, idfreeairgrip) VALUES "
for j in range(len(self.tpsgriprotmats[i])):
cct0 = self.tpsgripcontacts[i][j][0]
cct1 = self.tpsgripcontacts[i][j][1]
cctn0 = self.tpsgripnormals[i][j][0]
cctn1 = self.tpsgripnormals[i][j][1]
sql += "('%s', '%s', '%s', '%s', '%s', '%s', %d, %d), " % \
(dc.v3ToStr(cct0), dc.v3ToStr(cct1), dc.v3ToStr(cctn0), dc.v3ToStr(cctn1), \
dc.mat4ToStr(self.tpsgriprotmats[i][j]), str(self.tpsgripjawwidth[i][j]), \
idfreetabletopplacement, self.tpsgripidfreeair[i][j])
sql = sql[:-2] + ";"
self.gdb.execute(sql)
else:
print "Freetabletopgrip already exist!"
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 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 == 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
class World(DirectObject):
global traverser, queue
def __init__(self):
startMenu = menu.Menu(self)
#Please do not remove this function, it makes the menu work.
def beginGame(self):
"""beginGame
parameters:
self
returns:
none
Description:
What would normalley be the contants of __init__. Called only after the menu.
"""
self.configurePanda()
camera.setPosHpr(0, -15, 0, 0, 0, 0) # x,y,z,heading, pitch, roll
#world init
self.setupBullet()
self.loadModels()
self.setupLights()
#self.initMove()
self.accept("escape",sys.exit)
self.overlay = overlay.Overlay(self)
#self.ball = ball.Ball(self)
self.onRay = list()
self.offRay = list()
self.activeRay = list()
def configurePanda(self):
"""configurePanda
parameters:
self
returns:
none
Description:
Set the rendering and display options for panda.
"""
props = WindowProperties()
props.setCursorHidden(True)
#props.setSize(1440, 900)
#props.setFullscreen(1)
base.win.requestProperties(props)
render.setShaderAuto()
base.disableMouse()
base.setFrameRateMeter(True)
#render.setAntialias(AntialiasAttrib.MAuto)
#render.setAntialias(AntialiasAttrib.MMultisample,4)
self.filters = CommonFilters(base.win, base.cam)
self.filters.setBloom(blend=(1,0,0,1), desat=-0.5, intensity=6.0, size=2)
#self.filters.setAmbientOcclusion()
#self.filters.setCartoonInk()
def setupBullet(self):
"""setupBullet
parameters:
self
returns:
none
Description:
Initialize bullet and (if needed) the bullet debug renderer.
"""
taskMgr.add(self.update, 'updateWorld')
self.worldNP = render.attachNewNode('World')
# World
#self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
#self.debugNP.show()
#self.debugNP.node().showWireframe(True)
#self.debugNP.node().showConstraints(False)
#self.debugNP.node().showBoundingBoxes(False)
#self.debugNP.node().showNormals(False)
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, 0))
#self.world.setDebugNode(self.debugNP.node())
def loadModels(self):
"""loadModels
parameters:
self
returns:
none
Description:
Loads the models and related collisions. Most game init goes here.
"""
# create the environment
self.env = collision.loadAndPositionModelFromFile("../assets/3d/mayaActors/arena_collisions_nogrid.egg")
self.envBoxes = objects.genBulletBoxes(self.env,self.world)
#load objects out of the environment
#human
self.mover = Human(self,self.world,self.worldNP)
tmp = self.env.find("**/PlayerSpawn1")
self.mover.bulletInit(self.world,tmp.getPos())
tmp.detachNode()
#AI players
self.players = objects.loadPlayers(self.env,self.world,self.worldNP)
for i in range(0,5):
self.players[i].bulletInit(self.world,self.players[i].instance.getPos())
#Spawners
collisionHandler=0
self.aTrapSpawn = objects.loadTrapAs(self.env, self.world, self.worldNP)
self.bTrapSpawn = objects.loadTrapBs(self.env, self.world, self.worldNP)
self.ammoSpawn = objects.loadAmmo(self.env, self.world, self.worldNP)
#optimization
self.env.flattenStrong()
render.analyze()
self.crowd = loader.loadModel("../assets/3d/Actors/crowd.egg")
self.crowd.reparentTo(render)
self.crowd.setScale(10)
self.crowd.setPos(0,0,-1000)
def setupLights(self):
"""setupLights
parameters:
self
returns:
none
Description:
Stick in some general lights.
"""
self.ambientLight = lights.setupAmbientLight()
self.dirLight = DirectionalLight("dirLight")
self.dirLight.setColor((.4,.4,.4,1))
self.dirLightNP = render.attachNewNode(self.dirLight)
self.dirLightNP.setHpr(0,-25,0)
render.setLight(self.dirLightNP)
#self.light = lights.loadModelSpotlightByName(self.human,"humanlight","humanlight1","segwaLight")
#panda2 = collision.loadAndPositionModelFromFile("panda-model",scale=.005,pos=tifLeftLight.getPos())
def update(self, task):
"""update
parameters:
self
returns:
none
Description:
Game Loop
"""
dt = globalClock.getDt()
# Update the spawners
for i in self.aTrapSpawn:
contacts = self.world.contactTest(i.sphere).getContacts()
if len(contacts)>0:
i.trap1Collide(contacts,self.mover)
for i in self.bTrapSpawn:
contacts = self.world.contactTest(i.sphere).getContacts()
if len(contacts)>0:
i.trap2Collide(contacts,self.mover)
for i in self.ammoSpawn:
contacts = self.world.contactTest(i.sphere).getContacts()
if len(contacts)>0:
i.ammoCollide(contacts,self.mover)
# update the traps and projectiles
for i in human.floatTrap.traps:
contacts = self.world.contactTest(i.sphere).getContacts()
if len(contacts)>0:
i.check(contacts,self.mover,self.players)
for i in human.clawTrap.traps:
contacts = self.world.contactTest(i.sphere).getContacts()
if len(contacts)>0:
i.check(contacts,self.mover,self.players)
for i in Projectile.projectiles:
contacts = self.world.contactTest(i.sphere).getContacts()
if len(contacts)>0:
i.check(contacts,self.mover,self.players)
# step forward the physics simulation
self.world.doPhysics(dt,1)
return task.cont
class Freesuc(object):
def __init__(self, ompath, handpkg, ser=False, torqueresist = 50):
self.objtrimesh = None
# the sampled points and their normals
self.objsamplepnts = None
self.objsamplenrmls = None
# the sampled points (bad samples removed)
self.objsamplepnts_ref = None
self.objsamplenrmls_ref = None
# the sampled points (bad samples removed + clustered)
self.objsamplepnts_refcls = None
self.objsamplenrmls_refcls = None
# facets is used to avoid repeated computation
self.facets = None
# facetnormals is used to plot overlapped facets with different heights
self.facetnormals = None
# facet2dbdries saves the 2d boundaries of each facet
self.facet2dbdries = None
# for plot
self.facetcolorarray = None
self.counter = 0
# for collision detection
self.bulletworld = BulletWorld()
self.hand = handpkg.newHandNM(hndcolor = [.2,0.7,.2,.3])
# collision free grasps
self.sucrotmats = []
self.succontacts = []
self.succontactnormals = []
# collided grasps
self.sucrotmatscld = []
self.succontactscld = []
self.succontactnormalscld = []
self.objcenter = [0,0,0]
self.torqueresist = torqueresist
if ser is False:
self.loadObjModel(ompath)
self.saveSerialized("tmpfsc.pickle")
else:
self.loadSerialized("tmpfsc.pickle", ompath)
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 loadSerialized(self, filename, ompath):
self.objtrimesh=trimesh.load_mesh(ompath)
try:
self.facets, self.facetnormals, self.facetcolorarray, self.objsamplepnts, \
self.objsamplenrmls, self.objsamplepnts_ref, self.objsamplenrmls_ref, \
self.objsamplepnts_refcls, self.objsamplenrmls_refcls = \
pickle.load(open(filename, mode="rb"))
except:
print str(sys.exc_info()[0])+" cannot load tmpcp.pickle"
raise
def saveSerialized(self, filename):
pickle.dump([self.facets, self.facetnormals, self.facetcolorarray, self.objsamplepnts, \
self.objsamplenrmls, self.objsamplepnts_ref, self.objsamplenrmls_ref, \
self.objsamplepnts_refcls, self.objsamplenrmls_refcls], open(filename, mode="wb"))
def sampleObjModel(self, numpointsoververts=5):
"""
sample the object model
self.objsamplepnts and self.objsamplenrmls
are filled in this function
:param: numpointsoververts: the number of sampled points = numpointsoververts*mesh.vertices.shape[0]
:return: nverts: the number of verts sampled
author: weiwei
date: 20160623 flight to tokyo
"""
nverts = self.objtrimesh.vertices.shape[0]
samples, face_idx = manipulation.suction.sample.sample_surface_even(self.objtrimesh,
count=(1000 if nverts*numpointsoververts > 1000 \
else nverts*numpointsoververts))
# print nverts
self.objsamplepnts = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object)
self.objsamplenrmls = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object)
for i, faces in enumerate(self.facets):
for face in faces:
sample_idx = np.where(face_idx==face)[0]
if len(sample_idx) > 0:
if self.objsamplepnts[i] is not None:
self.objsamplepnts[i] = np.vstack((self.objsamplepnts[i], samples[sample_idx]))
self.objsamplenrmls[i] = np.vstack((self.objsamplenrmls[i],
[self.objtrimesh.face_normals[face]]*samples[sample_idx].shape[0]))
else:
self.objsamplepnts[i] = np.array(samples[sample_idx])
self.objsamplenrmls[i] = np.array([self.objtrimesh.face_normals[face]]*samples[sample_idx].shape[0])
if self.objsamplepnts[i] is None:
self.objsamplepnts[i] = np.empty(shape=[0,0])
self.objsamplenrmls[i] = np.empty(shape=[0,0])
return nverts
def removeBadSamples(self, mindist=7, maxdist=9999):
'''
Do the following refinement:
(1) remove the samples who's minimum distance to facet boundary is smaller than mindist
(2) remove the samples who's maximum distance to facet boundary is larger than mindist
## input
mindist, maxdist
as explained in the begining
author: weiwei
date: 20160623 flight to tokyo
'''
# ref = refine
self.objsamplepnts_ref = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object)
self.objsamplenrmls_ref = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object)
self.facet2dbdries = []
for i, faces in enumerate(self.facets):
# print "removebadsample"
# print i,len(self.facets)
facetp = None
face0verts = self.objtrimesh.vertices[self.objtrimesh.faces[faces[0]]]
facetmat = robotmath.rotmatfacet(self.facetnormals[i], face0verts[0], face0verts[1])
# face samples
samplepntsp =[]
for j, apnt in enumerate(self.objsamplepnts[i]):
apntp = np.dot(facetmat, apnt)[:2]
samplepntsp.append(apntp)
# face boundaries
for j, faceidx in enumerate(faces):
vert0 = self.objtrimesh.vertices[self.objtrimesh.faces[faceidx][0]]
vert1 = self.objtrimesh.vertices[self.objtrimesh.faces[faceidx][1]]
vert2 = self.objtrimesh.vertices[self.objtrimesh.faces[faceidx][2]]
vert0p = np.dot(facetmat, vert0)[:2]
vert1p = np.dot(facetmat, vert1)[:2]
vert2p = np.dot(facetmat, vert2)[:2]
facep = Polygon([vert0p, vert1p, vert2p])
if facetp is None:
facetp = facep
else:
try:
facetp = facetp.union(facep)
except:
continue
self.facet2dbdries.append(facetp)
selectedele = []
for j, apntp in enumerate(samplepntsp):
try:
apntpnt = Point(apntp[0], apntp[1])
dbnds = []
dbnds.append(apntpnt.distance(facetp.exterior))
for fpinter in facetp.interiors:
dbnds.append(apntpnt.distance(fpinter))
dbnd = min(dbnds)
if dbnd < mindist or dbnd > maxdist:
pass
else:
selectedele.append(j)
except:
pass
self.objsamplepnts_ref[i] = np.asarray([self.objsamplepnts[i][j] for j in selectedele])
self.objsamplenrmls_ref[i] = np.asarray([self.objsamplenrmls[i][j] for j in selectedele])
self.facet2dbdries = np.array(self.facet2dbdries)
# if i is 3:
# for j, apntp in enumerate(samplepntsp):
# apntpnt = Point(apntp[0], apntp[1])
# plt.plot(apntpnt.x, apntpnt.y, 'bo')
# for j, apnt in enumerate([samplepntsp[j] for j in selectedele]):
# plt.plot(apnt[0], apnt[1], 'ro')
# ftpx, ftpy = facetp.exterior.xy
# plt.plot(ftpx, ftpy)
# for fpinters in facetp.interiors:
# ftpxi, ftpyi = fpinters.xy
# plt.plot(ftpxi, ftpyi)
# plt.axis('equal')
# plt.show()
# pass
# old code for concatenating in 3d space
# boundaryedges = []
# for faceid in faces:
# faceverts = self.objtrimesh.faces[faceid]
# try:
# boundaryedges.remove([faceverts[1], faceverts[0]])
# except:
# boundaryedges.append([faceverts[0], faceverts[1]])
# try:
# boundaryedges.remove([faceverts[2], faceverts[1]])
# except:
# boundaryedges.append([faceverts[1], faceverts[2]])
# try:
# boundaryedges.remove([faceverts[0], faceverts[2]])
# except:
# boundaryedges.append([faceverts[2], faceverts[0]])
# print boundaryedges
# print len(boundaryedges)
# TODO: compute boundary polygons, both outsider and inner should be considered
# assort boundaryedges
# boundarypolygonlist = []
# boundarypolygon = [boundaryedges[0]]
# boundaryedgesfirstcolumn = [row[0] for row in boundaryedges]
# for i in range(len(boundaryedges)-1):
# vertpivot = boundarypolygon[i][1]
# boundarypolygon.append(boundaryedges[boundaryedgesfirstcolumn.index(vertpivot)])
# print boundarypolygon
# print len(boundarypolygon)
# return boundaryedges, boundarypolygon
def clusterFacetSamplesKNN(self, reduceRatio=3, maxNPnts=5):
"""
cluster the samples of each facet using k nearest neighbors
the cluster center and their correspondent normals will be saved
in self.objsamplepnts_refcls and self.objsamplenrmals_refcls
:param: reduceRatio: the ratio of points to reduce
:param: maxNPnts: the maximum number of points on a facet
:return: None
author: weiwei
date: 20161129, tsukuba
"""
self.objsamplepnts_refcls = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object)
self.objsamplenrmls_refcls = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object)
for i, facet in enumerate(self.facets):
self.objsamplepnts_refcls[i] = np.empty(shape=(0,0))
self.objsamplenrmls_refcls[i] = np.empty(shape=(0,0))
X = self.objsamplepnts_ref[i]
nX = X.shape[0]
if nX > reduceRatio:
kmeans = KMeans(n_clusters=maxNPnts if nX/reduceRatio>maxNPnts else nX/reduceRatio, random_state=0).fit(X)
self.objsamplepnts_refcls[i] = kmeans.cluster_centers_
self.objsamplenrmls_refcls[i] = np.tile(self.facetnormals[i], [self.objsamplepnts_refcls.shape[0],1])
def clusterFacetSamplesRNN(self, reduceRadius=3):
"""
cluster the samples of each facet using radius nearest neighbours
the cluster center and their correspondent normals will be saved
in self.objsamplepnts_refcls and self.objsamplenrmals_refcls
:param: reduceRadius: the neighbors that fall inside the reduceradius will be removed
:return: None
author: weiwei
date: 20161130, osaka
"""
self.objsamplepnts_refcls = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object)
self.objsamplenrmls_refcls = np.ndarray(shape=(self.facets.shape[0],), dtype=np.object)
for i, facet in enumerate(self.facets):
# print "cluster"
# print i,len(self.facets)
self.objsamplepnts_refcls[i] = []
self.objsamplenrmls_refcls[i] = []
X = self.objsamplepnts_ref[i]
nX = X.shape[0]
if nX > 0:
neigh = RadiusNeighborsClassifier(radius=1.0)
neigh.fit(X, range(nX))
neigharrays = neigh.radius_neighbors(X, radius=reduceRadius, return_distance=False)
delset = set([])
for j in range(nX):
if j not in delset:
self.objsamplepnts_refcls[i].append(np.array(X[j]))
self.objsamplenrmls_refcls[i].append(np.array(self.objsamplenrmls_ref[i][j]))
# if self.objsamplepnts_refcls[i].size:
# self.objsamplepnts_refcls[i] = np.vstack((self.objsamplepnts_refcls[i], X[j]))
# self.objsamplenrmls_refcls[i] = np.vstack((self.objsamplenrmls_refcls[i],
# self.objsamplenrmls_ref[i][j]))
# else:
# self.objsamplepnts_refcls[i] = np.array([])
# self.objsamplenrmls_refcls[i] = np.array([])
# self.objsamplepnts_refcls[i] = np.hstack((self.objsamplepnts_refcls[i], X[j]))
# self.objsamplenrmls_refcls[i] = np.hstack((self.objsamplenrmls_refcls[i],
# self.objsamplenrmls_ref[i][j]))
delset.update(neigharrays[j].tolist())
if self.objsamplepnts_refcls[i]:
self.objsamplepnts_refcls[i] = np.vstack(self.objsamplepnts_refcls[i])
self.objsamplenrmls_refcls[i] = np.vstack(self.objsamplenrmls_refcls[i])
else:
self.objsamplepnts_refcls[i] = np.empty(shape=(0,0))
self.objsamplenrmls_refcls[i] = np.empty(shape=(0,0))
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.sucrotmats = []
self.succontacts = []
self.succontactnormals = []
# collided
self.sucrotmatscld = []
self.succontactscld = []
self.succontactnormalscld = []
plotoffsetfp = 3
self.counter = 0
while self.counter < self.facets.shape[0]:
# print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
for i in range(self.objsamplepnts_refcls[self.counter].shape[0]):
for angleid in range(discretesize):
cctpnt = self.objsamplepnts_refcls[self.counter][i] + plotoffsetfp * self.objsamplenrmls_refcls[self.counter][i]
# check torque resistance
print Vec3(cctpnt[0],cctpnt[1],cctpnt[2]).length()
if Vec3(cctpnt[0],cctpnt[1],cctpnt[2]).length() < self.torqueresist:
cctnrml = self.objsamplenrmls_refcls[self.counter][i]
rotangle = 360.0 / discretesize * angleid
tmphand = self.hand
tmphand.attachTo(cctpnt[0], cctpnt[1], cctpnt[2], cctnrml[0], cctnrml[1], cctnrml[2], rotangle)
hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.succontacts.append(self.objsamplepnts_refcls[self.counter][i])
self.succontactnormals.append(self.objsamplenrmls_refcls[self.counter][i])
self.sucrotmats.append(tmphand.getMat())
else:
self.succontactscld.append(self.objsamplepnts_refcls[self.counter][i])
self.succontactnormalscld.append(self.objsamplenrmls_refcls[self.counter][i])
self.sucrotmatscld.append(tmphand.getMat())
self.counter+=1
self.counter = 0
def segShow(self, base, togglesamples=False, togglenormals=False,
togglesamples_ref=False, togglenormals_ref=False,
togglesamples_refcls=False, togglenormals_refcls=False, alpha = .1):
"""
: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
# offsetf = facet
# plot the segments
plotoffsetf = .0
for i, facet in enumerate(self.facets):
geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i],
[self.objtrimesh.vertices.shape[0],1]), \
# -np.tile(self.objcenter,[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], alpha))
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)
rgbapnts0 = [1,1,1,1]
rgbapnts1 = [.5,.5,0,1]
rgbapnts2 = [1,0,0,1]
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(star, pos=apnt+plotoffsetf*i*self.facetnormals[i], radius=3, 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, 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 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)
class Freesuc(object):
def __init__(self, ompath, handpkg, ser=False, torqueresist=50):
self.objtrimesh = None
# the sampled points and their normals
self.objsamplepnts = None
self.objsamplenrmls = None
# the sampled points (bad samples removed)
self.objsamplepnts_ref = None
self.objsamplenrmls_ref = None
# the sampled points (bad samples removed + clustered)
self.objsamplepnts_refcls = None
self.objsamplenrmls_refcls = None
# facets is used to avoid repeated computation
self.facets = None
# facetnormals is used to plot overlapped facets with different heights
self.facetnormals = None
# facet2dbdries saves the 2d boundaries of each facet
self.facet2dbdries = None
# for plot
self.facetcolorarray = None
self.counter = 0
# for collision detection
self.bulletworld = BulletWorld()
self.hand = handpkg.newHandNM(hndcolor=[.2, 0.7, .2, .3])
# collision free grasps
self.sucrotmats = []
self.succontacts = []
self.succontactnormals = []
# collided grasps
self.sucrotmatscld = []
self.succontactscld = []
self.succontactnormalscld = []
self.objcenter = [0, 0, 0]
self.torqueresist = torqueresist
if ser is False:
self.loadObjModel(ompath)
self.saveSerialized("tmpfsc.pickle")
else:
self.loadSerialized("tmpfsc.pickle", ompath)
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 loadSerialized(self, filename, ompath):
self.objtrimesh = trimesh.load_mesh(ompath)
try:
self.facets, self.facetnormals, self.facetcolorarray, self.objsamplepnts, \
self.objsamplenrmls, self.objsamplepnts_ref, self.objsamplenrmls_ref, \
self.objsamplepnts_refcls, self.objsamplenrmls_refcls = \
pickle.load(open(filename, mode="rb"))
except:
print(str(sys.exc_info()[0]) + " cannot load tmpcp.pickle")
raise
def saveSerialized(self, filename):
pickle.dump([self.facets, self.facetnormals, self.facetcolorarray, self.objsamplepnts, \
self.objsamplenrmls, self.objsamplepnts_ref, self.objsamplenrmls_ref, \
self.objsamplepnts_refcls, self.objsamplenrmls_refcls], open(filename, mode="wb"))
def sampleObjModel(self, numpointsoververts=5):
"""
sample the object model
self.objsamplepnts and self.objsamplenrmls
are filled in this function
:param: numpointsoververts: the number of sampled points = numpointsoververts*mesh.vertices.shape[0]
:return: nverts: the number of verts sampled
author: weiwei
date: 20160623 flight to tokyo
"""
nverts = self.objtrimesh.vertices.shape[0]
samples, face_idx = manipulation.suction.sample.sample_surface_even(self.objtrimesh,
count=(1000 if nverts*numpointsoververts > 1000 \
else nverts*numpointsoververts))
# print(nverts)
self.objsamplepnts = np.ndarray(shape=(self.facets.shape[0], ),
dtype=np.object)
self.objsamplenrmls = np.ndarray(shape=(self.facets.shape[0], ),
dtype=np.object)
for i, faces in enumerate(self.facets):
for face in faces:
sample_idx = np.where(face_idx == face)[0]
if len(sample_idx) > 0:
if self.objsamplepnts[i] is not None:
self.objsamplepnts[i] = np.vstack(
(self.objsamplepnts[i], samples[sample_idx]))
self.objsamplenrmls[i] = np.vstack(
(self.objsamplenrmls[i],
[self.objtrimesh.face_normals[face]] *
samples[sample_idx].shape[0]))
else:
self.objsamplepnts[i] = np.array(samples[sample_idx])
self.objsamplenrmls[i] = np.array(
[self.objtrimesh.face_normals[face]] *
samples[sample_idx].shape[0])
if self.objsamplepnts[i] is None:
self.objsamplepnts[i] = np.empty(shape=[0, 0])
self.objsamplenrmls[i] = np.empty(shape=[0, 0])
return nverts
def removeBadSamples(self, mindist=7, maxdist=9999):
'''
Do the following refinement:
(1) remove the samples who's minimum distance to facet boundary is smaller than mindist
(2) remove the samples who's maximum distance to facet boundary is larger than mindist
## input
mindist, maxdist
as explained in the begining
author: weiwei
date: 20160623 flight to tokyo
'''
# ref = refine
self.objsamplepnts_ref = np.ndarray(shape=(self.facets.shape[0], ),
dtype=np.object)
self.objsamplenrmls_ref = np.ndarray(shape=(self.facets.shape[0], ),
dtype=np.object)
self.facet2dbdries = []
for i, faces in enumerate(self.facets):
# print("removebadsample")
# print(i,len(self.facets))
facetp = None
face0verts = self.objtrimesh.vertices[self.objtrimesh.faces[
faces[0]]]
facetmat = robotmath.rotmatfacet(self.facetnormals[i],
face0verts[0], face0verts[1])
# face samples
samplepntsp = []
for j, apnt in enumerate(self.objsamplepnts[i]):
apntp = np.dot(facetmat, apnt)[:2]
samplepntsp.append(apntp)
# face boundaries
for j, faceidx in enumerate(faces):
vert0 = self.objtrimesh.vertices[self.objtrimesh.faces[faceidx]
[0]]
vert1 = self.objtrimesh.vertices[self.objtrimesh.faces[faceidx]
[1]]
vert2 = self.objtrimesh.vertices[self.objtrimesh.faces[faceidx]
[2]]
vert0p = np.dot(facetmat, vert0)[:2]
vert1p = np.dot(facetmat, vert1)[:2]
vert2p = np.dot(facetmat, vert2)[:2]
facep = Polygon([vert0p, vert1p, vert2p])
if facetp is None:
facetp = facep
else:
try:
facetp = facetp.union(facep)
except:
continue
self.facet2dbdries.append(facetp)
selectedele = []
for j, apntp in enumerate(samplepntsp):
try:
apntpnt = Point(apntp[0], apntp[1])
dbnds = []
dbnds.append(apntpnt.distance(facetp.exterior))
for fpinter in facetp.interiors:
dbnds.append(apntpnt.distance(fpinter))
dbnd = min(dbnds)
if dbnd < mindist or dbnd > maxdist:
pass
else:
selectedele.append(j)
except:
pass
self.objsamplepnts_ref[i] = np.asarray(
[self.objsamplepnts[i][j] for j in selectedele])
self.objsamplenrmls_ref[i] = np.asarray(
[self.objsamplenrmls[i][j] for j in selectedele])
self.facet2dbdries = np.array(self.facet2dbdries)
# if i is 3:
# for j, apntp in enumerate(samplepntsp):
# apntpnt = Point(apntp[0], apntp[1])
# plt.plot(apntpnt.x, apntpnt.y, 'bo')
# for j, apnt in enumerate([samplepntsp[j] for j in selectedele]):
# plt.plot(apnt[0], apnt[1], 'ro')
# ftpx, ftpy = facetp.exterior.xy
# plt.plot(ftpx, ftpy)
# for fpinters in facetp.interiors:
# ftpxi, ftpyi = fpinters.xy
# plt.plot(ftpxi, ftpyi)
# plt.axis('equal')
# plt.show()
# pass
# old code for concatenating in 3d space
# boundaryedges = []
# for faceid in faces:
# faceverts = self.objtrimesh.faces[faceid]
# try:
# boundaryedges.remove([faceverts[1], faceverts[0]])
# except:
# boundaryedges.append([faceverts[0], faceverts[1]])
# try:
# boundaryedges.remove([faceverts[2], faceverts[1]])
# except:
# boundaryedges.append([faceverts[1], faceverts[2]])
# try:
# boundaryedges.remove([faceverts[0], faceverts[2]])
# except:
# boundaryedges.append([faceverts[2], faceverts[0]])
# print(boundaryedges)
# print(len(boundaryedges))
# TODO: compute boundary polygons, both outsider and inner should be considered
# assort boundaryedges
# boundarypolygonlist = []
# boundarypolygon = [boundaryedges[0]]
# boundaryedgesfirstcolumn = [row[0] for row in boundaryedges]
# for i in range(len(boundaryedges)-1):
# vertpivot = boundarypolygon[i][1]
# boundarypolygon.append(boundaryedges[boundaryedgesfirstcolumn.index(vertpivot)])
# print(boundarypolygon)
# print(len(boundarypolygon))
# return boundaryedges, boundarypolygon
def clusterFacetSamplesKNN(self, reduceRatio=3, maxNPnts=5):
"""
cluster the samples of each facet using k nearest neighbors
the cluster center and their correspondent normals will be saved
in self.objsamplepnts_refcls and self.objsamplenrmals_refcls
:param: reduceRatio: the ratio of points to reduce
:param: maxNPnts: the maximum number of points on a facet
:return: None
author: weiwei
date: 20161129, tsukuba
"""
self.objsamplepnts_refcls = np.ndarray(shape=(self.facets.shape[0], ),
dtype=np.object)
self.objsamplenrmls_refcls = np.ndarray(shape=(self.facets.shape[0], ),
dtype=np.object)
for i, facet in enumerate(self.facets):
self.objsamplepnts_refcls[i] = np.empty(shape=(0, 0))
self.objsamplenrmls_refcls[i] = np.empty(shape=(0, 0))
X = self.objsamplepnts_ref[i]
nX = X.shape[0]
if nX > reduceRatio:
kmeans = KMeans(n_clusters=maxNPnts
if nX / reduceRatio > maxNPnts else nX /
reduceRatio,
random_state=0).fit(X)
self.objsamplepnts_refcls[i] = kmeans.cluster_centers_
self.objsamplenrmls_refcls[i] = np.tile(
self.facetnormals[i],
[self.objsamplepnts_refcls.shape[0], 1])
def clusterFacetSamplesRNN(self, reduceRadius=3):
"""
cluster the samples of each facet using radius nearest neighbours
the cluster center and their correspondent normals will be saved
in self.objsamplepnts_refcls and self.objsamplenrmals_refcls
:param: reduceRadius: the neighbors that fall inside the reduceradius will be removed
:return: None
author: weiwei
date: 20161130, osaka
"""
self.objsamplepnts_refcls = np.ndarray(shape=(self.facets.shape[0], ),
dtype=np.object)
self.objsamplenrmls_refcls = np.ndarray(shape=(self.facets.shape[0], ),
dtype=np.object)
for i, facet in enumerate(self.facets):
# print("cluster")
# print(i,len(self.facets))
self.objsamplepnts_refcls[i] = []
self.objsamplenrmls_refcls[i] = []
X = self.objsamplepnts_ref[i]
nX = X.shape[0]
if nX > 0:
neigh = RadiusNeighborsClassifier(radius=1.0)
neigh.fit(X, range(nX))
neigharrays = neigh.radius_neighbors(X,
radius=reduceRadius,
return_distance=False)
delset = set([])
for j in range(nX):
if j not in delset:
self.objsamplepnts_refcls[i].append(np.array(X[j]))
self.objsamplenrmls_refcls[i].append(
np.array(self.objsamplenrmls_ref[i][j]))
# if self.objsamplepnts_refcls[i].size:
# self.objsamplepnts_refcls[i] = np.vstack((self.objsamplepnts_refcls[i], X[j]))
# self.objsamplenrmls_refcls[i] = np.vstack((self.objsamplenrmls_refcls[i],
# self.objsamplenrmls_ref[i][j]))
# else:
# self.objsamplepnts_refcls[i] = np.array([])
# self.objsamplenrmls_refcls[i] = np.array([])
# self.objsamplepnts_refcls[i] = np.hstack((self.objsamplepnts_refcls[i], X[j]))
# self.objsamplenrmls_refcls[i] = np.hstack((self.objsamplenrmls_refcls[i],
# self.objsamplenrmls_ref[i][j]))
delset.update(neigharrays[j].tolist())
if self.objsamplepnts_refcls[i]:
self.objsamplepnts_refcls[i] = np.vstack(
self.objsamplepnts_refcls[i])
self.objsamplenrmls_refcls[i] = np.vstack(
self.objsamplenrmls_refcls[i])
else:
self.objsamplepnts_refcls[i] = np.empty(shape=(0, 0))
self.objsamplenrmls_refcls[i] = np.empty(shape=(0, 0))
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.sucrotmats = []
self.succontacts = []
self.succontactnormals = []
# collided
self.sucrotmatscld = []
self.succontactscld = []
self.succontactnormalscld = []
plotoffsetfp = 3
self.counter = 0
while self.counter < self.facets.shape[0]:
# print(str(self.counter) + "/" + str(self.facetpairs.shape[0]-1))
# print(self.gripcontactpairs_precc)
for i in range(self.objsamplepnts_refcls[self.counter].shape[0]):
for angleid in range(discretesize):
cctpnt = self.objsamplepnts_refcls[self.counter][
i] + plotoffsetfp * self.objsamplenrmls_refcls[
self.counter][i]
# check torque resistance
print(Vec3(cctpnt[0], cctpnt[1], cctpnt[2]).length())
if Vec3(cctpnt[0], cctpnt[1],
cctpnt[2]).length() < self.torqueresist:
cctnrml = self.objsamplenrmls_refcls[self.counter][i]
rotangle = 360.0 / discretesize * angleid
tmphand = self.hand
tmphand.attachTo(cctpnt[0], cctpnt[1], cctpnt[2],
cctnrml[0], cctnrml[1], cctnrml[2],
rotangle)
hndbullnode = cd.genCollisionMeshMultiNp(
tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.succontacts.append(
self.objsamplepnts_refcls[self.counter][i])
self.succontactnormals.append(
self.objsamplenrmls_refcls[self.counter][i])
self.sucrotmats.append(tmphand.getMat())
else:
self.succontactscld.append(
self.objsamplepnts_refcls[self.counter][i])
self.succontactnormalscld.append(
self.objsamplenrmls_refcls[self.counter][i])
self.sucrotmatscld.append(tmphand.getMat())
self.counter += 1
self.counter = 0
def segShow(self,
base,
togglesamples=False,
togglenormals=False,
togglesamples_ref=False,
togglenormals_ref=False,
togglesamples_refcls=False,
togglenormals_refcls=False,
alpha=.1):
"""
: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
# offsetf = facet
# plot the segments
plotoffsetf = .0
for i, facet in enumerate(self.facets):
geom = pandageom.packpandageom(self.objtrimesh.vertices+np.tile(plotoffsetf*i*self.facetnormals[i],
[self.objtrimesh.vertices.shape[0],1]), \
# -np.tile(self.objcenter,[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], alpha))
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)
rgbapnts0 = [1, 1, 1, 1]
rgbapnts1 = [.5, .5, 0, 1]
rgbapnts2 = [1, 0, 0, 1]
if togglesamples:
for j, apnt in enumerate(self.objsamplepnts[i]):
pandageom.plotSphere(
star,
pos=apnt + plotoffsetf * i * self.facetnormals[i],
radius=3,
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,
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 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)
class Panda3dBulletPhysics(World):
# NOTE: the model ids of objects that correspond to opened doors. They will be ignored for collisions.
openedDoorModelIds = [
'122',
'133',
'214',
'246',
'247',
'361',
'73',
'756',
'757',
'758',
'759',
'760',
'761',
'762',
'763',
'764',
'765',
'768',
'769',
'770',
'771',
'778',
'779',
'780',
's__1762',
's__1763',
's__1764',
's__1765',
's__1766',
's__1767',
's__1768',
's__1769',
's__1770',
's__1771',
's__1772',
's__1773',
]
# FIXME: is not a complete list of movable objects
movableObjectCategories = [
'table', 'dressing_table', 'sofa', 'trash_can', 'chair', 'ottoman',
'bed'
]
# For more material, see table: http://www.ambrsoft.com/CalcPhysics/Density/Table_2.htm
defaultDensity = 1000.0 # kg/m^3
# For more coefficients, see table: https://www.thoughtspike.com/friction-coefficients-for-bullet-physics/
defaultMaterialFriction = 0.7
defaultMaterialRestitution = 0.5
def __init__(self,
scene,
suncgDatasetRoot=None,
debug=False,
objectMode='box',
agentRadius=0.1,
agentHeight=1.6,
agentMass=60.0,
agentMode='capsule'):
super(Panda3dBulletPhysics, self).__init__()
self.__dict__.update(scene=scene,
suncgDatasetRoot=suncgDatasetRoot,
debug=debug,
objectMode=objectMode,
agentRadius=agentRadius,
agentHeight=agentHeight,
agentMass=agentMass,
agentMode=agentMode)
if suncgDatasetRoot is not None:
self.modelCatMapping = ModelCategoryMapping(
os.path.join(suncgDatasetRoot, "metadata",
"ModelCategoryMapping.csv"))
else:
self.modelCatMapping = None
self.bulletWorld = BulletWorld()
self.bulletWorld.setGravity(Vec3(0, 0, -9.81))
if debug:
debugNode = BulletDebugNode('physic-debug')
debugNode.showWireframe(True)
debugNode.showConstraints(False)
debugNode.showBoundingBoxes(True)
debugNode.showNormals(False)
self.debugNodePath = self.scene.scene.attachNewNode(debugNode)
self.debugNodePath.show()
self.bulletWorld.setDebugNode(debugNode)
else:
self.debugNodePath = None
self._initLayoutModels()
self._initAgents()
self._initObjects()
self.scene.worlds['physics'] = self
def destroy(self):
# Nothing to do
pass
def _initLayoutModels(self):
# Load layout objects as meshes
for model in self.scene.scene.findAllMatches(
'**/layouts/object*/model*'):
shape, transform = getCollisionShapeFromModel(
model, mode='mesh', defaultCentered=False)
node = BulletRigidBodyNode('physics')
node.setMass(0.0)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.setStatic(True)
node.addShape(shape)
node.setDeactivationEnabled(True)
node.setIntoCollideMask(BitMask32.allOn())
self.bulletWorld.attach(node)
# Attach the physic-related node to the scene graph
physicsNp = model.getParent().attachNewNode(node)
physicsNp.setTransform(transform)
if node.isStatic():
model.getParent().setTag('physics-mode', 'static')
else:
model.getParent().setTag('physics-mode', 'dynamic')
# Reparent render and acoustics nodes (if any) below the new physic node
#XXX: should be less error prone to just reparent all children (except the hidden model)
renderNp = model.getParent().find('**/render')
if not renderNp.isEmpty():
renderNp.reparentTo(physicsNp)
acousticsNp = model.getParent().find('**/acoustics')
if not acousticsNp.isEmpty():
acousticsNp.reparentTo(physicsNp)
# NOTE: we need this to update the transform state of the internal bullet node
physicsNp.node().setTransformDirty()
# Validation
assert np.allclose(
mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(model.getNetTransform().getMat()),
atol=1e-6)
def _initAgents(self):
# Load agents
for agent in self.scene.scene.findAllMatches('**/agents/agent*'):
transform = TransformState.makeIdentity()
if self.agentMode == 'capsule':
shape = BulletCapsuleShape(
self.agentRadius, self.agentHeight - 2 * self.agentRadius)
elif self.agentMode == 'sphere':
shape = BulletCapsuleShape(self.agentRadius,
2 * self.agentRadius)
# XXX: use BulletCharacterControllerNode class, which already handles local transform?
node = BulletRigidBodyNode('physics')
node.setMass(self.agentMass)
node.setStatic(False)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.addShape(shape)
self.bulletWorld.attach(node)
# Constrain the agent to have fixed position on the Z-axis
node.setLinearFactor(Vec3(1.0, 1.0, 0.0))
# Constrain the agent not to be affected by rotations
node.setAngularFactor(Vec3(0.0, 0.0, 0.0))
node.setIntoCollideMask(BitMask32.allOn())
node.setDeactivationEnabled(True)
# Enable continuous collision detection (CCD)
node.setCcdMotionThreshold(1e-7)
node.setCcdSweptSphereRadius(0.50)
if node.isStatic():
agent.setTag('physics-mode', 'static')
else:
agent.setTag('physics-mode', 'dynamic')
# Attach the physic-related node to the scene graph
physicsNp = NodePath(node)
physicsNp.setTransform(transform)
# Reparent all child nodes below the new physic node
for child in agent.getChildren():
child.reparentTo(physicsNp)
physicsNp.reparentTo(agent)
# NOTE: we need this to update the transform state of the internal bullet node
physicsNp.node().setTransformDirty()
# Validation
assert np.allclose(
mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(agent.getNetTransform().getMat()),
atol=1e-6)
def _initObjects(self):
# Load objects
for model in self.scene.scene.findAllMatches(
'**/objects/object*/model*'):
modelId = model.getParent().getTag('model-id')
# XXX: we could create BulletGhostNode instance for non-collidable objects, but we would need to filter out the collisions later on
if not modelId in self.openedDoorModelIds:
shape, transform = getCollisionShapeFromModel(
model, self.objectMode, defaultCentered=True)
node = BulletRigidBodyNode('physics')
node.addShape(shape)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.setIntoCollideMask(BitMask32.allOn())
node.setDeactivationEnabled(True)
if self.suncgDatasetRoot is not None:
# Check if it is a movable object
category = self.modelCatMapping.getCoarseGrainedCategoryForModelId(
modelId)
if category in self.movableObjectCategories:
# Estimate mass of object based on volumetric data and default material density
objVoxFilename = os.path.join(self.suncgDatasetRoot,
'object_vox',
'object_vox_data',
modelId,
modelId + '.binvox')
voxelData = ObjectVoxelData.fromFile(objVoxFilename)
mass = Panda3dBulletPhysics.defaultDensity * voxelData.getFilledVolume(
)
node.setMass(mass)
else:
node.setMass(0.0)
node.setStatic(True)
else:
node.setMass(0.0)
node.setStatic(True)
if node.isStatic():
model.getParent().setTag('physics-mode', 'static')
else:
model.getParent().setTag('physics-mode', 'dynamic')
# Attach the physic-related node to the scene graph
physicsNp = model.getParent().attachNewNode(node)
physicsNp.setTransform(transform)
# Reparent render and acoustics nodes (if any) below the new physic node
#XXX: should be less error prone to just reparent all children (except the hidden model)
renderNp = model.getParent().find('**/render')
if not renderNp.isEmpty():
renderNp.reparentTo(physicsNp)
acousticsNp = model.getParent().find('**/acoustics')
if not acousticsNp.isEmpty():
acousticsNp.reparentTo(physicsNp)
# NOTE: we need this to update the transform state of the internal bullet node
node.setTransformDirty()
# NOTE: we need to add the node to the bullet engine only after setting all attributes
self.bulletWorld.attach(node)
# Validation
assert np.allclose(
mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(
model.getParent().getNetTransform().getMat()),
atol=1e-6)
else:
logger.debug('Object %s ignored from physics' % (modelId))
def step(self, dt):
self.bulletWorld.doPhysics(dt)
def isCollision(self, root):
isCollisionDetected = False
if isinstance(root.node(), BulletRigidBodyNode):
result = self.bulletWorld.contactTest(root.node())
if result.getNumContacts() > 0:
isCollisionDetected = True
else:
for nodePath in root.findAllMatches('**/+BulletBodyNode'):
isCollisionDetected |= self.isCollision(nodePath)
return isCollisionDetected
def getCollisionInfo(self, root, dt):
result = self.bulletWorld.contactTest(root.node())
force = 0.0
relativePosition = LVecBase3f(0.0, 0.0, 0.0)
isCollisionDetected = False
for _ in result.getContacts():
# Iterate over all manifolds of the world
# NOTE: it seems like the contact manifold doesn't hold the information
# to calculate contact force. We need the persistent manifolds for that.
for manifold in self.bulletWorld.getManifolds():
# Check if the root node is part of that manifold, by checking positions
# TODO: there is surely a better way to compare the two nodes here
#if (manifold.getNode0().getTransform().getPos() == root.getNetTransform().getPos()):
if manifold.getNode0().getTag('model-id') == root.getTag(
'model-id'):
# Calculate the to
totalImpulse = 0.0
maxImpulse = 0.0
for pt in manifold.getManifoldPoints():
impulse = pt.getAppliedImpulse()
totalImpulse += impulse
if impulse > maxImpulse:
maxImpulse = impulse
relativePosition = pt.getLocalPointA()
force = totalImpulse / dt
isCollisionDetected = True
return force, relativePosition, isCollisionDetected
def calculate2dNavigationMap(self, agent, z=0.1, precision=0.1, yup=True):
agentRbNp = agent.find('**/+BulletRigidBodyNode')
# Calculate the bounding box of the scene
bounds = []
for nodePath in self.scene.scene.findAllMatches(
'**/object*/+BulletRigidBodyNode'):
node = nodePath.node()
#NOTE: the bounding sphere doesn't seem to take into account the transform, so apply it manually (translation only)
bsphere = node.getShapeBounds()
center = nodePath.getNetTransform().getPos()
bounds.extend(
[center + bsphere.getMin(), center + bsphere.getMax()])
minBounds, maxBounds = np.min(bounds, axis=0), np.max(bounds, axis=0)
# Using the X and Y dimensions of the bounding box, discretize the 2D plan into a uniform grid with given precision
X = np.arange(minBounds[0], maxBounds[0], step=precision)
Y = np.arange(minBounds[1], maxBounds[1], step=precision)
nbTotalCells = len(X) * len(Y)
threshold10Perc = int(nbTotalCells / 10)
# XXX: the simulation needs to be run a little before moving the agent, not sure why
self.bulletWorld.doPhysics(0.1)
# Sweep the position of the agent across the grid, checking if collision/contacts occurs with objects or walls in the scene.
occupancyMap = np.zeros((len(X), len(Y)))
occupancyMapCoord = np.zeros((len(X), len(Y), 2))
n = 0
for i, x in enumerate(X):
for j, y in enumerate(Y):
agentRbNp.setPos(LVecBase3f(x, y, z))
if self.isCollision(agentRbNp):
occupancyMap[i, j] = 1.0
occupancyMapCoord[i, j, 0] = x
occupancyMapCoord[i, j, 1] = y
n += 1
if n % threshold10Perc == 0:
logger.debug('Collision test no.%d (out of %d total)' %
(n, nbTotalCells))
if yup:
# Convert to image format (y,x)
occupancyMap = np.flipud(occupancyMap.T)
occupancyMapCoord = np.flipud(
np.transpose(occupancyMapCoord, axes=(1, 0, 2)))
return occupancyMap, occupancyMapCoord
class RegripTppAss(object):
def __init__(self, objpath, nxtrobot, handpkg, gdb):
self.graphtpp0 = GraphTpp(objpath, nxtrobot, handpkg, gdb, 'rgt')
self.armname = armname
self.gdb = gdb
self.robot = nxtrobot
self.hand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
# plane to remove hand
self.bulletworld = BulletWorld()
self.planebullnode = cd.genCollisionPlane()
self.bulletworld.attachRigidBody(self.planebullnode)
# add tabletop plane model to bulletworld
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"))
self.ttnodepath = NodePath("tabletop")
ttl = loader.loadModel(ttpath)
ttl.instanceTo(self.ttnodepath)
# self.endnodeids is a dictionary where
# self.endnodeids['rgt'] equals self.startnodeids
# self.endnodeids['lft'] equals self.endnodeids # in right->left order
self.endnodeids = {}
# load retraction distances
self.rethandx, self.retworldz, self.retworlda, self.worldz = self.gdb.loadIKRet()
# worlda is default for the general grasps on table top
# for assembly at start and goal, worlda is computed by assembly planner
self.worlda = Vec3(0,0,1)
self.gnodesplotpos = {}
self.freegripid = []
self.freegripcontacts = []
self.freegripnormals = []
self.freegriprotmats = []
self.freegripjawwidth = []
# for start and goal grasps poses:
radiusgrip = 1
self.__xyzglobalgrippos_startgoal={}
for k, globalgripid in enumerate(self.graphtpp.globalgripids):
xypos = [radiusgrip * math.cos(2 * math.pi / len(self.graphtpp.globalgripids) * k),
radiusgrip * math.sin(2 * math.pi / len(self.graphtpp.globalgripids) * k)]
self.__xyzglobalgrippos_startgoal[globalgripid] = [xypos[0],xypos[1],0]
self.__loadFreeAirGrip()
@property
def dbobjname(self):
# read-only property
return self.graphtpp.dbobjname
def __loadFreeAirGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: weiwei
date: 20170110
"""
freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname)
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripid = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def addEnds(self, rotmat4, armname):
# the node id of a globalgripid in end
nodeidofglobalidinend = {}
# the endnodeids is also for quick access
self.endnodeids[armname] = []
for j, rotmat in enumerate(self.freegriprotmats):
assgsrotmat = rotmat * rotmat4
# for collision detection, we move the object back to x=0,y=0
assgsrotmatx0y0 = Mat4(rotmat4)
assgsrotmatx0y0.setCell(3,0,0)
assgsrotmatx0y0.setCell(3,1,0)
assgsrotmatx0y0 = rotmat * assgsrotmatx0y0
# check if the hand collide with tabletop
tmphand = self.hand
initmat = tmphand.getMat()
initjawwidth = tmphand.jawwidth
# set jawwidth to 80 to avoid collision with surrounding obstacles
# set to gripping with is unnecessary
# tmphand.setJawwidth(self.freegripjawwidth[j])
tmphand.setJawwidth(tmphand.jawwidthopen)
tmphand.setMat(assgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
assgscct0=rotmat4.xformPoint(self.freegripcontacts[j][0])
assgscct1=rotmat4.xformPoint(self.freegripcontacts[j][1])
assgsfgrcenter = (assgscct0+assgscct1)/2
handx = assgsrotmat.getRow3(0)
assgsfgrcenterhandx = assgsfgrcenter + handx*self.rethandx
# handxworldz is not necessary for start
# assgsfgrcenterhandxworldz = assgsfgrcenterhandx + self.worldz*self.retworldz
assgsfgrcenterworlda = assgsfgrcenter + self.worlda*self.retworlda
assgsfgrcenterworldaworldz = assgsfgrcenterworlda+ self.worldz*self.retworldz
assgsjawwidth = self.freegripjawwidth[j]
assgsidfreeair = self.freegripid[j]
assgsfgrcenternp = pg.v3ToNp(assgsfgrcenter)
assgsfgrcenternp_handx = pg.v3ToNp(assgsfgrcenterhandx)
# handxworldz is not necessary for start
# assgsfgrcenternp_handxworldz = pg.v3ToNp(assgsfgrcenterhandxworldz)
assgsfgrcenternp_worlda = pg.v3ToNp(assgsfgrcenterworlda)
assgsfgrcenternp_worldaworldz = pg.v3ToNp(assgsfgrcenterworldaworldz)
assgsrotmat3np = pg.mat3ToNp(assgsrotmat.getUpper3())
ikc = self.robot.numikr(assgsfgrcenternp, assgsrotmat3np)
ikcx = self.robot.numikr(assgsfgrcenternp_handx, assgsrotmat3np)
ikca = self.robot.numikr(assgsfgrcenternp_worlda, assgsrotmat3np)
ikcaz = self.robot.numikr(assgsfgrcenternp_worldaworldz, assgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = rotmat4.getRow3(3)
startrotmat4worlda = Mat4(rotmat4)
startrotmat4worlda.setRow(3, rotmat4.getRow3(3)+self.worlda*self.retworlda)
startrotmat4worldaworldz = Mat4(startrotmat4worlda)
startrotmat4worldaworldz.setRow(3, startrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.graphtpp.regg.add_node('end'+armname+str(j), fgrcenter=assgsfgrcenternp,
fgrcenterhandx = assgsfgrcenternp_handx,
fgrcenterhandxworldz = 'na',
fgrcenterworlda = assgsfgrcenternp_worlda,
fgrcenterworldaworldz = assgsfgrcenternp_worldaworldz,
jawwidth=assgsjawwidth, hndrotmat3np=assgsrotmat3np,
globalgripid = assgsidfreeair, freetabletopplacementid = 'na',
tabletopplacementrotmat = rotmat4,
tabletopplacementrotmathandx = rotmat4,
tabletopplacementrotmathandxworldz = 'na',
tabletopplacementrotmatworlda = startrotmat4worlda,
tabletopplacementrotmatworldaworldz = startrotmat4worldaworldz,
angle = 'na', tabletopposition = tabletopposition)
nodeidofglobalidinend[assgsidfreeair]='start'+str(j)
self.endnodeids[armname].append('start'+str(j))
tmphand.setMat(initmat)
tmphand.setJawwidth(initjawwidth)
if len(self.endnodeids[armname]) == 0:
raise ValueError("No available end grip at " + armname)
# add start transit edge
for edge in list(itertools.combinations(self.endnodeids[armname], 2)):
self.graphtpp.regg.add_edge(*edge, weight = 1, edgetype = 'endtransit')
# add start transfer edge
for reggnode, reggnodedata in self.graphtpp.regg.nodes(data=True):
if reggnode.startswith(self.armname):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidinend.keys():
endnodeid = nodeidofglobalidinend[globalgripid]
self.graphtpp.regg.add_edge(endnodeid, reggnode, weight=1, edgetype = 'endtransfer')
for nid in self.graphtpp.regg.nodes():
if nid.startswith('end'):
ggid = self.graphtpp.regg.node[nid]['globalgripid']
tabletopposition = self.graphtpp.regg.node[nid]['tabletopposition']
xyzpos = map(add, self.__xyzglobalgrippos_startgoal[ggid],
[tabletopposition[0], tabletopposition[1], tabletopposition[2]])
self.gnodesplotpos[nid] = xyzpos[:2]
def plotGraph(self, pltax, endname = 'start', gtppoffset = [0,0]):
"""
:param pltax:
:param endname:
:param gtppoffset: where to plot graphtpp, see the plotGraph function of GraphTpp class
:return:
"""
self.graphtpp.plotGraph(pltax, offset = gtppoffset)
self.__plotEnds(pltax, endname)
def __plotEnds(self, pltax, endname = 'end'):
transitedges = []
transferedges = []
for nid0, nid1, reggedgedata in self.graphtpp.regg.edges(data=True):
if nid0.startswith('end'):
pos0 = self.gnodesplotpos[nid0][:2]
else:
pos0 = self.graphtpp.gnodesplotpos[nid0][:2]
if nid1.startswith('end'):
pos1 = self.gnodesplotpos[nid1][:2]
else:
pos1 = self.graphtpp.gnodesplotpos[nid1][:2]
if (reggedgedata['edgetype'] == 'endtransit'):
transitedges.append([pos0, pos1])
elif (reggedgedata['edgetype'] is 'endtransfer'):
transferedges.append([pos0, pos1])
transitec = mc.LineCollection(transitedges, colors = [.5,0,0,.3], linewidths = 1)
transferec = mc.LineCollection(transferedges, colors = [1,0,0,.3], linewidths = 1)
pltax.add_collection(transferec)
pltax.add_collection(transitec)
class RegripTpp():
#def __init__(self, objpath,workcellpath, robot, handpkg, gdb, offset = -55):
def __init__(self, objpath, workcellpath, robot, handpkg, gdb, offset=-55):
self.objtrimesh=trimesh.load_mesh(objpath)
self.handpkg = handpkg
self.handname = handpkg.getHandName()
# for dbaccess
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
# regg = regrip graph
self.regg = nx.Graph()
self.ndiscreterot = 0
self.nplacements = 0
self.globalgripids = []
# for removing the grasps at start and goal
self.robothand = handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
# plane to remove hand
self.bulletworld = BulletWorld()
self.planebullnode = cd.genCollisionPlane(offset =offset)
self.bulletworld.attachRigidBody(self.planebullnode)
self.startnodeids = None
self.goalnodeids = None
self.shortestpaths = None
self.gdb = gdb
self.robot = robot
# load retraction distances
self.rethandx, self.retworldz, self.retworlda, self.worldz = self.gdb.loadIKRet()
# worlda is default for the general grasps on table top
# for assembly at start and goal, worlda is computed by assembly planner
self.worlda = Vec3(0,0,1)
# loadfreeairgrip
#self.__loadDropFreeGrip()
self.__loadFreeAirGrip()
self.__loadGripsToBuildGraph()
def __loadFreeAirGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: weiwei
date: 20170110
"""
freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname, self.handname)
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripid1 = freeairgripdata[0]
self.freegripcontacts1 = freeairgripdata[1]
self.freegripnormals1 = freeairgripdata[2]
self.freegriprotmats1 = freeairgripdata[3]
self.freegripjawwidth1 = freeairgripdata[4]
def __loadDropFreeGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: jiayao
date: 20170821
"""
# freeairgripdata = self.gdb.loadDropFreeGrip(self.dbobjname, self.handname)
# if freeairgripdata is None:
# raise ValueError("Plan the DropFreeGrip first!")
#
# self.freegripid = freeairgripdata[0]
# self.freegripcontacts = freeairgripdata[1]
# self.freegripnormals = freeairgripdata[2]
# self.freegriprotmats = freeairgripdata[3]
# self.freegripjawwidth = freeairgripdata[4]
handname = "rtq85"
freegripid = []
freegripcontacts = []
freegripnormals = []
freegriprotmats = []
freegripjawwidth = []
# access to db
gdb = db.GraspDB()
sql = "SELECT dropfreegrip.iddropfreegrip, dropfreegrip.contactpnt0, dropfreegrip.contactpnt1, \
dropfreegrip.contactnormal0, dropfreegrip.contactnormal1, dropfreegrip.rotmat, \
dropfreegrip.jawwidth FROM dropfreegrip, hand, object\
WHERE dropfreegrip.idobject = object.idobject AND object.name like '%s' \
AND dropfreegrip.idhand = hand.idhand AND hand.name like '%s' \
" % (self.dbobjname, handname)
data = gdb.execute(sql)
if len(data) != 0:
for i in range(len(data)):
freegripid.append(int(data[i][0]))
freegripcontacts.append([dc.strToV3(data[i][1]), dc.strToV3(data[i][2])])
freegripnormals.append([dc.strToV3(data[i][3]), dc.strToV3(data[i][4])])
freegriprotmats.append(dc.strToMat4(data[i][5]))
freegripjawwidth.append(float(data[i][6]))
else:
print "no DropFreeGrip select"
self.freegripid = freegripid
self.freegripcontacts = freegripcontacts
self.freegripnormals = freegripnormals
self.freegriprotmats = freegriprotmats
self.freegripjawwidth = freegripjawwidth
print "ok"
def __loadGripsToBuildGraph(self, armname = "rgt"):
"""
load tabletopgrips
retraction distance are also loaded from database
:param robot: an robot defined in robotsim.hrp5 or robotsim.nextage
:param gdb: an object of the database.GraspDB class
:param idarm: value = 1 "lft" or 2 "rgt", which arm to use
:return:
author: weiwei
date: 20170112
"""
# load idarm
idarm = self.gdb.loadIdArm(armname)
idhand = self.gdb.loadIdHand(self.handname)
# get the global grip ids
# and prepare the global edges
# for each globalgripid, find all its tabletopids (pertaining to placements)
globalidsedges = {}
sql = "SELECT idfreeairgrip FROM freeairgrip,object WHERE freeairgrip.idobject=object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand = %d" % (self.dbobjname, idhand)
# sql = "SELECT dropfreegrip.iddropfreegrip FROM dropfreegrip,object WHERE dropfreegrip.idobject=object.idobject AND \
# object.name LIKE '%s' AND dropfreegrip.idhand = %d" % (self.dbobjname, idhand)
# sql = "SELECT dropworkcellgrip.iddropworkcellgrip FROM dropworkcellgrip,object WHERE dropworkcellgrip.idobject=object.idobject AND \
# object.name LIKE '%s' AND dropworkcellgrip.idhand = %d" % (self.dbobjname, idhand)
result = self.gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan freeairgrip first!")
for ggid in result:
globalidsedges[str(ggid[0])] = []
self.globalgripids.append(ggid[0])
sql = "SELECT dropstablepos.iddropstablepos,dropstablepos.rot,dropstablepos.pos,angle_drop.value FROM \
dropstablepos,object,angle_drop WHERE \
dropstablepos.idobject=object.idobject AND \
object.name LIKE '%s' " % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
tpsrows = np.array(result)
#self.angles = list([0.0])
self.angles = list(set(map(float, tpsrows[:, 3])))
# for plotting
self.fttpsids = list(set(map(int, tpsrows[:, 0])))
self.nfttps = len(self.fttpsids)
idrobot = self.gdb.loadIdRobot(self.robot)
for i, idtps in enumerate(tpsrows[:,0]):
sql = "SELECT dropworkcellgrip.iddropworkcellgrip, dropworkcellgrip.contactpnt0, dropworkcellgrip.contactpnt1, \
dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth ,dropworkcellgrip.idfreeairgrip\
FROM dropworkcellgrip,dropfreegrip,freeairgrip,ik_drop\
WHERE \
dropworkcellgrip.iddropstablepos = %d \
AND dropworkcellgrip.iddropworkcellgrip=ik_drop.iddropworkcellgrip AND ik_drop.idrobot=%d AND ik_drop.idarm = %d AND\
ik_drop.feasibility='True' AND ik_drop.feasibility_handx='True' AND ik_drop.feasibility_handxworldz='True' \
AND ik_drop.feasibility_worlda='True' AND ik_drop.feasibility_worldaworldz='True' \
AND dropworkcellgrip.idfreeairgrip = freeairgrip.idfreeairgrip \
AND dropworkcellgrip.idhand = % d AND dropworkcellgrip.iddropfreegrip = dropfreegrip.iddropfreegrip " \
% (int(idtps),idrobot, idarm, idhand)
resultttgs = self.gdb.execute(sql)
if len(resultttgs)==0:
print "no result"
continue
localidedges = []
for ttgsrow in resultttgs:
ttgsid = int(ttgsrow[0])
ttgscct0 = dc.strToV3(ttgsrow[1])
ttgscct1 = dc.strToV3(ttgsrow[2])
ttgsrotmat = dc.strToMat4(ttgsrow[3])
ttgsjawwidth = float(ttgsrow[4])
ttgsidfreeair = int(ttgsrow[5])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
objrotmat4 = pg.npToMat4(np.transpose(pg.mat3ToNp(dc.strToMat3(tpsrows[:, 1][i]))),
pg.v3ToNp(dc.strToV3(tpsrows[:, 2][i])))
objrotmat4worlda = Mat4(objrotmat4)
objrotmat4worlda.setRow(3, objrotmat4.getRow3(3)+self.worlda*self.retworlda)
objrotmat4worldaworldz = Mat4(objrotmat4worlda)
objrotmat4worldaworldz.setRow(3, objrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('mid' + str(ttgsid), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz,
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair,
#freetabletopplacementid = int(tpsrows[:,2][i]),
freetabletopplacementid=int(tpsrows[:, 0][i]),
tabletopplacementrotmat = objrotmat4,
tabletopplacementrotmathandx = objrotmat4,
tabletopplacementrotmathandxworldz = objrotmat4,
tabletopplacementrotmatworlda = objrotmat4worlda,
tabletopplacementrotmatworldaworldz = objrotmat4worldaworldz,
angle = float(tpsrows[:,3][i]),
tabletopposition = dc.strToV3(tpsrows[:,2][i]))
print "str(ttgsidfreeair),str(ttgsid)",str(ttgsidfreeair),str(ttgsid)
globalidsedges[str(ttgsidfreeair)].append('mid' + str(ttgsid))
localidedges.append('mid' + str(ttgsid))
for edge in list(itertools.combinations(localidedges, 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transit')
#toc = time.clock()
#print "(toc - tic2)", (toc - tic)
if len(globalidsedges) == 0:
raise ValueError("Plan tabletopgrips first!")
#tic = time.clock()
for globalidedgesid in globalidsedges:
for edge in list(itertools.combinations(globalidsedges[globalidedgesid], 2)):
self.regg.add_edge(*edge, weight=1, edgetype = 'transfer')
#toc = time.clock()
#print "(toc - tic3)", (toc - tic)
else:
print ('No placements planned!')
assert('No placements planned!')
def __addstartgoal(self, startrotmat4, goalrotmat4, base):
"""
add start and goal for the regg
:param startrotmat4 and goalrotmat4: both are 4by4 panda3d matrix
:return:
author: weiwei
date: 20161216, sapporo
"""
### start
# the node id of a globalgripid in startnode
nodeidofglobalidinstart= {}
# the startnodeids is also for quick access
self.startnodeids = []
for j, rotmat in enumerate(self.freegriprotmats1):
#print j, len(self.freegriprotmats)
# print rotmat
ttgsrotmat = rotmat * startrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(startrotmat4)
ttgsrotmatx0y0.setCell(3,0,0)
ttgsrotmatx0y0.setCell(3,1,0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
# tmphnd = self.robothand
tmphnd = self.handpkg.newHandNM(hndcolor=[1, 0, 0, .1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# set jawwidth to 80 to avoid collision with surrounding obstacles
# set to gripping with is unnecessary
tmphnd.setJawwidth(80)
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
# tmphnd.setMat(ttgsrotmat)
# tmphnd.reparentTo(base.render)
# if j > 3:
# base.run()
if not result.getNumContacts():
ttgscct0=startrotmat4.xformPoint(self.freegripcontacts1[j][0])
ttgscct1=startrotmat4.xformPoint(self.freegripcontacts1[j][1])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
# handxworldz is not necessary for start
# ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsjawwidth = self.freegripjawwidth1[j]
ttgsidfreeair = self.freegripid1[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
# handxworldz is not necessary for start
# ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
#print "solving starting iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = startrotmat4.getRow3(3)
startrotmat4worlda = Mat4(startrotmat4)
startrotmat4worlda.setRow(3, startrotmat4.getRow3(3)+self.worlda*self.retworlda)
startrotmat4worldaworldz = Mat4(startrotmat4worlda)
startrotmat4worldaworldz.setRow(3, startrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('start'+str(j), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = 'na',
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = 'na',
tabletopplacementrotmat = startrotmat4,
tabletopplacementrotmathandx = startrotmat4,
tabletopplacementrotmathandxworldz = 'na',
tabletopplacementrotmatworlda = startrotmat4worlda,
tabletopplacementrotmatworldaworldz = startrotmat4worldaworldz,
angle = 'na', tabletopposition = tabletopposition)
nodeidofglobalidinstart[ttgsidfreeair]='start'+str(j)
self.startnodeids.append('start'+str(j))
# tmprtq85.reparentTo(base.render)
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
if len(self.startnodeids) == 0:
#raise ValueError("No available starting grip!")
print ("No available start grip!")
return False
# add start transit edge
for edge in list(itertools.combinations(self.startnodeids, 2)):
self.regg.add_edge(*edge, weight = 1, edgetype = 'starttransit')
# add start transfer edge
for reggnode, reggnodedata in self.regg.nodes(data=True):
if reggnode.startswith('mid'):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidinstart.keys():
startnodeid = nodeidofglobalidinstart[globalgripid]
self.regg.add_edge(startnodeid, reggnode, weight=1, edgetype = 'starttransfer')
### goal
# the node id of a globalgripid in goalnode, for quick setting up edges
nodeidofglobalidingoal= {}
# the goalnodeids is also for quick access
self.goalnodeids = []
for j, rotmat in enumerate(self.freegriprotmats1):
#print j, len(self.freegriprotmats)
ttgsrotmat = rotmat * goalrotmat4
# for collision detection, we move the object back to x=0,y=0
ttgsrotmatx0y0 = Mat4(goalrotmat4)
ttgsrotmatx0y0.setCell(3,0,0)
ttgsrotmatx0y0.setCell(3,1,0)
ttgsrotmatx0y0 = rotmat * ttgsrotmatx0y0
# check if the hand collide with tabletop
tmphnd = self.robothand
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
tmphnd.setJawwidth(self.freegripjawwidth1[j])
tmphnd.setMat(ttgsrotmatx0y0)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
ttgscct0=goalrotmat4.xformPoint(self.freegripcontacts1[j][0])
ttgscct1=goalrotmat4.xformPoint(self.freegripcontacts1[j][1])
ttgsfgrcenter = (ttgscct0+ttgscct1)/2
handx = ttgsrotmat.getRow3(0)
ttgsfgrcenterhandx = ttgsfgrcenter + handx*self.rethandx
ttgsfgrcenterhandxworldz = ttgsfgrcenterhandx + self.worldz*self.retworldz
ttgsfgrcenterworlda = ttgsfgrcenter + self.worlda*self.retworlda
ttgsfgrcenterworldaworldz = ttgsfgrcenterworlda+ self.worldz*self.retworldz
ttgsjawwidth = self.freegripjawwidth1[j]
ttgsidfreeair = self.freegripid1[j]
ttgsfgrcenternp = pg.v3ToNp(ttgsfgrcenter)
ttgsfgrcenternp_handx = pg.v3ToNp(ttgsfgrcenterhandx)
ttgsfgrcenternp_handxworldz = pg.v3ToNp(ttgsfgrcenterhandxworldz)
ttgsfgrcenternp_worlda = pg.v3ToNp(ttgsfgrcenterworlda)
ttgsfgrcenternp_worldaworldz = pg.v3ToNp(ttgsfgrcenterworldaworldz)
ttgsrotmat3np = pg.mat3ToNp(ttgsrotmat.getUpper3())
#print "solving goal iks"
ikc = self.robot.numikr(ttgsfgrcenternp, ttgsrotmat3np)
ikcx = self.robot.numikr(ttgsfgrcenternp_handx, ttgsrotmat3np)
ikcxz = self.robot.numikr(ttgsfgrcenternp_handxworldz, ttgsrotmat3np)
ikca = self.robot.numikr(ttgsfgrcenternp_worlda, ttgsrotmat3np)
ikcaz = self.robot.numikr(ttgsfgrcenternp_worldaworldz, ttgsrotmat3np)
if (ikc is not None) and (ikcx is not None) and (ikcxz is not None) \
and (ikca is not None) and (ikcaz is not None):
# note the tabletopposition here is not the contact for the intermediate states
# it is the zero pos
tabletopposition = goalrotmat4.getRow3(3)
goalrotmat4worlda = Mat4(goalrotmat4)
goalrotmat4worlda.setRow(3, goalrotmat4.getRow3(3)+self.worlda*self.retworlda)
goalrotmat4worldaworldz = Mat4(goalrotmat4worlda)
goalrotmat4worldaworldz.setRow(3, goalrotmat4worlda.getRow3(3)+self.worldz*self.retworldz)
self.regg.add_node('goal'+str(j), fgrcenter=ttgsfgrcenternp,
fgrcenterhandx = ttgsfgrcenternp_handx,
fgrcenterhandxworldz = ttgsfgrcenternp_handxworldz,
fgrcenterworlda = ttgsfgrcenternp_worlda,
fgrcenterworldaworldz = ttgsfgrcenternp_worldaworldz,
jawwidth=ttgsjawwidth, hndrotmat3np=ttgsrotmat3np,
globalgripid = ttgsidfreeair, freetabletopplacementid = 'na',
tabletopplacementrotmat = goalrotmat4,
tabletopplacementrotmathandx = goalrotmat4,
tabletopplacementrotmathandxworldz = goalrotmat4,
tabletopplacementrotmatworlda = goalrotmat4worlda,
tabletopplacementrotmatworldaworldz = goalrotmat4worldaworldz,
angleid = 'na', tabletopposition = tabletopposition)
nodeidofglobalidingoal[ttgsidfreeair]='goal'+str(j)
self.goalnodeids.append('goal'+str(j))
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
if len(self.goalnodeids) == 0:
#raise ValueError("No available goal grip!")
print ("No available goal grip!")
return False
# add goal transit edges
for edge in list(itertools.combinations(self.goalnodeids, 2)):
self.regg.add_edge(*edge, weight = 1, edgetype = 'goaltransit')
# add goal transfer edge
for reggnode, reggnodedata in self.regg.nodes(data=True):
if reggnode.startswith('mid'):
globalgripid = reggnodedata['globalgripid']
if globalgripid in nodeidofglobalidingoal.keys():
goalnodeid = nodeidofglobalidingoal[globalgripid]
self.regg.add_edge(goalnodeid, reggnode, weight=1, edgetype = 'goaltransfer')
# add start to goal direct edges
for startnodeid in self.startnodeids:
for goalnodeid in self.goalnodeids:
# startnodeggid = start node global grip id
startnodeggid = self.regg.node[startnodeid]['globalgripid']
goalnodeggid = self.regg.node[goalnodeid]['globalgripid']
if startnodeggid == goalnodeggid:
self.regg.add_edge(startnodeid, goalnodeid, weight=1, edgetype = 'startgoaltransfer')
return True
def findshortestpath(self, startrotmat4, goalrotmat4, base):
self.directshortestpaths = []
if self.__addstartgoal(startrotmat4, goalrotmat4, base) == False:
print "No path found! add start goal false"
self.directshortestpaths = []
return False
# startgrip = random.select(self.startnodeids)
# goalgrip = random.select(self.goalnodeids)
startgrip = self.startnodeids[0]
goalgrip = self.goalnodeids[0]
self.shortestpaths = nx.all_shortest_paths(self.regg, source = startgrip, target = goalgrip)
self.directshortestpaths = []
# directshortestpaths removed the repeated start and goal transit
try:
for path in self.shortestpaths:
print path
for i, pathnode in enumerate(path):
if pathnode.startswith('start') and i < len(path)-1:
continue
else:
self.directshortestpaths.append(path[i-1:])
break
for i, pathnode in enumerate(self.directshortestpaths[-1]):
if i > 0 and pathnode.startswith('goal'):
self.directshortestpaths[-1]=self.directshortestpaths[-1][:i+1]
break
except:
print "No path found!"
pass
def plotgraph(self, pltfig):
"""
plot the graph without start and goal
:param pltfig: the matplotlib object
:return:
author: weiwei
date: 20161217, sapporos
"""
# biggest circle: grips; big circle: rotation; small circle: placements
# radiusplacement = 30
# radiusrot = 6
# radiusgrip = 1
radiusplacement = 0
radiusrot = 0
radiusgrip = 0
xyplacementspos = {}
xydiscreterotspos = {}
self.xyzglobalgrippos = {}
for i, ttpsid in enumerate(self.fttpsids):
xydiscreterotspos[ttpsid]={}
self.xyzglobalgrippos[ttpsid]={}
xypos = [radiusplacement*math.cos(2*math.pi/self.nfttps*i),
radiusplacement*math.sin(2*math.pi/self.nfttps*i)]
xyplacementspos[ttpsid] = xypos
for j, anglevalue in enumerate(self.angles):
self.xyzglobalgrippos[ttpsid][anglevalue]={}
xypos = [radiusrot*math.cos(math.radians(anglevalue)), radiusrot*math.sin(math.radians(anglevalue))]
xydiscreterotspos[ttpsid][anglevalue] = \
[xyplacementspos[ttpsid][0]+xypos[0], xyplacementspos[ttpsid][1]+xypos[1]]
for k, globalgripid in enumerate(self.globalgripids):
xypos = [radiusgrip*math.cos(2*math.pi/len(self.globalgripids)* k),
radiusgrip*math.sin(2*math.pi/len(self.globalgripids)*k)]
self.xyzglobalgrippos[ttpsid][anglevalue][globalgripid]=\
[xydiscreterotspos[ttpsid][anglevalue][0]+xypos[0],
xydiscreterotspos[ttpsid][anglevalue][1]+xypos[1], 0]
# for start and goal grasps poses:
self.xyzlobalgrippos={}
for k, globalgripid in enumerate(self.globalgripids):
xypos = [radiusgrip * math.cos(2 * math.pi / len(self.globalgripids) * k),
radiusgrip * math.sin(2 * math.pi / len(self.globalgripids) * k)]
self.xyzlobalgrippos[globalgripid] = [xypos[0],xypos[1],0]
transitedges = []
transferedges = []
starttransferedges = []
goaltransferedges = []
starttransitedges = []
goaltransitedges = []
for nid0, nid1, reggedgedata in self.regg.edges(data=True):
if (reggedgedata['edgetype'] is 'transit') or (reggedgedata['edgetype'] is 'transfer'):
fttpid0 = self.regg.node[nid0]['freetabletopplacementid']
anglevalue0 = self.regg.node[nid0]['angle']
ggid0 = self.regg.node[nid0]['globalgripid']
fttpid1 = self.regg.node[nid1]['freetabletopplacementid']
anglevalue1 = self.regg.node[nid1]['angle']
ggid1 = self.regg.node[nid1]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][ggid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][ggid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
# 3d
# if reggedgedata['edgetype'] is 'transit':
# transitedges.append([xyzpos0, xyzpos1])
# if reggedgedata['edgetype'] is 'transfer':
# transferedges.append([xyzpos0, xyzpos1])
#2d
if reggedgedata['edgetype'] is 'transit':
transitedges.append([xyzpos0[:2], xyzpos1[:2]])
if reggedgedata['edgetype'] is 'transfer':
transferedges.append([xyzpos0[:2], xyzpos1[:2]])
else:
if (reggedgedata['edgetype'] is 'starttransit') or (reggedgedata['edgetype'] is 'goaltransit'):
gid0 = self.regg.node[nid0]['globalgripid']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzlobalgrippos[gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzlobalgrippos[gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
if reggedgedata['edgetype'] is 'starttransit':
starttransitedges.append([xyzpos0[:2], xyzpos1[:2]])
if reggedgedata['edgetype'] is 'goaltransit':
goaltransitedges.append([xyzpos0[:2], xyzpos1[:2]])
else:
# start or goal transfer
if nid0.startswith('mid'):
fttpid0 = self.regg.node[nid0]['freetabletopplacementid']
anglevalue0 = self.regg.node[nid0]['angle']
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzlobalgrippos[gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
if nid1[:4] == 'goal':
goaltransferedges.append([xyzpos0[:2], xyzpos1[:2]])
else:
starttransferedges.append([xyzpos0[:2], xyzpos1[:2]])
if nid1.startswith('mid'):
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
fttpid1 = self.regg.node[nid1]['freetabletopplacementid']
anglevalue1 = self.regg.node[nid1]['angle']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzlobalgrippos[gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
if nid0[:4] == 'goal':
goaltransferedges.append([xyzpos0[:2], xyzpos1[:2]])
else:
starttransferedges.append([xyzpos0[:2], xyzpos1[:2]])
#3d
# transitec = mc3d.Line3DCollection(transitedges, colors=[0,1,1,1], linewidths=1)
# transferec = mc3d.Line3DCollection(transferedges, colors=[0,0,0,.1], linewidths=1)
#2d
transitec = mc.LineCollection(transitedges, colors=[0,1,1,1], linewidths=1)
transferec = mc.LineCollection(transferedges, colors=[0,0,0,.1], linewidths=1)
starttransferec = mc.LineCollection(starttransferedges, colors=[1,0,0,.3], linewidths=1)
goaltransferec = mc.LineCollection(goaltransferedges, colors=[0,0,1,.3], linewidths=1)
starttransitec = mc.LineCollection(starttransitedges, colors=[.5,0,0,.3], linewidths=1)
goaltransitec = mc.LineCollection(goaltransitedges, colors=[0,0,.5,.3], linewidths=1)
ax = pltfig.add_subplot(111)
ax.add_collection(transferec)
ax.add_collection(transitec)
ax.add_collection(starttransferec)
ax.add_collection(goaltransferec)
ax.add_collection(starttransitec)
ax.add_collection(goaltransitec)
# for reggnode, reggnodedata in self.regg.nodes(data=True):
# placementid = reggnodedata['placementid']
# angleid = reggnodedata['angleid']
# globalgripid = reggnodedata['globalgripid']
# tabletopposition = reggnodedata['tabletopposition']
# xyzpos = map(add, xyzglobalgrippos[placementid][angleid][str(globalgripid)],[tabletopposition[0], tabletopposition[1], tabletopposition[2]])
# plt.plot(xyzpos[0], xyzpos[1], 'ro')
def plotshortestpath(self, pltfig, id = 0):
"""
plot the shortest path
about transit and transfer:
The tabletoppositions of start and goal are the local zero of the mesh model
in contrast, the tabletoppositions of the other nodes in the graph are the local zero of the supporting facet
if tabletopposition start == tabletop position goal
there are two possibilities:
1) start and goal are the same, then it is transit
2) start and goal are different, then it is tranfer
Note that start and the second will never be the same since they are in different coordinate systems.
It is reasonable since the shortest path will never let the start go to the same position again.
if the second item is not the goal, the path between the first and second items is
sure to be a transfer path
:param id:
:return:
"""
for i,path in enumerate(self.directshortestpaths):
if i is id:
pathedgestransit = []
pathedgestransfer = []
pathlength = len(path)
for pnidx in range(pathlength-1):
nid0 = path[pnidx]
nid1 = path[pnidx+1]
if pnidx == 0 and pnidx+1 == pathlength-1:
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzlobalgrippos[gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzlobalgrippos[gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
if tabletopposition0 == tabletopposition1:
pathedgestransit.append([xyzpos0[:2], xyzpos1[:2]])
else:
pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]])
else:
if pnidx == 0:
# this is sure to be transfer
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
fttpid1 = self.regg.node[nid1]['freetabletopplacementid']
anglevalue1 = self.regg.node[nid1]['angle']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzlobalgrippos[gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][gid1], [
tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]])
if pnidx+1 == pathlength-1:
# also definitely transfer
fttpid0 = self.regg.node[nid0]['freetabletopplacementid']
anglevalue0 = self.regg.node[nid0]['angle']
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzlobalgrippos[gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]])
if pnidx > 0 and pnidx+1 < pathlength-1:
fttpid0 = self.regg.node[nid0]['freetabletopplacementid']
anglevalue0 = self.regg.node[nid0]['angle']
gid0 = self.regg.node[nid0]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
fttpid1 = self.regg.node[nid1]['freetabletopplacementid']
anglevalue1 = self.regg.node[nid1]['angle']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, self.xyzglobalgrippos[fttpid0][anglevalue0][gid0],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, self.xyzglobalgrippos[fttpid1][anglevalue1][gid1],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
if tabletopposition0 == tabletopposition1:
pathedgestransit.append([xyzpos0[:2], xyzpos1[:2]])
else:
pathedgestransfer.append([xyzpos0[:2], xyzpos1[:2]])
pathtransitec = mc.LineCollection(pathedgestransit, colors=[.5, 1, 0, 1], linewidths=5)
pathtransferec = mc.LineCollection(pathedgestransfer, colors=[0, 1, 0, 1], linewidths=5)
ax = pltfig.gca()
ax.add_collection(pathtransitec)
ax.add_collection(pathtransferec)
def plotgraphp3d(self, base):
"""
draw the graph in panda3d
:param base:
:return:
author: weiwei
date: 20161216, osaka itami airport
"""
# big circle: rotation; small circle: placements
radiusplacement = 30
radiusrot = 6
radiusgrip = 1
xyplacementspos = []
xydiscreterotspos = []
xyzglobalgrippos = []
for i in range(self.nplacements):
xydiscreterotspos.append([])
xyzglobalgrippos.append([])
xypos = [radiusplacement*math.cos(2*math.pi/self.nplacements*i), radiusplacement*math.sin(2*math.pi/self.nplacements*i)]
xyplacementspos.append(xypos)
for j in range(self.ndiscreterot):
xyzglobalgrippos[-1].append({})
xypos = [radiusrot*math.cos(2*math.pi/self.ndiscreterot* j), radiusrot*math.sin(2*math.pi/self.ndiscreterot * j)]
xydiscreterotspos[-1].append([xyplacementspos[-1][0]+xypos[0],xyplacementspos[-1][1]+xypos[1]])
for k, globalgripid in enumerate(self.globalgripids):
xypos = [radiusgrip*math.cos(2*math.pi/len(self.globalgripids)* k), radiusgrip*math.sin(2*math.pi/len(self.globalgripids)*k)]
xyzglobalgrippos[-1][-1][globalgripid]=[xydiscreterotspos[-1][-1][0]+xypos[0],xydiscreterotspos[-1][-1][1]+xypos[1], 0]
transitedges = []
transferedges = []
for nid0, nid1, reggedgedata in self.regg.edges(data=True):
fttpid0 = self.regg.node[nid0]['freetabletopplacementid']
anglevalue0 = self.regg.node[nid0]['angle']
gid0 = self.regg.node[nid0]['globalgripid']
fttpid1 = self.regg.node[nid1]['freetabletopplacementid']
angelvalue1 = self.regg.node[nid1]['angle']
gid1 = self.regg.node[nid1]['globalgripid']
tabletopposition0 = self.regg.node[nid0]['tabletopposition']
tabletopposition1 = self.regg.node[nid1]['tabletopposition']
xyzpos0 = map(add, xyzglobalgrippos[fttpid0][anglevalue0][str(gid0)],
[tabletopposition0[0], tabletopposition0[1], tabletopposition0[2]])
xyzpos1 = map(add, xyzglobalgrippos[fttpid1][angelvalue1][str(gid1)],
[tabletopposition1[0], tabletopposition1[1], tabletopposition1[2]])
# 3d
if reggedgedata['edgetype'] is 'transit':
transitedges.append([xyzpos0, xyzpos1])
if reggedgedata['edgetype'] is 'transfer':
transferedges.append([xyzpos0, xyzpos1])
#3d
transitecnp = pg.makelsnodepath(transitedges, rgbacolor=[0,1,1,1])
transferecnp = pg.makelsnodepath(transferedges, rgbacolor=[0,0,0,.1])
transitecnp.reparentTo(base.render)
transferecnp.reparentTo(base.render)
class Freegrip(fgcp.FreegripContactpairs):
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 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 removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
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
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
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]]
tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = tmphand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
tmphand.setJawwidth(fgrdist)
tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmphand.setMat(pandageom.cvtMat4(rotmat) * tmphand.getMat())
axx = tmphand.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
tmphand.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmphand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j])
# pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
# tmprtq85.setColor([.5, .5, .5, 1])
# tmprtq85.reparentTo(base.render)
# self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
# reset initial hand pose
tmphand.setMat(initmat)
self.counter+=1
self.counter = 0
def removeFgrpcc(self, base):
"""
Fgrpcc means finger pre collision detection
:return:
author: weiwei
date: 20161212, tsukuba
"""
self.gripcontactpairs_precc = []
self.gripcontactpairnormals_precc = []
self.gripcontactpairfacets_precc = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
print self.gripcontactpairs
self.gripcontactpairs_precc.append([])
self.gripcontactpairnormals_precc.append([])
self.gripcontactpairfacets_precc.append([])
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0)
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(handfgrpcc1)
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
self.gripcontactpairs_precc[-1].append(contactpair)
self.gripcontactpairnormals_precc[-1].append(self.gripcontactpairnormals[self.counter][j])
self.gripcontactpairfacets_precc[-1].append(self.gripcontactpairfacets[self.counter])
self.counter += 1
self.counter=0
def saveToDB(self, gdb):
"""
save the result to mysqldatabase
:param gdb: is an object of the GraspDB class in the database package
:return:
author: weiwei
date: 20170110
"""
# save to database
gdb = db.GraspDB()
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT * FROM freeairgrip, object WHERE freeairgrip.idobject = object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand LIKE '%s'" % (self.dbobjname, idhand)
result = gdb.execute(sql)
if not result:
sql = "SELECT idobject FROM object WHERE name LIKE '%s'" % self.dbobjname
returnlist = gdb.execute(sql)
if len(returnlist) != 0:
idobject = returnlist[0][0]
else:
sql = "INSERT INTO object(name) VALUES('%s')" % self.dbobjname
idobject = gdb.execute(sql)
print self.gripcontacts
for i in range(len(self.gripcontacts)):
sql = "INSERT INTO freeairgrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d)" % \
(idobject, dc.v3ToStr(self.gripcontacts[i][0]), dc.v3ToStr(self.gripcontacts[i][1]),
dc.v3ToStr(self.gripcontactnormals[i][0]), dc.v3ToStr(self.gripcontactnormals[i][1]),
dc.mat4ToStr(self.griprotmats[i]), str(self.gripjawwidth[i]), idhand)
gdb.execute(sql)
else:
print "Grasps already saved or duplicated filename!"
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 removeFgrpccShowLeft(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
Plot the available grips
:return:
author: weiwei
date: 20161212, tsukuba
"""
plotoffsetfp = 6
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
return
else:
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[self.counter]):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0)
handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2])
handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0], cctpnt0[1] + cctnormal0[1], cctpnt0[2] + cctnormal0[2])
handfgrpcc1 = NodePath("rtq85fgrpcc1")
self.handfgrpcc_uninstanced.instanceTo(handfgrpcc1)
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(base.render)
handfgrpcc1.reparentTo(base.render)
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)
def plotObj(self):
geomnodeobj = GeomNode('obj')
geomnodeobj.addGeom(self.objgeom)
npnodeobj = NodePath('obj')
npnodeobj.attachNewNode(geomnodeobj)
npnodeobj.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)
class Freegrip(fgcp.FreegripContactpairs):
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 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 removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
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
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
# print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(
self.gripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
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]
]
tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = tmphand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
tmphand.setJawwidth(fgrdist)
tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmphand.setMat(
pandageom.cvtMat4(rotmat) * tmphand.getMat())
axx = tmphand.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array(
[axx[0], axx[1], axx[2]])
tmphand.setPos(
Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(
tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmphand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(
self.gripcontactpairnormals_precc[self.counter][j])
# pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
# tmprtq85.setColor([.5, .5, .5, 1])
# tmprtq85.reparentTo(base.render)
# self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
# reset initial hand pose
tmphand.setMat(initmat)
self.counter += 1
self.counter = 0
def removeFgrpcc(self, base):
"""
Fgrpcc means finger pre collision detection
:return:
author: weiwei
date: 20161212, tsukuba
"""
self.gripcontactpairs_precc = []
self.gripcontactpairnormals_precc = []
self.gripcontactpairfacets_precc = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1)
print self.gripcontactpairs
self.gripcontactpairs_precc.append([])
self.gripcontactpairnormals_precc.append([])
self.gripcontactpairfacets_precc.append([])
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(
self.gripcontactpairs[self.counter]):
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0)
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(handfgrpcc1)
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
self.gripcontactpairs_precc[-1].append(contactpair)
self.gripcontactpairnormals_precc[-1].append(
self.gripcontactpairnormals[self.counter][j])
self.gripcontactpairfacets_precc[-1].append(
self.gripcontactpairfacets[self.counter])
self.counter += 1
self.counter = 0
def saveToDB(self, gdb):
"""
save the result to mysqldatabase
:param gdb: is an object of the GraspDB class in the database package
:return:
author: weiwei
date: 20170110
"""
# save to database
gdb = db.GraspDB()
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT * FROM freeairgrip, object WHERE freeairgrip.idobject = object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand LIKE '%s'" % (
self.dbobjname, idhand)
result = gdb.execute(sql)
if not result:
sql = "SELECT idobject FROM object WHERE name LIKE '%s'" % self.dbobjname
returnlist = gdb.execute(sql)
if len(returnlist) != 0:
idobject = returnlist[0][0]
else:
sql = "INSERT INTO object(name) VALUES('%s')" % self.dbobjname
idobject = gdb.execute(sql)
print self.gripcontacts
for i in range(len(self.gripcontacts)):
sql = "INSERT INTO freeairgrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d)" % \
(idobject, dc.v3ToStr(self.gripcontacts[i][0]), dc.v3ToStr(self.gripcontacts[i][1]),
dc.v3ToStr(self.gripcontactnormals[i][0]), dc.v3ToStr(self.gripcontactnormals[i][1]),
dc.mat4ToStr(self.griprotmats[i]), str(self.gripjawwidth[i]), idhand)
gdb.execute(sql)
else:
print "Grasps already saved or duplicated filename!"
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 removeFgrpccShowLeft(self, base):
"""
Fgrpcc means finger pre collision detection
This one is specially written for demonstration
Plot the available grips
:return:
author: weiwei
date: 20161212, tsukuba
"""
plotoffsetfp = 6
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
return
else:
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[self.counter]):
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.facetnormals[facetidx0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
handfgrpcc0 = NodePath("handfgrpcc0")
self.handfgrpcc_uninstanced.instanceTo(handfgrpcc0)
handfgrpcc0.setPos(cctpnt0[0], cctpnt0[1], cctpnt0[2])
handfgrpcc0.lookAt(cctpnt0[0] + cctnormal0[0],
cctpnt0[1] + cctnormal0[1],
cctpnt0[2] + cctnormal0[2])
handfgrpcc1 = NodePath("rtq85fgrpcc1")
self.handfgrpcc_uninstanced.instanceTo(handfgrpcc1)
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)
result = self.bulletworld.contactTest(facetmeshbullnode)
if not result.getNumContacts():
handfgrpcc0.setColor(1, 1, 1, .3)
handfgrpcc1.setColor(1, 1, 1, .3)
handfgrpcc0.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc1.setTransparency(TransparencyAttrib.MAlpha)
handfgrpcc0.reparentTo(base.render)
handfgrpcc1.reparentTo(base.render)
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)
def plotObj(self):
geomnodeobj = GeomNode('obj')
geomnodeobj.addGeom(self.objgeom)
npnodeobj = NodePath('obj')
npnodeobj.attachNewNode(geomnodeobj)
npnodeobj.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)
class World():
CULLDISTANCE = 10
def __init__(self, size):
self.bw = BulletWorld()
self.bw.setGravity(0, 0, 0)
self.size = size
self.perlin = PerlinNoise2()
#utilities.app.accept('bullet-contact-added', self.onContactAdded)
#utilities.app.accept('bullet-contact-destroyed', self.onContactDestroyed)
self.player = Player(self)
self.player.initialise()
self.entities = list()
self.bgs = list()
self.makeChunk(Point2(0, 0), Point2(3.0, 3.0))
self.cmap = buildMap(self.entities, self.player.location)
self.mps = list()
self.entities.append(
Catcher(Point2(10, 10), self.player, self.cmap, self))
def update(self, timer):
dt = globalClock.getDt()
self.bw.doPhysics(dt, 5, 1.0 / 180.0)
self.doHits(Flame)
self.doHits(Catcher)
for entity in self.entities:
if entity.remove == True:
entity.destroy()
self.entities.remove(entity)
self.player.update(dt)
self.cmap = buildMap(self.entities, self.player.location)
for entity in self.entities:
entity.update(dt)
def doHits(self, hit_type):
for entity in self.entities:
if isinstance(entity, hit_type):
ctest = self.bw.contactTest(entity.bnode)
if ctest.getNumContacts() > 0:
entity.removeOnHit()
mp = ctest.getContacts()[0].getManifoldPoint()
if isinstance(
ctest.getContacts()[0].getNode0().getPythonTag(
"entity"), hit_type):
ctest.getContacts()[0].getNode1().getPythonTag(
"entity").hitby(hit_type, mp.getIndex0())
else:
ctest.getContacts()[0].getNode0().getPythonTag(
"entity").hitby(hit_type, mp.getIndex0())
def makeChunk(self, pos, size):
self.bgs.append(
utilities.loadObject("stars",
depth=100,
scaleX=200,
scaleY=200.0,
pos=Point2(pos.x * worldsize.x,
pos.y * worldsize.y)))
genFillBox(self, Point2(5, 5), 3, 6, 'metalwalls')
genBox(self, Point2(10, 5), 2, 2, 'metalwalls')
#self.entities[0].bnode.applyTorque(Vec3(0,50,10))
def addEntity(self, entity):
self.entities.append(entity)
def onContactAdded(self, node1, node2):
e1 = node1.getPythonTag("entity")
e2 = node2.getPythonTag("entity")
if isinstance(e1, Flame):
e1.remove = True
if isinstance(e2, Flame):
e2.remove = True
print "contact"
def onContactDestroyed(self, node1, node2):
return
