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